/*!
 * \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-2015  (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 <http://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */

#include "control_thread.h"
#include <unistd.h>
#include <iostream>
#include <map>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/chrono.hpp>
#include <gnuradio/message.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "gps_almanac.h"
#include "galileo_ephemeris.h"
#include "galileo_iono.h"
#include "galileo_utc_model.h"
#include "galileo_almanac.h"
#include "concurrent_queue.h"
#include "concurrent_map.h"
#include "gnss_flowgraph.h"
#include "file_configuration.h"
#include "control_message_factory.h"



extern concurrent_map<Gps_Ephemeris> global_gps_ephemeris_map;
extern concurrent_map<Gps_Iono> global_gps_iono_map;
extern concurrent_map<Gps_Utc_Model> global_gps_utc_model_map;
extern concurrent_map<Gps_Almanac> global_gps_almanac_map;
extern concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
extern concurrent_map<Gps_Ref_Time> global_gps_ref_time_map;
extern concurrent_map<Gps_Ref_Location> global_gps_ref_location_map;

extern concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
extern concurrent_queue<Gps_Iono> global_gps_iono_queue;
extern concurrent_queue<Gps_Utc_Model> global_gps_utc_model_queue;
extern concurrent_queue<Gps_Almanac> global_gps_almanac_queue;
extern concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
extern concurrent_queue<Gps_Ref_Location> global_gps_ref_location_queue;
extern concurrent_queue<Gps_Ref_Time> global_gps_ref_time_queue;

extern concurrent_map<Galileo_Ephemeris> global_galileo_ephemeris_map;
extern concurrent_map<Galileo_Iono> global_galileo_iono_map;
extern concurrent_map<Galileo_Utc_Model> global_galileo_utc_model_map;
extern concurrent_map<Galileo_Almanac> global_galileo_almanac_map;
//extern concurrent_map<Galileo_Acq_Assist> global_gps_acq_assist_map;

extern concurrent_queue<Galileo_Ephemeris> global_galileo_ephemeris_queue;
extern concurrent_queue<Galileo_Iono> global_galileo_iono_queue;
extern concurrent_queue<Galileo_Utc_Model> global_galileo_utc_model_queue;
extern concurrent_queue<Galileo_Almanac> global_galileo_almanac_queue;
//extern concurrent_queue<Galileo_Acq_Assist> global_gps_acq_assist_queue;


using google::LogMessage;

DEFINE_string(config_file, std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf"),
		"File containing the configuration parameters");

ControlThread::ControlThread()
{
    configuration_ = std::make_shared<FileConfiguration>(FLAGS_config_file);
    init();
}


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



ControlThread::~ControlThread()
{
    // save navigation data to files
    if (save_assistance_to_XML() == true) {}
}



/*
 * 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 }
 */
void ControlThread::run()
{
    // Connect the flowgraph
    flowgraph_->connect();
    if (flowgraph_->connected())
        {
            LOG(INFO) << "Flowgraph connected";
        }
    else
        {
            LOG(ERROR) << "Unable to connect flowgraph";
            return;
        }
    // Start the flowgraph
    flowgraph_->start();
    if (flowgraph_->running())
        {
            LOG(INFO) << "Flowgraph started";
        }
    else
        {
            LOG(ERROR) << "Unable to start flowgraph";
            return;
        }
    // start the keyboard_listener thread
    keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this);

    //start the GNSS SV data collector thread
    gps_ephemeris_data_collector_thread_ = boost::thread(&ControlThread::gps_ephemeris_data_collector, this);
    gps_iono_data_collector_thread_ = boost::thread(&ControlThread::gps_iono_data_collector, this);
    gps_utc_model_data_collector_thread_ = boost::thread(&ControlThread::gps_utc_model_data_collector, this);
    gps_acq_assist_data_collector_thread_= boost::thread(&ControlThread::gps_acq_assist_data_collector, this);
    gps_ref_location_data_collector_thread_ = boost::thread(&ControlThread::gps_ref_location_data_collector, this);
    gps_ref_time_data_collector_thread_ = boost::thread(&ControlThread::gps_ref_time_data_collector, this);

    galileo_ephemeris_data_collector_thread_ = boost::thread(&ControlThread::galileo_ephemeris_data_collector, this);
    galileo_iono_data_collector_thread_ = boost::thread(&ControlThread::galileo_iono_data_collector, this);
    galileo_almanac_data_collector_thread_ = boost::thread(&ControlThread::galileo_almanac_data_collector, this);
    galileo_utc_model_data_collector_thread_ = boost::thread(&ControlThread::galileo_utc_model_data_collector, this);
    // 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();

#ifdef OLD_BOOST
    // Join GPS threads
    gps_ephemeris_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    gps_iono_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    gps_utc_model_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    gps_acq_assist_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    gps_ref_location_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    gps_ref_time_data_collector_thread_.timed_join(boost::posix_time::seconds(1));

    //Join Galileo threads
    galileo_ephemeris_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    galileo_iono_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    galileo_almanac_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
    galileo_utc_model_data_collector_thread_.timed_join(boost::posix_time::seconds(1));

    //Join keyboard threads
    keyboard_thread_.timed_join(boost::posix_time::seconds(1));
#endif
#ifndef OLD_BOOST
    // Join GPS threads
    gps_ephemeris_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    gps_iono_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    gps_utc_model_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    gps_acq_assist_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    gps_ref_location_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    gps_ref_time_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));

    //Join Galileo threads
    galileo_ephemeris_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    galileo_iono_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    galileo_almanac_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
    galileo_utc_model_data_collector_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));

    //Join keyboard threads
    keyboard_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
#endif

    LOG(INFO) << "Flowgraph stopped";
}


void ControlThread::set_control_queue(boost::shared_ptr<gr::msg_queue> control_queue)
{
    if (flowgraph_->running())
        {
            LOG(WARNING) << "Unable to set control queue while flowgraph is running";
            return;
        }
    control_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 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::cout << "SUPL: Try read GPS ephemeris from XML file " << eph_xml_filename << std::endl;
    if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true)
        {
            std::map<int,Gps_Ephemeris>::iterator gps_eph_iter;
            for(gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin();
                    gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end();
                    gps_eph_iter++)
                {
                    std::cout << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first << std::endl;
                    global_gps_ephemeris_queue.push(gps_eph_iter->second);
                }
            ret = true;
        }
    else
        {
            std::cout << "ERROR: SUPL client error reading XML" << std::endl;
            std::cout << "Disabling SUPL assistance..." << std::endl;
        }
    // Only look for {utc, iono, 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 UTC model from XML
            if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true)
                {
                    LOG(INFO) << "SUPL: Read XML UTC model";
                    global_gps_utc_model_queue.push(supl_client_acquisition_.gps_utc);
                }
            else
                {
                    LOG(INFO) << "SUPL: couldn't read UTC model XML";
                }

            // Try to read Iono model from XML
            if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true)
                {
                    LOG(INFO) << "SUPL: Read XML IONO model";
                    global_gps_iono_queue.push(supl_client_acquisition_.gps_iono);
                }
            else
                {
                    LOG(INFO) << "SUPL: couldn't read IONO model XML";
                }

            // 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";
                    global_gps_ref_time_queue.push(supl_client_acquisition_.gps_time);
                }
            else
                {
                    LOG(INFO) << "SUPL: couldn't 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";
                    global_gps_ref_location_queue.push(supl_client_acquisition_.gps_ref_loc);
                }
            else
                {
                    LOG(INFO) << "SUPL: couldn't read Ref Location XML";
                }
        }

    return ret;
}


// Returns true if reading was successful
bool ControlThread::save_assistance_to_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 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);

    LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename;
    std::map<int, Gps_Ephemeris> eph_copy = global_gps_ephemeris_map.get_map_copy();
    if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, eph_copy) == true)
        {
            LOG(INFO) << "SUPL: Successfully saved ephemeris XML file";
            ret = true;
        }
    else
        {
            LOG(INFO) << "SUPL: Error while trying to save ephemeris XML file. Maybe an empty map?";
            ret = false;
        }
    // Only try to save {utc, iono, 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 save utc model xml file
            std::map<int, Gps_Utc_Model> utc_copy = global_gps_utc_model_map.get_map_copy();
            if (supl_client_acquisition_.save_utc_map_xml(utc_xml_filename, utc_copy) == true)
                {
                    LOG(INFO) << "SUPL: Successfully saved UTC Model XML file";
                    //ret = true;
                }
            else
                {
                    LOG(INFO) << "SUPL: Error while trying to save utc XML file";
                    //ret = false;
                }
            // try to save iono model xml file
            std::map<int, Gps_Iono> iono_copy = global_gps_iono_map.get_map_copy();
            if (supl_client_acquisition_.save_iono_map_xml(iono_xml_filename, iono_copy) == true)
                {
                    LOG(INFO) << "SUPL: Successfully saved IONO Model XML file";
                    //ret = true;
                }
            else
                {
                    LOG(INFO) << "SUPL: Error while trying to save iono XML file";
                    //ret = false;
                }
            // try to save ref time xml file
            std::map<int, Gps_Ref_Time> ref_time_copy = global_gps_ref_time_map.get_map_copy();
            if (supl_client_acquisition_.save_ref_time_map_xml(ref_time_xml_filename, ref_time_copy) == true)
                {
                    LOG(INFO) << "SUPL: Successfully saved Ref Time XML file";
                    //ret = true;
                }
            else
                {
                    LOG(INFO) << "SUPL: Error while trying to save ref time XML file";
                    //ref = false;
                }
            // try to save ref location xml file
            std::map<int, Gps_Ref_Location> ref_location_copy = global_gps_ref_location_map.get_map_copy();
            if (supl_client_acquisition_.save_ref_location_map_xml(ref_location_xml_filename, ref_location_copy) == true)
                {
                    LOG(INFO) << "SUPL: Successfully saved Ref Location XML file";
                    //ref = true;
                }
            else
                {
                    LOG(INFO) << "SUPL: Error while trying to save ref location XML file";
                    //ret = false;
                }
        }
    return ret;
}


void ControlThread::init()
{
    // Instantiates a control queue, a GNSS flowgraph, and a control message factory
    control_queue_ = gr::msg_queue::make(0);
    flowgraph_ = std::make_shared<GNSSFlowgraph>(configuration_, control_queue_);
    control_message_factory_ = std::make_shared<ControlMessageFactory>();
    stop_ = false;
    processed_control_messages_ = 0;
    applied_actions_ = 0;

    //######### GNSS Assistance #################################
    // GNSS Assistance configuration
    bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
    if (enable_gps_supl_assistance == true)
        //SUPL SERVER TEST. Not operational yet!
        {
            std::cout << "SUPL RRLP GPS assistance enabled!" << std::endl;
            std::string default_acq_server = "supl.nokia.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_MNS", 5);

            std::string default_lac = "0x59e2";
            std::string default_ci = "0x31b0";
            try
            {
                    supl_lac = boost::lexical_cast<int>(configuration_->property("GNSS-SDR.SUPL_LAC", default_lac));
            }
            catch(boost::bad_lexical_cast &)
            {
                    supl_lac = 0x59e2;
            }

            try
            {
                    supl_ci = boost::lexical_cast<int>(configuration_->property("GNSS-SDR.SUPL_CI", default_ci));
            }
            catch(boost::bad_lexical_cast &)
            {
                    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()) {}
                }
            else
                {
                    // Request ephemeris from SUPL server
                    int error;
                    supl_client_ephemeris_.request = 1;
                    std::cout << "SUPL: Try to read GPS ephemeris 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>::iterator gps_eph_iter;
                            for(gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin();
                                    gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end();
                                    gps_eph_iter++)
                                {
                                    std::cout << "SUPL: Received Ephemeris for GPS SV " << gps_eph_iter->first << std::endl;
                                    global_gps_ephemeris_map.write(gps_eph_iter->second.i_satellite_PRN, gps_eph_iter->second);
                                }
                            //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 file created" << std::endl;
                                }
                            else
                                {
                                    std::cout << "SUPL: Failed to create XML Ephemeris file" << std::endl;
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client for Ephemeris returned " << error << std::endl;
                            std::cout << "Please check internet connection and SUPL server configuration" << error << std::endl;
                            std::cout << "Trying to read ephemeris from XML file" << std::endl;
                            if (read_assistance_from_XML() == false)
                                {
                                    std::cout << "ERROR: Could not read Ephemeris file: Disabling SUPL assistance.." << std::endl;
                                }
                        }

                    // Request almanac , IONO and UTC Model
                    supl_client_ephemeris_.request = 0;
                    std::cout << "SUPL: Try read Almanac, Iono, Utc Model, Ref Time and Ref Location 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>::iterator gps_alm_iter;
                            for(gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.begin();
                                    gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.end();
                                    gps_alm_iter++)
                                {
                                    std::cout << "SUPL: Received Almanac for GPS SV " << gps_alm_iter->first << std::endl;
                                    global_gps_almanac_queue.push(gps_alm_iter->second);
                                }
                            if (supl_client_ephemeris_.gps_iono.valid == true)
                                {
                                    std::cout << "SUPL: Received GPS Iono" << std::endl;
                                    global_gps_iono_map.write(0, supl_client_ephemeris_.gps_iono);
                                }
                            if (supl_client_ephemeris_.gps_utc.valid == true)
			        {
                                    std::cout << "SUPL: Received GPS UTC Model" << std::endl;
                                    global_gps_utc_model_map.write(0, supl_client_ephemeris_.gps_utc);
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client for Almanac returned " << error << std::endl;
                            std::cout << "Please check internet connection and SUPL server configuration" << error << std::endl;
                            std::cout << "Disabling SUPL assistance.." << std::endl;
                        }

                    // Request acquisition assistance
                    supl_client_acquisition_.request = 2;
                    std::cout << "SUPL: Try read Acquisition assistance 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>::iterator gps_acq_iter;
                            for(gps_acq_iter = supl_client_acquisition_.gps_acq_map.begin();
                                    gps_acq_iter != supl_client_acquisition_.gps_acq_map.end();
                                    gps_acq_iter++)
                                {
                                    std::cout << "SUPL: Received Acquisition assistance for GPS SV " << gps_acq_iter->first << 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 (Acquisition Assistance)" << std::endl;
                                    global_gps_ref_location_map.write(0, supl_client_acquisition_.gps_ref_loc);
                                }
                            if (supl_client_acquisition_.gps_time.valid == true)
                                {
                                    std::cout << "SUPL: Received Ref Time (Acquisition Assistance)" << std::endl;
                                    global_gps_ref_time_map.write(0, supl_client_acquisition_.gps_time);
                                }
                        }
                    else
                        {
                            std::cout << "ERROR: SUPL client for Acquisition assistance returned " << error << std::endl;
                            std::cout << "Please check internet connection and SUPL server configuration" << error << std::endl;
                            std::cout << "Disabling SUPL assistance.." << std::endl;
                        }
                }
        }
}


void ControlThread::read_control_messages()
{
    DLOG(INFO) << "Reading control messages from queue";
    boost::shared_ptr<gr::message> 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
                {
                    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)
{
    switch (what)
    {
    case 0:
        DLOG(INFO) << "Received action STOP";
        stop_ = true;
        applied_actions_++;
        break;
    default:
        DLOG(INFO) << "Unrecognized action.";
        break;
    }
}


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);

            // 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::gps_ephemeris_data_collector()
{
    // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
    Gps_Ephemeris gps_eph;
    Gps_Ephemeris gps_eph_old;
    while(stop_ == false)
        {
            global_gps_ephemeris_queue.wait_and_pop(gps_eph);

            // DEBUG MESSAGE
            LOG(INFO) << "Ephemeris record has arrived from SAT ID "
                      << gps_eph.i_satellite_PRN << " (Block "
                      <<  gps_eph.satelliteBlock[gps_eph.i_satellite_PRN] << ")";
            // insert new ephemeris record to the global ephemeris map
            if (global_gps_ephemeris_map.read(gps_eph.i_satellite_PRN, gps_eph_old))
                {
                    // Check the EPHEMERIS timestamp. If it is newer, then update the ephemeris
                    if (gps_eph.i_GPS_week > gps_eph_old.i_GPS_week)
                        {
                            std::cout << "Ephemeris record updated (GPS week=" << gps_eph.i_GPS_week << std::endl;
                            global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph);
                        }
                    else
                        {
                            if (gps_eph.d_Toe > gps_eph_old.d_Toe)
                                {
                                    LOG(INFO) << "Ephemeris record updated (Toe=" << gps_eph.d_Toe;
                                    global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph);
                                }
                            else
                                {
                                    LOG(INFO) << "Not updating the existing ephemeris";
                                }
                        }
                }
            else
                {
                    // insert new ephemeris record
                    LOG(INFO) << "New Ephemeris record inserted with Toe="
                              << gps_eph.d_Toe<<" and GPS Week="
                              << gps_eph.i_GPS_week;
                    global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph);
                }
        }
}


void ControlThread::galileo_ephemeris_data_collector()
{
    // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
    Galileo_Ephemeris galileo_eph;
    Galileo_Ephemeris galileo_eph_old;
    while(stop_ == false)
        {
            global_galileo_ephemeris_queue.wait_and_pop(galileo_eph);

            // DEBUG MESSAGE
            std::cout << "Galileo Ephemeris record has arrived from SAT ID "
                      << galileo_eph.SV_ID_PRN_4 << std::endl;

            // insert new ephemeris record to the global ephemeris map
            if (global_galileo_ephemeris_map.read(galileo_eph.SV_ID_PRN_4, galileo_eph_old))
                {
                    // Check the EPHEMERIS timestamp. If it is newer, then update the ephemeris
                    if (galileo_eph.WN_5 > galileo_eph_old.WN_5) //further check because it is not clear when IOD is reset
                        {
                            LOG(INFO) << "Galileo Ephemeris record in global map updated -- GALILEO Week Number ="
                                      << galileo_eph.WN_5;
                            global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4,galileo_eph);
                        }
                    else
                        {
                            if (galileo_eph.IOD_ephemeris > galileo_eph_old.IOD_ephemeris)
                                {
                                    LOG(INFO) << "Galileo Ephemeris record updated in global map-- IOD_ephemeris ="
                                              << galileo_eph.IOD_ephemeris;
                                    global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4, galileo_eph);
                                    LOG(INFO) << "IOD_ephemeris OLD: " << galileo_eph_old.IOD_ephemeris;
                                    LOG(INFO) << "satellite: " << galileo_eph.SV_ID_PRN_4;
                                }
                            else
                                {
                                    LOG(INFO) << "Not updating the existing Galileo ephemeris, IOD is not changing";
                                }
                        }
                }
            else
                {
                    // insert new ephemeris record
                    LOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph.TOW_5
                              << ", GALILEO Week Number =" << galileo_eph.WN_5
                              << " and Ephemeris IOD = " << galileo_eph.IOD_ephemeris;
                    global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4, galileo_eph);
                }
        }

}

void ControlThread::gps_iono_data_collector()
{
    // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
    Gps_Iono gps_iono;
    while(stop_ == false)
        {
            global_gps_iono_queue.wait_and_pop(gps_iono);

            LOG(INFO) << "New IONO record has arrived ";
            // there is no timestamp for the iono data, new entries must always be added
            global_gps_iono_map.write(0, gps_iono);
        }
}

void ControlThread::galileo_almanac_data_collector()
{
    // ############ 1.bis READ ALMANAC QUEUE ####################
    Galileo_Almanac galileo_almanac;
    while(stop_ == false)
        {
            global_galileo_almanac_queue.wait_and_pop(galileo_almanac);

            LOG(INFO) << "New galileo_almanac record has arrived ";
            // there is no timestamp for the galileo_almanac data, new entries must always be added
            global_galileo_almanac_map.write(0, galileo_almanac);
        }
}

void ControlThread::galileo_iono_data_collector()
{
    Galileo_Iono galileo_iono;
    Galileo_Iono galileo_iono_old;
    while(stop_ == false)
        {
            global_galileo_iono_queue.wait_and_pop(galileo_iono);

            // DEBUG MESSAGE
            LOG(INFO) << "Iono record has arrived";

            // insert new Iono record to the global Iono map
            if (global_galileo_iono_map.read(0, galileo_iono_old))
                {
                    // Check the Iono timestamp from UTC page (page 6). If it is newer, then update the Iono parameters
                    if (galileo_iono.WN_5 > galileo_iono_old.WN_5)
                        {
                            LOG(INFO) << "IONO record updated in global map--new GALILEO UTC-IONO Week Number";
                            global_galileo_iono_map.write(0, galileo_iono);
                        }
                    else
                        {
                            if (galileo_iono.TOW_5 > galileo_iono_old.TOW_5)
                                {
                                    LOG(INFO) << "IONO record updated in global map--new GALILEO UTC-IONO time of Week";
                                    global_galileo_iono_map.write(0, galileo_iono);
                                    //std::cout << "GALILEO IONO time of Week old: " << galileo_iono_old.t0t_6<<std::endl;
                                }
                            else
                                {
                                    LOG(INFO) << "Not updating the existing Iono parameters in global map, Iono timestamp is not changing";
                                }
                        }
                }
            else
                {
                    // insert new ephemeris record
                    LOG(INFO) << "New IONO record inserted in global map";
                    global_galileo_iono_map.write(0, galileo_iono);
                }
        }
}


void ControlThread::gps_utc_model_data_collector()
{
    // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
    Gps_Utc_Model gps_utc;
    Gps_Utc_Model gps_utc_old;
    while(stop_ == false)
        {
            global_gps_utc_model_queue.wait_and_pop(gps_utc);
            LOG(INFO) << "New UTC MODEL record has arrived with A0=" << gps_utc.d_A0;
            // insert new utc record to the global utc model map
            if (global_gps_utc_model_map.read(0, gps_utc_old))
                {
                    if (gps_utc.i_WN_T > gps_utc_old.i_WN_T)
                        {
                            global_gps_utc_model_map.write(0, gps_utc);
                        }
                    else if ((gps_utc.i_WN_T == gps_utc_old.i_WN_T) and (gps_utc.d_t_OT > gps_utc_old.d_t_OT))
                        {
                            global_gps_utc_model_map.write(0, gps_utc);
                        }
                    else
                        {
                            LOG(INFO) << "Not updating the existing utc model";
                        }
                }
            else
                {
                    // insert new utc model record
                    global_gps_utc_model_map.write(0, gps_utc);
                }
        }
}


void ControlThread::gps_ref_location_data_collector()
{
    // ############ READ REF LOCATION ####################
    Gps_Ref_Location gps_ref_location;
    while(stop_ == false)
        {
            global_gps_ref_location_queue.wait_and_pop(gps_ref_location);
            LOG(INFO) << "New ref location record has arrived with lat=" << gps_ref_location.lat << " lon=" << gps_ref_location.lon;
            // insert new ref location record to the global ref location map
            global_gps_ref_location_map.write(0, gps_ref_location);
        }
}


void ControlThread::gps_ref_time_data_collector()
{
    // ############ READ REF TIME ####################
    Gps_Ref_Time gps_ref_time;
    Gps_Ref_Time gps_ref_time_old;
    while(stop_ == false)
        {
            global_gps_ref_time_queue.wait_and_pop(gps_ref_time);
            LOG(INFO) << "New ref time record has arrived with TOW=" << gps_ref_time.d_TOW << " Week=" << gps_ref_time.d_Week;
            // insert new ref time record to the global ref time map
            if (global_gps_ref_time_map.read(0, gps_ref_time_old))
                {
                    if (gps_ref_time.d_Week > gps_ref_time_old.d_Week)
                        {
                            global_gps_ref_time_map.write(0, gps_ref_time);
                        }
                    else if ((gps_ref_time.d_Week == gps_ref_time_old.d_Week) and (gps_ref_time.d_TOW > gps_ref_time_old.d_TOW))
                        {
                            global_gps_ref_time_map.write(0, gps_ref_time);
                        }
                    else
                        {
                            LOG(INFO) << "Not updating the existing ref time";
                        }
                }
            else
                {
                    // insert new ref time record
                    global_gps_ref_time_map.write(0, gps_ref_time);
                }
        }
}


void ControlThread::galileo_utc_model_data_collector()
{
    Galileo_Utc_Model galileo_utc;
    Galileo_Utc_Model galileo_utc_old;
    while(stop_ == false)
        {
            global_galileo_utc_model_queue.wait_and_pop(galileo_utc);

            // DEBUG MESSAGE
            LOG(INFO) << "UTC record has arrived" << std::endl;

            // insert new UTC record to the global UTC map
            if (global_galileo_utc_model_map.read(0, galileo_utc_old))
                {
                    // Check the UTC timestamp. If it is newer, then update the ephemeris
                    if (galileo_utc.WNot_6 > galileo_utc_old.WNot_6) //further check because it is not clear when IOD is reset
                        {
                            DLOG(INFO) << "UTC record updated --new GALILEO UTC Week Number =" << galileo_utc.WNot_6;
                            global_galileo_utc_model_map.write(0, galileo_utc);
                        }
                    else
                        {
                            if (galileo_utc.t0t_6 > galileo_utc_old.t0t_6)
                                {
                                    DLOG(INFO) << "UTC record updated --new GALILEO UTC time of Week =" << galileo_utc.t0t_6;
                                    global_galileo_utc_model_map.write(0, galileo_utc);
                                }
                            else
                                {
                                    LOG(INFO) << "Not updating the existing UTC in global map, timestamp is not changing";
                                }
                        }
                }
            else
                {
                    // insert new ephemeris record
                    LOG(INFO) << "New UTC record inserted in global map";
                    global_galileo_utc_model_map.write(0, galileo_utc);
                }
        }
}


void ControlThread::keyboard_listener()
{
    bool read_keys = true;
    char c;
    while(read_keys)
        {
            c = std::cin.get();
            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;
                }
        }
}