/*
 * sat_position.cpp
 *
 *  Created on: April 3, 2020
 *      Author: Joan Vallvé, Pep Martí-Saumell
 */

#include "gnss_utils/utils/satellite.h"

#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
#include "gnss_utils/gnss_utils.h"

namespace GnssUtils
{
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
    // ecef 2 geodetic
    Eigen::Vector3d receiver_geo;
    ecef2pos(receiver_ecef.data(), receiver_geo.data());

    // receiver-sat vector ecef
    Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;

    // receiver-sat vector enu (receiver ecef as origin)
    Eigen::Vector3d receiver_sat_enu;
    ecef2enu(receiver_geo.data(),       // geodetic position {lat,lon} (rad)
             receiver_sat_ecef.data(),  // vector in ecef coordinate {x,y,z}
             receiver_sat_enu.data());  // vector in local tangental coordinate {e,n,u}

    // elevation
    return atan2(receiver_sat_enu(2),
                 sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
}

Satellites computeSatellites(const Observations&             obs,
                             const Navigation&               nav,
                             const int&                      eph_opt)
{
    double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
    int    svh[obs.size()];

    //   std::cout << "computing position of sats: ";
    //   for (auto&& obs_ref : obs.getObservations())
    //      std::cout << (int)obs_ref.sat << " ";
    //   std::cout << std::endl;

    // compute positions
    if (not obs.getObservations().empty())
        satposs(obs.getObservations().front().time,
                obs.data(),
                obs.size(),
                &nav.getNavigation(),
                eph_opt,
                rs,
                dts,
                var,
                svh);

    // fill Satellites
    Satellites sats;
    //std::cout << "computeSatellites: filling satellites: \n";
    for (int i = 0; i < obs.size(); i++)
    {
        auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
                                     Satellite({satsys(obs.getObservationByIdx(i).sat,NULL),
                                                obs.getObservationByIdx(i).sat, // Constructor...
                                                        (Eigen::Vector3d() << rs[6*i],rs[6*i+1],rs[6*i+2]).finished(),
                                                        (Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished() ,
                                                var[i],
                                                dts[2*i],
                                                dts[2*i+1],
                                                svh[i]}));
        /*//DEBUG
        if (sat_pair.first->second.pos == Eigen::Vector3d::Zero())
            std::cout << "\tsat: " << sat_pair.first->second.sat << " ephemeris not available" << std::endl;
        else
        {
            std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl
                      << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl
                      << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl
                      << "\t\tvar: " << sat_pair.first->second.var << std::endl
                      << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl
                      << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl
                      << "\t\tsvh: " << sat_pair.first->second.svh << std::endl;
        }//*/
    }

    return sats;
}

}  // namespace GnssUtils