/*!
 * \file front_end_cal.cc
 * \brief Implementation of the Front-end calibration program.
 * \author Javier Arribas, 2013. jarribas(at)cttc.es
 *
 *
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2013  (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 <exception>
#include <boost/filesystem.hpp>
#include <gflags/gflags.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <boost/lexical_cast.hpp>
#include "file_configuration.h"
#include "gps_navigation_message.h"
#include "gps_ephemeris.h"
#include "gps_almanac.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "gnss_sdr_supl_client.h"
#include <sys/time.h>
#include <ctime>
#include <memory>
#include <math.h>
#include "front_end_cal.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;

FrontEndCal::FrontEndCal()
{}

FrontEndCal::~FrontEndCal()
{}

bool FrontEndCal::read_assistance_from_XML()
{
    gnss_sdr_supl_client supl_client_ephemeris_;
    std::string eph_xml_filename = "gps_ephemeris.xml";
    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;
                    std::cout << "New Ephemeris record inserted with Toe=" << gps_eph_iter->second.d_Toe << " and GPS Week=" << gps_eph_iter->second.i_GPS_week << std::endl;
                    global_gps_ephemeris_map.write(gps_eph_iter->second.i_satellite_PRN, gps_eph_iter->second);
                }
            return true;
        }
    else
        {
            std::cout <<  "ERROR: SUPL client error reading XML" << std::endl;
            return false;
        }
}

int FrontEndCal::Get_SUPL_Assist()
{
    //#########GNSS Asistence #################################
    gnss_sdr_supl_client supl_client_acquisition_;
    gnss_sdr_supl_client supl_client_ephemeris_;
    int supl_mcc; // Current network MCC (Mobile country code), 3 digits.
    int supl_mns; //Current network MNC (Mobile Network code), 2 or 3 digits.
    int supl_lac; // Current network LAC (Location area code),16 bits, 1-65520 are valid values.
    int supl_ci;  // Cell Identity (16 bits, 0-65535 are valid values).
    // GNSS Assistance configuration
    int error = 0;
    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!
        {
            DLOG(INFO) << "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
                    read_assistance_from_XML();
                }
            else
                {
                    // Request ephemeris from SUPL server
                    supl_client_ephemeris_.request = 1;
                    DLOG(INFO) << "SUPL: Trying to read GPS ephemeris from SUPL server...";
                    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++)
                                {
                                    DLOG(INFO)  << "SUPL: Received Ephemeris for GPS SV " << gps_eph_iter->first;
                                    DLOG(INFO)  << "New Ephemeris record inserted with Toe=" << gps_eph_iter->second.d_Toe << " and GPS Week=" << gps_eph_iter->second.i_GPS_week;
                                    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 = "gps_ephemeris.xml";
                            if (supl_client_ephemeris_.save_ephemeris_xml(eph_xml_filename) == true)
                                {
                                    DLOG(INFO)  << "SUPL: XML Ephemeris file created.";
                                }
                        }
                    else
                        {
                            DLOG(INFO) << "ERROR: SUPL client for Ephemeris returned " << error;
                            DLOG(INFO) << "Please check Internet connection and SUPL server configuration" << error;
                        }

                    // Request almanac , IONO and UTC Model
                    supl_client_ephemeris_.request = 0;
                    DLOG(INFO) << "SUPL: Try read Almanac, Iono, Utc Model, Ref Time and Ref Location from SUPL server...";
                    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++)
                                {
                                    DLOG(INFO)  << "SUPL: Received Almanac for GPS SV " << gps_alm_iter->first;
                                    global_gps_almanac_map.write(gps_alm_iter->first, gps_alm_iter->second);
                                }
                            if (supl_client_ephemeris_.gps_iono.valid == true)
                                {
                                    DLOG(INFO)  << "SUPL: Received GPS Iono";
                                    global_gps_iono_map.write(0,supl_client_ephemeris_.gps_iono);
                                }
                            if (supl_client_ephemeris_.gps_utc.valid == true)
                                {
                                    DLOG(INFO)  << "SUPL: Received GPS UTC Model";
                                    global_gps_utc_model_map.write(0, supl_client_ephemeris_.gps_utc);
                                }
                        }
                    else
                        {
                            DLOG(INFO) << "ERROR: SUPL client for Almanac returned " << error;
                            DLOG(INFO) << "Please check Internet connection and SUPL server configuration" << error;
                        }

                    // Request acquisition assistance
                    supl_client_acquisition_.request = 2;
                    DLOG(INFO) << "SUPL: Trying to read Acquisition assistance from SUPL server...";

                    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++)
                                {
                                    DLOG(INFO) << "SUPL: Received Acquisition assistance for GPS SV " << gps_acq_iter->first;
                                    DLOG(INFO) << "New acq assist record inserted";
                                    global_gps_acq_assist_map.write(gps_acq_iter->second.i_satellite_PRN, gps_acq_iter->second);
                                }
                        }
                    else
                        {
                            DLOG(INFO) << "ERROR: SUPL client for Acquisition assistance returned " << error;
                            DLOG(INFO) << "Please check internet connection and SUPL server configuration" << error;
                        }
                }
        }
    return error;
}


void FrontEndCal::set_configuration(ConfigurationInterface *configuration)
{
    configuration_ = configuration;
}


bool FrontEndCal::get_ephemeris()
{
    bool read_ephemeris_from_xml=configuration_->property("GNSS-SDR.read_eph_from_xml", false);

    if (read_ephemeris_from_xml == true)
        {
            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: Trying to get ephemeris from SUPL server..." << std::endl;
                    if (Get_SUPL_Assist() == 1)
                        {
                            return true;
                        }
                    else
                        {
                            return false;
                        }
                }
            else
                {
                    return true;
                }
        }
    else
        {
            std::cout <<  "Trying to read ephemeris from SUPL server..." << std::endl;
            if (Get_SUPL_Assist() == 0)
                {
                    return true;
                }
            else
                {
                    return false;
                }
        }
}


arma::vec FrontEndCal::lla2ecef(arma::vec lla)
{
    // WGS84 flattening
    double f;
    f  = 1/298.257223563;

    // WGS84 equatorial radius
    double R;
    R =  6378137;

    double phi, lambda;
    arma::vec ellipsoid = "0.0 0.0";
    phi = (lla(0)/360.0) * GPS_TWO_PI;
    lambda = (lla(1)/360.0) * GPS_TWO_PI;

    ellipsoid(0) = R;
    ellipsoid(1) = sqrt(1-(1-f)*(1-f));

    arma::vec ecef = "0.0 0.0 0.0 0.0";
    ecef = geodetic2ecef(phi, lambda, lla(3), ellipsoid);

    return ecef;
}


arma::vec FrontEndCal::geodetic2ecef(double phi, double lambda, double h, arma::vec ellipsoid)
{
    double a;
    a  = ellipsoid(0);
    double e2;
    e2 = ellipsoid(1)*ellipsoid(1);

    double sinphi, cosphi;

    sinphi = sin(phi);
    cosphi = cos(phi);
    double N;
    N  = a / sqrt(1 - e2 * sinphi*sinphi);

    arma::vec ecef = "0.0 0.0 0.0 0.0";

    ecef(0) = (N + h) * cosphi * cos(lambda);
    ecef(1) = (N + h) * cosphi * sin(lambda);
    ecef(2) = (N*(1 - e2) + h) * sinphi;

    return ecef;
}


double FrontEndCal::estimate_doppler_from_eph(unsigned int PRN, double TOW, double lat, double lon, double height)
{
    int num_secs = 10;
    double step_secs = 0.5;

    //Observer position ECEF
    arma::vec obs_ecef = "0.0 0.0 0.0 0.0";
    arma::vec lla = "0.0 0.0 0.0 0.0";
    lla(0) = lat;
    lla(1) = lon;
    lla(2) = height;
    obs_ecef = lla2ecef(lla);

    //Satellite positions ECEF
    std::map<int,Gps_Ephemeris> eph_map;
    eph_map = global_gps_ephemeris_map.get_map_copy();

    std::map<int,Gps_Ephemeris>::iterator eph_it;
    eph_it = eph_map.find(PRN);

    if (eph_it!=eph_map.end())
        {
            arma::vec SV_pos_ecef = "0.0 0.0 0.0 0.0";
            double obs_time_start, obs_time_stop;
            obs_time_start = TOW - num_secs/2;
            obs_time_stop = TOW + num_secs/2;
            int n_points = round((obs_time_stop - obs_time_start)/step_secs);
            arma::vec ranges = arma::zeros(n_points, 1);
            double obs_time = obs_time_start;
            for (int i=0; i<n_points; i++)
                {
                    eph_it->second.satellitePosition(obs_time);
                    SV_pos_ecef(0) = eph_it->second.d_satpos_X;
                    SV_pos_ecef(1) = eph_it->second.d_satpos_Y;
                    SV_pos_ecef(2) = eph_it->second.d_satpos_Z;
                    // SV distances to observer (true range)
                    ranges(i) = arma::norm(SV_pos_ecef-obs_ecef, 2);

                    obs_time += step_secs;
                }
            // Observer to satellite radial velocity
            // Numeric derivative: Positive slope means that the distance from obs to
            // satellite is increasing
            arma::vec obs_to_sat_velocity;
            obs_to_sat_velocity = (ranges.subvec(1, (n_points-1)) - ranges.subvec(0, (n_points-2)))/step_secs;
            // Doppler equations are formulated accounting for positive velocities if the
            // tx and rx are approaching to each other. So, the satellite velocity must
            // be redefined as:

            obs_to_sat_velocity = -obs_to_sat_velocity;

            //Doppler estimation
            arma::vec Doppler_Hz;
            Doppler_Hz = (obs_to_sat_velocity/GPS_C_m_s)*GPS_L1_FREQ_HZ;
            double mean_Doppler_Hz;
            mean_Doppler_Hz = arma::mean(Doppler_Hz);
            return mean_Doppler_Hz;
            return 0;
        }
    else
        {
            throw(1);
        }
}


void FrontEndCal::GPS_L1_front_end_model_E4000(double f_bb_true_Hz, double f_bb_meas_Hz, double fs_nominal_hz, double *estimated_fs_Hz, double *estimated_f_if_Hz, double *f_osc_err_ppm)
{
    double f_osc_n = 28.8e6;
    //PLL registers settings (according to E4000 datasheet)

    double N = 109;
    double Y = 65536;
    double X = 26487;
    double R = 2;
    // Obtained RF center frequency
    double f_rf_pll;
    f_rf_pll = (f_osc_n*(N+X/Y))/R;
    // RF frequency error caused by fractional PLL roundings
    double f_bb_err_pll;
    f_bb_err_pll = GPS_L1_FREQ_HZ - f_rf_pll;

    // Measured F_rf error
    double f_rf_err;
    f_rf_err = (f_bb_meas_Hz - f_bb_true_Hz) - f_bb_err_pll;

    double f_osc_err_hz;
    f_osc_err_hz = (f_rf_err*R)/(N+X/Y);

    // OJO,segun los datos gnss, la IF positiva hace disminuir la fs!!
    f_osc_err_hz = -f_osc_err_hz;

    *f_osc_err_ppm = f_osc_err_hz/(f_osc_n/1e6);
    double frac;
    frac = fs_nominal_hz/f_osc_n;

    *estimated_fs_Hz = frac*(f_osc_n + f_osc_err_hz);
    *estimated_f_if_Hz = f_rf_err;
}