/*!
 * \file control_thread.cc
 * \brief This class implements the receiver control plane
 * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
 *
 * GNSS Receiver Control Plane: connects the flowgraph, starts running it,
 * and while it does not stop, reads the control messages generated by the blocks,
 * process them, and apply the corresponding actions.
 *
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2018  (see AUTHORS file for a list of contributors)
 *
 * GNSS-SDR is a software defined Global Navigation
 *          Satellite Systems receiver
 *
 * This file is part of GNSS-SDR.
 *
 * GNSS-SDR is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GNSS-SDR is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */

#include "control_thread.h"
#include "concurrent_queue.h"
#include "concurrent_map.h"
#include "pvt_interface.h"
#include "control_message_factory.h"
#include "file_configuration.h"
#include "gnss_flowgraph.h"
#include "gnss_sdr_flags.h"
#include "galileo_ephemeris.h"
#include "galileo_iono.h"
#include "galileo_utc_model.h"
#include "galileo_almanac.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "gps_almanac.h"
#include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h"
#include "geofunctions.h"
#include "rtklib_rtkcmn.h"
#include "rtklib_conversions.h"
#include "rtklib_ephemeris.h"
#include <boost/lexical_cast.hpp>
#include <boost/chrono.hpp>
#include <glog/logging.h>
#include <gnuradio/message.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
#include <string>


extern concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
extern concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;

using google::LogMessage;


ControlThread::ControlThread()
{
    if (!FLAGS_c.compare("-"))
        {
            configuration_ = std::make_shared<FileConfiguration>(FLAGS_config_file);
        }
    else
        {
            configuration_ = std::make_shared<FileConfiguration>(FLAGS_c);
        }
    delete_configuration_ = false;
    restart_ = false;
    init();
}


ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> configuration)
{
    configuration_ = configuration;
    delete_configuration_ = false;
    restart_ = false;
    init();
}


ControlThread::~ControlThread()
{
    // save navigation data to files
    // if (save_assistance_to_XML() == true) {}
    if (msqid != -1) msgctl(msqid, IPC_RMID, NULL);
}


void ControlThread::telecommand_listener()
{
    int tcp_cmd_port = configuration_->property("Channel.telecontrol_tcp_port", 3333);
    cmd_interface_.run_cmd_server(tcp_cmd_port);
}


/*
 * Runs the control thread that manages the receiver control plane
 *
 * This is the main loop that reads and process the control messages
 * 1- Connect the GNSS receiver flowgraph
 * 2- Start the GNSS receiver flowgraph
 *    while (flowgraph_->running() && !stop)_{
 * 3- Read control messages and process them }
 */
int ControlThread::run()
{
    // Connect the flowgraph
    try
        {
            flowgraph_->connect();
        }
    catch (const std::exception &e)
        {
            LOG(ERROR) << e.what();
            return 0;
        }
    if (flowgraph_->connected())
        {
            LOG(INFO) << "Flowgraph connected";
        }
    else
        {
            LOG(ERROR) << "Unable to connect flowgraph";
            return 0;
        }
    // Start the flowgraph
    flowgraph_->start();
    if (flowgraph_->running())
        {
            LOG(INFO) << "Flowgraph started";
        }
    else
        {
            LOG(ERROR) << "Unable to start flowgraph";
            return 0;
        }

    // launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs
    assist_GNSS();
    // start the keyboard_listener thread
    keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this);
    sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);

    // start the telecommand listener thread
    cmd_interface_.set_pvt(flowgraph_->get_pvt());
    cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);

    bool enable_FPGA = configuration_->property("Channel.enable_FPGA", false);
    if (enable_FPGA == true)
        {
            flowgraph_->start_acquisition_helper();
        }

    // Main loop to read and process the control messages
    while (flowgraph_->running() && !stop_)
        {
            //TODO re-enable the blocking read messages functions and fork the process
            read_control_messages();
            if (control_messages_ != 0) process_control_messages();
        }
    std::cout << "Stopping GNSS-SDR, please wait!" << std::endl;
    flowgraph_->stop();
    stop_ = true;
    flowgraph_->disconnect();

//Join keyboard thread
#ifdef OLD_BOOST
    keyboard_thread_.timed_join(boost::posix_time::seconds(1));
    sysv_queue_thread_.timed_join(boost::posix_time::seconds(1));
    cmd_interface_thread_.timed_join(boost::posix_time::seconds(1));
#endif
#ifndef OLD_BOOST
    keyboard_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
    sysv_queue_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
    cmd_interface_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
#endif

    LOG(INFO) << "Flowgraph stopped";

    if (restart_)
        {
            return 42;  //signal the gnss-sdr-harness.sh to restart the receiver program
        }
    else
        {
            return 0;  //normal shutdown
        }
}


void ControlThread::set_control_queue(gr::msg_queue::sptr control_queue)
{
    if (flowgraph_->running())
        {
            LOG(WARNING) << "Unable to set control queue while flowgraph is running";
            return;
        }
    control_queue_ = control_queue;
    cmd_interface_.set_msg_queue(control_queue_);
}


/*
 * Returns true if reading was successful
 */
bool ControlThread::read_assistance_from_XML()
{
    // return variable (true == succeeded)
    bool ret = false;
    // getting names from the config file, if available
    std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename);
    std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename);
    std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename);
    std::string gal_iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_iono_xml", gal_iono_default_xml_filename);
    std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename);
    std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename);
    std::string eph_gal_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_ephemeris_xml", eph_gal_default_xml_filename);
    std::string eph_cnav_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_cnav_ephemeris_xml", eph_cnav_default_xml_filename);
    std::string gal_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_utc_model_xml", gal_utc_default_xml_filename);
    std::string cnav_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_cnav_utc_model_xml", cnav_utc_default_xml_filename);
    std::string eph_glo_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename);
    std::string glo_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_utc_model_xml", glo_utc_default_xml_filename);
    std::string gal_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_almanac_xml", gal_almanac_default_xml_filename);
    std::string gps_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_almanac_xml", gps_almanac_default_xml_filename);

    if (configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false) == true)
        {
            eph_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ephemeris_xml", eph_default_xml_filename);
            utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_utc_model_xml", utc_default_xml_filename);
            iono_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_iono_xml", iono_default_xml_filename);
            gal_iono_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_iono_xml", gal_iono_default_xml_filename);
            ref_time_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ref_time_xml", ref_time_default_xml_filename);
            ref_location_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ref_location_xml", ref_location_default_xml_filename);
            eph_gal_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_ephemeris_xml", eph_gal_default_xml_filename);
            eph_cnav_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_cnav_ephemeris_xml", eph_cnav_default_xml_filename);
            gal_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_utc_model_xml", gal_utc_default_xml_filename);
            cnav_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_cnav_utc_model_xml", cnav_utc_default_xml_filename);
            eph_glo_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename);
            glo_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_utc_model_xml", glo_utc_default_xml_filename);
            gal_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_almanac_xml", gal_almanac_default_xml_filename);
            gps_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_almanac_xml", gps_almanac_default_xml_filename);
        }

    std::cout << "Trying to read GNSS ephemeris from XML file(s)..." << std::endl;

    if (configuration_->property("Channels_1C.count", 0) > 0)
        {
            if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true)
                {
                    std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter;
                    for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.cbegin();
                         gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend();
                         gps_eph_iter++)
                        {
                            std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }

            if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true)
                {
                    std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_acquisition_.gps_utc);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read GPS UTC model parameters." << std::endl;
                    ret = true;
                }

            if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true)
                {
                    std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_acquisition_.gps_iono);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read GPS ionosphere model parameters." << std::endl;
                    ret = true;
                }

            if (supl_client_ephemeris_.load_gps_almanac_xml(gps_almanac_xml_filename) == true)
                {
                    std::map<int, Gps_Almanac>::const_iterator gps_alm_iter;
                    for (gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.cbegin();
                         gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.cend();
                         gps_alm_iter++)
                        {
                            std::cout << "From XML file: Read GPS almanac for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }
        }

    if ((configuration_->property("Channels_1B.count", 0) > 0) or (configuration_->property("Channels_5X.count", 0) > 0))
        {
            if (supl_client_ephemeris_.load_gal_ephemeris_xml(eph_gal_xml_filename) == true)
                {
                    std::map<int, Galileo_Ephemeris>::const_iterator gal_eph_iter;
                    for (gal_eph_iter = supl_client_ephemeris_.gal_ephemeris_map.cbegin();
                         gal_eph_iter != supl_client_ephemeris_.gal_ephemeris_map.cend();
                         gal_eph_iter++)
                        {
                            std::cout << "From XML file: Read ephemeris for satellite " << Gnss_Satellite("Galileo", gal_eph_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(gal_eph_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }

            if (supl_client_acquisition_.load_gal_iono_xml(gal_iono_xml_filename) == true)
                {
                    std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(supl_client_acquisition_.gal_iono);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read Galileo ionosphere model parameters." << std::endl;
                    ret = true;
                }

            if (supl_client_acquisition_.load_gal_utc_xml(gal_utc_xml_filename) == true)
                {
                    std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(supl_client_acquisition_.gal_utc);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read Galileo UTC model parameters." << std::endl;
                    ret = true;
                }

            if (supl_client_ephemeris_.load_gal_almanac_xml(gal_almanac_xml_filename) == true)
                {
                    std::map<int, Galileo_Almanac>::const_iterator gal_alm_iter;
                    for (gal_alm_iter = supl_client_ephemeris_.gal_almanac_map.cbegin();
                         gal_alm_iter != supl_client_ephemeris_.gal_almanac_map.cend();
                         gal_alm_iter++)
                        {
                            std::cout << "From XML file: Read Galileo almanac for satellite " << Gnss_Satellite("Galileo", gal_alm_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(gal_alm_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }
        }

    if ((configuration_->property("Channels_2S.count", 0) > 0) or (configuration_->property("Channels_L5.count", 0) > 0))
        {
            if (supl_client_ephemeris_.load_cnav_ephemeris_xml(eph_cnav_xml_filename) == true)
                {
                    std::map<int, Gps_CNAV_Ephemeris>::const_iterator gps_cnav_eph_iter;
                    for (gps_cnav_eph_iter = supl_client_ephemeris_.gps_cnav_ephemeris_map.cbegin();
                         gps_cnav_eph_iter != supl_client_ephemeris_.gps_cnav_ephemeris_map.cend();
                         gps_cnav_eph_iter++)
                        {
                            std::cout << "From XML file: Read CNAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_cnav_eph_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(gps_cnav_eph_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }

            if (supl_client_acquisition_.load_cnav_utc_xml(cnav_utc_xml_filename) == true)
                {
                    std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(supl_client_acquisition_.gps_cnav_utc);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read GPS CNAV UTC model parameters." << std::endl;
                    ret = true;
                }
        }

    if ((configuration_->property("Channels_1G.count", 0) > 0) or (configuration_->property("Channels_2G.count", 0) > 0))
        {
            if (supl_client_ephemeris_.load_gnav_ephemeris_xml(eph_glo_xml_filename) == true)
                {
                    std::map<int, Glonass_Gnav_Ephemeris>::const_iterator glo_gnav_eph_iter;
                    for (glo_gnav_eph_iter = supl_client_ephemeris_.glonass_gnav_ephemeris_map.cbegin();
                         glo_gnav_eph_iter != supl_client_ephemeris_.glonass_gnav_ephemeris_map.cend();
                         glo_gnav_eph_iter++)
                        {
                            std::cout << "From XML file: Read GLONASS GNAV ephemeris for satellite " << Gnss_Satellite("GLONASS", glo_gnav_eph_iter->second.i_satellite_PRN) << std::endl;
                            std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(glo_gnav_eph_iter->second);
                            flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                        }
                    ret = true;
                }

            if (supl_client_acquisition_.load_glo_utc_xml(glo_utc_xml_filename) == true)
                {
                    std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(supl_client_acquisition_.glo_gnav_utc);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                    std::cout << "From XML file: Read GLONASS UTC model parameters." << std::endl;
                    ret = true;
                }
        }

    if (ret == false)
        {
            std::cout << "Error reading XML files" << std::endl;
            std::cout << "Disabling GNSS assistance..." << std::endl;
        }

    // Only look for {ref time, ref location} if SUPL is enabled
    bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
    if (enable_gps_supl_assistance == true)
        {
            // Try to read Ref Time from XML
            if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true)
                {
                    LOG(INFO) << "SUPL: Read XML Ref Time";
                    std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(supl_client_acquisition_.gps_time);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                }
            else
                {
                    LOG(INFO) << "SUPL: could not read Ref Time XML";
                }

            // Try to read Ref Location from XML
            if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true)
                {
                    LOG(INFO) << "SUPL: Read XML Ref Location";
                    std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(supl_client_acquisition_.gps_ref_loc);
                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                }
            else
                {
                    LOG(INFO) << "SUPL: could not read Ref Location XML";
                }
        }

    return ret;
}


void ControlThread::assist_GNSS()
{
    //######### GNSS Assistance #################################
    // GNSS Assistance configuration
    bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
    bool enable_agnss_xml = configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false);
    if ((enable_gps_supl_assistance == true) and (enable_agnss_xml == false))
        {
            std::cout << "SUPL RRLP GPS assistance enabled!" << std::endl;
            std::string default_acq_server = "supl.google.com";
            std::string default_eph_server = "supl.google.com";
            supl_client_ephemeris_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_server", default_acq_server);
            supl_client_acquisition_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_server", default_eph_server);
            supl_client_ephemeris_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_port", 7275);
            supl_client_acquisition_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_port", 7275);
            supl_mcc = configuration_->property("GNSS-SDR.SUPL_MCC", 244);
            supl_mns = configuration_->property("GNSS-SDR.SUPL_MNC ", 5);

            std::string default_lac = "0x59e2";
            std::string default_ci = "0x31b0";
            std::string supl_lac_s = configuration_->property("GNSS-SDR.SUPL_LAC", default_lac);
            std::string supl_ci_s = configuration_->property("GNSS-SDR.SUPL_CI", default_ci);
            try
                {
                    supl_lac = std::stoi(supl_lac_s, nullptr, 0);
                }
            catch (const std::invalid_argument &ia)
                {
                    std::cerr << "Invalid argument for SUPL LAC: " << ia.what() << '\n';
                    supl_lac = -1;
                }
            try
                {
                    supl_ci = std::stoi(supl_ci_s, nullptr, 0);
                }
            catch (const std::invalid_argument &ia)
                {
                    std::cerr << "Invalid argument for SUPL CI: " << ia.what() << '\n';
                    supl_ci = -1;
                }

            if (supl_lac < 0 or supl_lac > 65535)
                {
                    supl_lac = 0x59e2;
                }

            if (supl_ci < 0 or supl_ci > 268435455)  // 2^16 for GSM and CDMA, 2^28 for UMTS and LTE networks
                {
                    supl_ci = 0x31b0;
                }

            bool SUPL_read_gps_assistance_xml = configuration_->property("GNSS-SDR.SUPL_read_gps_assistance_xml", false);
            if (SUPL_read_gps_assistance_xml == true)
                {
                    // read assistance from file
                    if (read_assistance_from_XML())
                        {
                            std::cout << "GNSS assistance data loaded from local XML file(s)." << std::endl;
                            std::cout << "No SUPL request has been performed." << std::endl;
                        }
                }
            else
                {
                    // Request ephemeris from SUPL server
                    int error;
                    supl_client_ephemeris_.request = 1;
                    std::cout << "SUPL: Try to read GPS ephemeris data from SUPL server..." << std::endl;
                    error = supl_client_ephemeris_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
                    if (error == 0)
                        {
                            std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter;
                            for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.cbegin();
                                 gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend();
                                 gps_eph_iter++)
                                {
                                    std::cout << "SUPL: Received ephemeris data for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.i_satellite_PRN) << std::endl;
                                    std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                }
                            // Save ephemeris to XML file
                            std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename);
                            if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, supl_client_ephemeris_.gps_ephemeris_map) == true)
                                {
                                    std::cout << "SUPL: XML ephemeris data file created" << std::endl;
                                }
                            else
                                {
                                    std::cout << "SUPL: Failed to create XML ephemeris data file" << std::endl;
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client request for ephemeris data returned " << error << std::endl;
                            std::cout << "Please check your network connectivity and SUPL server configuration" << std::endl;
                            std::cout << "Trying to read AGNSS data from local XML file(s)..." << std::endl;
                            if (read_assistance_from_XML() == false)
                                {
                                    std::cout << "ERROR: Could not read XML files: Disabling SUPL assistance." << std::endl;
                                }
                        }

                    // Request almanac, IONO and UTC Model data
                    supl_client_ephemeris_.request = 0;
                    std::cout << "SUPL: Try to read Almanac, Iono, Utc Model, Ref Time and Ref Location data from SUPL server..." << std::endl;
                    error = supl_client_ephemeris_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
                    if (error == 0)
                        {
                            std::map<int, Gps_Almanac>::const_iterator gps_alm_iter;
                            for (gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.cbegin();
                                 gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.cend();
                                 gps_alm_iter++)
                                {
                                    std::cout << "SUPL: Received almanac data for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.i_satellite_PRN) << std::endl;
                                    std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                }
                            supl_client_ephemeris_.save_gps_almanac_xml("gps_almanac_map.xml", supl_client_ephemeris_.gps_almanac_map);
                            if (supl_client_ephemeris_.gps_iono.valid == true)
                                {
                                    std::cout << "SUPL: Received GPS Ionosphere model parameters" << std::endl;
                                    std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_ephemeris_.gps_iono);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                }
                            if (supl_client_ephemeris_.gps_utc.valid == true)
                                {
                                    std::cout << "SUPL: Received GPS UTC model parameters" << std::endl;
                                    std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_ephemeris_.gps_utc);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                }
                            // Save iono and UTC model data to xml file
                            std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename);
                            if (supl_client_ephemeris_.save_iono_xml(iono_xml_filename, supl_client_ephemeris_.gps_iono) == true)
                                {
                                    std::cout << "SUPL: Iono data file created" << std::endl;
                                }
                            else
                                {
                                    std::cout << "SUPL: Failed to create Iono data file" << std::endl;
                                }
                            std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename);
                            if (supl_client_ephemeris_.save_utc_xml(utc_xml_filename, supl_client_ephemeris_.gps_utc) == true)
                                {
                                    std::cout << "SUPL: UTC model data file created" << std::endl;
                                }
                            else
                                {
                                    std::cout << "SUPL: Failed to create UTC model data file" << std::endl;
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client for almanac data returned " << error << std::endl;
                            std::cout << "Please check your network connectivity and SUPL server configuration" << std::endl;
                        }

                    // Request acquisition assistance
                    supl_client_acquisition_.request = 2;
                    std::cout << "SUPL: Try to read acquisition assistance data from SUPL server..." << std::endl;
                    error = supl_client_acquisition_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
                    if (error == 0)
                        {
                            std::map<int, Gps_Acq_Assist>::const_iterator gps_acq_iter;
                            for (gps_acq_iter = supl_client_acquisition_.gps_acq_map.cbegin();
                                 gps_acq_iter != supl_client_acquisition_.gps_acq_map.cend();
                                 gps_acq_iter++)
                                {
                                    std::cout << "SUPL: Received acquisition assistance data for satellite " << Gnss_Satellite("GPS", gps_acq_iter->second.i_satellite_PRN) << std::endl;
                                    global_gps_acq_assist_map.write(gps_acq_iter->second.i_satellite_PRN, gps_acq_iter->second);
                                }
                            if (supl_client_acquisition_.gps_ref_loc.valid == true)
                                {
                                    std::cout << "SUPL: Received Ref Location data (Acquisition Assistance)" << std::endl;
                                    agnss_ref_location_ = supl_client_acquisition_.gps_ref_loc;
                                    std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(agnss_ref_location_);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                    supl_client_acquisition_.save_ref_location_xml("agnss_ref_location.xml", agnss_ref_location_);
                                }
                            if (supl_client_acquisition_.gps_time.valid == true)
                                {
                                    std::cout << "SUPL: Received Ref Time data (Acquisition Assistance)" << std::endl;
                                    agnss_ref_time_ = supl_client_acquisition_.gps_time;
                                    std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(agnss_ref_time_);
                                    flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
                                    supl_client_acquisition_.save_ref_time_xml("agnss_ref_time.xml", agnss_ref_time_);
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client for acquisition assistance returned " << error << std::endl;
                            std::cout << "Please check your network connectivity and SUPL server configuration" << std::endl;
                            std::cout << "Disabling SUPL acquisition assistance." << std::endl;
                        }
                }
        }

    if ((enable_gps_supl_assistance == false) and (enable_agnss_xml == true))
        {
            // read assistance from file
            if (read_assistance_from_XML())
                {
                    std::cout << "GNSS assistance data loaded from local XML file(s)." << std::endl;
                }
        }

    // If we have enough AGNSS data, make use of it
    if (agnss_ref_location_.valid == true)  // and agnss_ref_time_.valid == true  and we have AGNSS data
        {
            // Get the list of visible satellites
            arma::vec ref_LLH = arma::zeros(3, 1);
            ref_LLH(0) = agnss_ref_location_.lat;
            ref_LLH(1) = agnss_ref_location_.lon;
            time_t ref_rx_utc_time = 0;
            if (agnss_ref_time_.valid == true)
                {
                    ref_rx_utc_time = agnss_ref_time_.d_tv_sec;
                }

            std::vector<std::pair<int, Gnss_Satellite>> visible_sats = get_visible_sats(ref_rx_utc_time, ref_LLH);
            // Set the receiver in Standby mode
            flowgraph_->apply_action(0, 10);
            // Give priority to visible satellites in the search list
            flowgraph_->priorize_satellites(visible_sats);
            // Hot Start
            flowgraph_->apply_action(0, 12);
        }
}


void ControlThread::init()
{
    // Instantiates a control queue, a GNSS flowgraph, and a control message factory
    control_queue_ = gr::msg_queue::make(0);
    cmd_interface_.set_msg_queue(control_queue_);  //set also the queue pointer for the telecommand thread
    try
        {
            flowgraph_ = std::make_shared<GNSSFlowgraph>(configuration_, control_queue_);
        }
    catch (const boost::bad_lexical_cast &e)
        {
            std::cout << "Caught bad lexical cast with error " << e.what() << std::endl;
        }
    control_message_factory_ = std::make_shared<ControlMessageFactory>();
    stop_ = false;
    processed_control_messages_ = 0;
    applied_actions_ = 0;
    supl_mcc = 0;
    supl_mns = 0;
    supl_lac = 0;
    supl_ci = 0;
    msqid = -1;
    agnss_ref_location_ = Agnss_Ref_Location();
    agnss_ref_time_ = Agnss_Ref_Time();

    std::string empty_string = "";
    std::string ref_location_str = configuration_->property("GNSS-SDR.AGNSS_ref_location", empty_string);
    std::string ref_time_str = configuration_->property("GNSS-SDR.AGNSS_ref_utc_time", empty_string);
    if (ref_location_str.compare(empty_string) != 0)
        {
            std::vector<double> vect;
            std::stringstream ss(ref_location_str);
            double d;
            while (ss >> d)
                {
                    vect.push_back(d);
                    if (ss.peek() == ',')
                        ss.ignore();
                }
            // fill agnss_ref_location_
            if (vect.size() >= 2)
                {
                    agnss_ref_location_.lat = vect[0];
                    agnss_ref_location_.lon = vect[1];
                    agnss_ref_location_.valid = true;
                }
        }
    if (ref_time_str.compare(empty_string) == 0)
        {
            // Make an educated guess
            time_t rawtime;
            time(&rawtime);
            agnss_ref_time_.d_tv_sec = rawtime;
            agnss_ref_time_.valid = true;
        }
    else
        {
            // fill agnss_ref_time_
            agnss_ref_time_.d_tv_sec = 0;  // fill
            agnss_ref_time_.valid = true;
        }
}


void ControlThread::read_control_messages()
{
    DLOG(INFO) << "Reading control messages from queue";
    gr::message::sptr queue_message = control_queue_->delete_head();
    if (queue_message != 0)
        {
            control_messages_ = control_message_factory_->GetControlMessages(queue_message);
        }
    else
        {
            control_messages_->clear();
        }
}


// Apply the corresponding control actions
// TODO:  May be it is better to move the apply_action state machine to the control_thread
void ControlThread::process_control_messages()
{
    for (unsigned int i = 0; i < control_messages_->size(); i++)
        {
            if (stop_) break;
            if (control_messages_->at(i)->who == 200)
                {
                    apply_action(control_messages_->at(i)->what);
                }
            else
                {
                    if (control_messages_->at(i)->who == 300)  //some TC commands require also actions from controlthread
                        {
                            apply_action(control_messages_->at(i)->what);
                        }
                    flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what);
                }
            processed_control_messages_++;
        }
    control_messages_->clear();
    DLOG(INFO) << "Processed all control messages";
}


void ControlThread::apply_action(unsigned int what)
{
    std::shared_ptr<PvtInterface> pvt_ptr;
    std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
    switch (what)
        {
        case 0:
            LOG(INFO) << "Received action STOP";
            stop_ = true;
            applied_actions_++;
            break;
        case 1:
            LOG(INFO) << "Received action RESTART";
            stop_ = true;
            restart_ = true;
            applied_actions_++;
            break;
        case 11:
            LOG(INFO) << "Receiver action COLDSTART";
            //delete all ephemeris and almanac information from maps (also the PVT map queue)
            pvt_ptr = flowgraph_->get_pvt();
            pvt_ptr->clear_ephemeris();
            //todo: reorder the satellite queues to the receiver default startup order.
            //This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
            break;
        case 12:
            LOG(INFO) << "Receiver action HOTSTART";
            visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
            //reorder the satellite queue to acquire first those visible satellites
            flowgraph_->priorize_satellites(visible_satellites);
            //start again the satellite acquisitions (done in chained apply_action to flowgraph)
            break;
        case 13:
            LOG(INFO) << "Receiver action WARMSTART";
            //delete all ephemeris and almanac information from maps (also the PVT map queue)
            pvt_ptr = flowgraph_->get_pvt();
            pvt_ptr->clear_ephemeris();
            //load the ephemeris and the almanac from XML files (receiver assistance)
            read_assistance_from_XML();
            //call here the function that computes the set of visible satellites and its elevation
            //for the date and time specified by the warm start command and the assisted position
            get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
            //reorder the satellite queue to acquire first those visible satellites
            flowgraph_->priorize_satellites(visible_satellites);
            //start again the satellite acquisitions (done in chained apply_action to flowgraph)
            break;
        default:
            LOG(INFO) << "Unrecognized action.";
            break;
        }
}


std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const arma::vec &LLH)
{
    // 1. Compute rx ECEF position from LLH WGS84
    arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)};
    arma::mat C_tmp = arma::zeros(3, 3);
    arma::vec r_eb_e = arma::zeros(3, 1);
    arma::vec v_eb_e = arma::zeros(3, 1);
    Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);

    // 2. Compute rx GPS time from UTC time
    gtime_t utc_gtime;
    utc_gtime.time = rx_utc_time;
    utc_gtime.sec = 0;
    gtime_t gps_gtime = utc2gpst(utc_gtime);

    // 3. loop through all the available ephemeris or almanac and compute satellite positions and elevations
    // store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
    std::vector<std::pair<int, Gnss_Satellite>> available_satellites;

    std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
    struct tm tstruct;
    char buf[80];
    tstruct = *gmtime(&rx_utc_time);
    strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
    std::string str_time = std::string(buf);
    std::cout << "Get visible satellites at " << str_time
              << " UTC, assuming RX position " << LLH(0) << " [deg], " << LLH(1) << " [deg], " << LLH(2) << " [m]" << std::endl;

    std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
    for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it)
        {
            eph_t rtklib_eph = eph_to_rtklib(it->second);
            double r_sat[3];
            double clock_bias_s;
            double sat_pos_variance_m2;
            eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
                &sat_pos_variance_m2);
            double Az, El, dist_m;
            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
            arma::vec dx = r_sat_eb_e - r_eb_e;
            topocent(&Az, &El, &dist_m, r_eb_e, dx);
            // push sat
            if (El > 0)
                {
                    std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                        (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
                }
        }

    std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
    for (std::map<int, Galileo_Ephemeris>::iterator it = gal_eph_map.begin(); it != gal_eph_map.end(); ++it)
        {
            eph_t rtklib_eph = eph_to_rtklib(it->second);
            double r_sat[3];
            double clock_bias_s;
            double sat_pos_variance_m2;
            eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
                &sat_pos_variance_m2);
            double Az, El, dist_m;
            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
            arma::vec dx = r_sat_eb_e - r_eb_e;
            topocent(&Az, &El, &dist_m, r_eb_e, dx);
            // push sat
            if (El > 0)
                {
                    std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                        (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
                }
        }

    std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
    for (std::map<int, Gps_Almanac>::iterator it = gps_alm_map.begin(); it != gps_alm_map.end(); ++it)
        {
            alm_t rtklib_alm = alm_to_rtklib(it->second);
            double r_sat[3];
            double clock_bias_s;
            alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
            double Az, El, dist_m;
            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
            arma::vec dx = r_sat_eb_e - r_eb_e;
            topocent(&Az, &El, &dist_m, r_eb_e, dx);
            // push sat
            if (El > 0)
                {
                    std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                        (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
                }
        }

    std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
    for (std::map<int, Galileo_Almanac>::iterator it = gal_alm_map.begin(); it != gal_alm_map.end(); ++it)
        {
            alm_t rtklib_alm = alm_to_rtklib(it->second);
            double r_sat[3];
            double clock_bias_s;
            gtime_t gal_gtime;
            gal_gtime.time = fmod(utc2gpst(gps_gtime).time + 345600, 604800);
            alm2pos(gal_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
            double Az, El, dist_m;
            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
            arma::vec dx = r_sat_eb_e - r_eb_e;
            topocent(&Az, &El, &dist_m, r_eb_e, dx);
            // push sat
            if (El > 0)
                {
                    std::cout << "Using Galileo Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                        (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
                }
        }

    // sort the visible satellites in ascending order of elevation
    std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) {  // use lambda. Cleaner and easier to read
        return a.first < b.first;
    });
    // std::reverse(available_satellites.begin(), available_satellites.end());
    return available_satellites;
}


void ControlThread::gps_acq_assist_data_collector()
{
    // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
    Gps_Acq_Assist gps_acq;
    Gps_Acq_Assist gps_acq_old;
    while (stop_ == false)
        {
            global_gps_acq_assist_queue.wait_and_pop(gps_acq);
            if (gps_acq.i_satellite_PRN == 0) break;

            // DEBUG MESSAGE
            std::cout << "Acquisition assistance record has arrived from SAT ID "
                      << gps_acq.i_satellite_PRN
                      << " with Doppler "
                      << gps_acq.d_Doppler0
                      << " [Hz] " << std::endl;
            // insert new acq record to the global ephemeris map
            if (global_gps_acq_assist_map.read(gps_acq.i_satellite_PRN, gps_acq_old))
                {
                    std::cout << "Acquisition assistance record updated" << std::endl;
                    global_gps_acq_assist_map.write(gps_acq.i_satellite_PRN, gps_acq);
                }
            else
                {
                    // insert new acq record
                    LOG(INFO) << "New acq assist record inserted";
                    global_gps_acq_assist_map.write(gps_acq.i_satellite_PRN, gps_acq);
                }
        }
}


void ControlThread::sysv_queue_listener()
{
    typedef struct
    {
        long mtype;  // required by SysV queue messaging
        double stop_message;
    } stop_msgbuf;

    bool read_queue = true;
    stop_msgbuf msg;
    double received_message = 0.0;
    int msgrcv_size = sizeof(msg.stop_message);

    key_t key = 1102;

    if ((msqid = msgget(key, 0644 | IPC_CREAT)) == -1)
        {
            perror("GNSS-SDR cannot create SysV message queues");
            exit(1);
        }

    while (read_queue && !stop_)
        {
            if (msgrcv(msqid, &msg, msgrcv_size, 1, 0) != -1)
                {
                    received_message = msg.stop_message;
                    if ((std::abs(received_message - (-200.0)) < 10 * std::numeric_limits<double>::epsilon()))
                        {
                            std::cout << "Quit order received, stopping GNSS-SDR !!" << std::endl;
                            std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
                            if (control_queue_ != gr::msg_queue::sptr())
                                {
                                    control_queue_->handle(cmf->GetQueueMessage(200, 0));
                                }
                            read_queue = false;
                        }
                }
        }
}


void ControlThread::keyboard_listener()
{
    bool read_keys = true;
    char c = '0';
    while (read_keys && !stop_)
        {
            std::cin.get(c);
            if (c == 'q')
                {
                    std::cout << "Quit keystroke order received, stopping GNSS-SDR !!" << std::endl;
                    std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
                    if (control_queue_ != gr::msg_queue::sptr())
                        {
                            control_queue_->handle(cmf->GetQueueMessage(200, 0));
                        }
                    read_keys = false;
                }
            usleep(500000);
        }
}