//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of gnss_utils
// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "gtest/utils_gtest.h"
#include "gnss_utils/utils/satellite.h"
#include "gnss_utils/utils/transformations.h"

// Geodetic system parameters
static double kSemimajorAxis = 6378137;
static double kSemiminorAxis = 6356752.3142;
static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
static double kSecondEccentricitySquared = 6.73949674228 * 0.001;

using namespace GnssUtils;

Eigen::Vector3d computeRandomReceiverLatLonAlt()
{
    Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
    receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
    receiver_LLA(1) *= M_PI;     // in [-PI, PI]
    receiver_LLA(2) += 1;        // ([0, 2])
    receiver_LLA(2) *= 5e2;      // in [0, 1e3]

    return receiver_LLA;
}

void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
                                   Eigen::Vector3d& sat_ENU,
                                   Eigen::Vector3d& sat_ECEF,
                                   Eigen::Vector2d& sat_azel,
                                   double range)
{
    Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
    Eigen::Matrix3d R_ENU_ECEF;

    t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);

    computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);

    // random elevation and azimuth
    sat_azel = Eigen::Vector2d::Random();               // in [-1, 1]
    sat_azel(0) *= M_PI;                                // in [-pi, pi]
    sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2;     // in [0, pi/2]
    range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3;  // in [500, 1500]

    // ENU
    sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
               cos(sat_azel(0)) * cos(sat_azel(1)) * range,
               sin(sat_azel(1)) * range;

    // ECEF
    sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
}


TEST(TransformationsTest, ecefToLatLonAlt)
{
    Eigen::Vector3d ecef, latlonalt;

    // aligned with ECEF X axis
    ecef << 1e3, 0, 0;
    latlonalt = ecefToLatLonAlt(ecef);

    ASSERT_NEAR(latlonalt(0),0.0,1e-6);
    ASSERT_NEAR(latlonalt(1),0.0,1e-6);

    // aligned with ECEF Y axis
    ecef << 0, 1e3, 0;
    latlonalt = ecefToLatLonAlt(ecef);

    ASSERT_NEAR(latlonalt(0),0.0,1e-6);
    ASSERT_NEAR(latlonalt(1),M_PI / 2,1e-6);

    // aligned with ECEF Z axis
    ecef << 0, 0, 1e3;
    latlonalt = ecefToLatLonAlt(ecef);

    ASSERT_NEAR(latlonalt(0),M_PI / 2,1e-6);
    ASSERT_NEAR(latlonalt(1),0.0,1e-6);
}

TEST(TransformationsTest, latLonAltToEcef)
{
    Eigen::Vector3d ecef, latlonalt;

    // aligned with ECEF X axis
    latlonalt << 0, 0, 0;
    ecef = latLonAltToEcef(latlonalt);

    ASSERT_NEAR(ecef(1),0.0,1e-6);
    ASSERT_NEAR(ecef(2),0.0,1e-6);

    // aligned with ECEF Y axis
    latlonalt << 0, M_PI / 2, 0;
    ecef = latLonAltToEcef(latlonalt);

    ASSERT_NEAR(ecef(0),0.0,1e-6);
    ASSERT_NEAR(ecef(2),0.0,1e-6);

    // aligned with ECEF Z axis
    latlonalt << M_PI / 2, 0, 0;
    ecef = latLonAltToEcef(latlonalt);

    ASSERT_NEAR(ecef(0),0.0,1e-6);
    ASSERT_NEAR(ecef(1),0.0,1e-6);
}

TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
{
    Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1;

    // ecef -> latlon -> ecef
    for (auto i = 0; i<100; i++)
    {
        ecef0 = 1e3 * Eigen::Vector3d::Random();

        latlonalt0 = ecefToLatLonAlt(ecef0);
        ecef1 = latLonAltToEcef(latlonalt0);
        ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6);
    }
    // latlon -> ecef -> latlon
    for (auto i = 0; i<100; i++)
    {
        latlonalt0 = computeRandomReceiverLatLonAlt();

        ecef0 = latLonAltToEcef(latlonalt0);
        latlonalt1 = ecefToLatLonAlt(ecef0);
        ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6);
    }
}

TEST(TransformationsTest, computeEnuEcef)
{
    Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1;
    Eigen::Vector3d t_ENU_ECEF1,t_ENU_ECEF2,t_ENU_ECEF3;
    Eigen::Matrix3d R_ENU_ECEF1,R_ENU_ECEF2,R_ENU_ECEF3;

    // random
    for (auto i = 0; i<100; i++)
    {
        t_ENU_latlonalt = computeRandomReceiverLatLonAlt();

        t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);

        // 1 from ECEF
        computeEnuEcefFromEcef(t_ECEF_ENU, R_ENU_ECEF1, t_ENU_ECEF1);
        // 2 from latlonalt
        computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF2, t_ENU_ECEF2);
        // 3 Hardcoded solution
        /* Convert ECEF coordinates to geodetic coordinates.
         *    J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
         *    to geodetic coordinates," IEEE Transactions on Aerospace and
         *    Electronic Systems, vol. 30, pp. 957-961, 1994.
         */
        double r = std::sqrt(t_ECEF_ENU(0) * t_ECEF_ENU(0) + t_ECEF_ENU(1) * t_ECEF_ENU(1));
        double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
        double F = 54 * kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) * t_ECEF_ENU(2);
        double G = r * r + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
        double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
        double S = cbrt(1 + C + sqrt(C * C + 2 * C));
        double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
        double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
        double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
                   + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
                   - P * (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
        double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2));
        double Z_0 = kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) / (kSemimajorAxis * V);

        double latitude = atan((t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
        double longitude = atan2(t_ECEF_ENU(1), t_ECEF_ENU(0));

        double sLat = sin(latitude);
        double cLat = cos(latitude);
        double sLon = sin(longitude);
        double cLon = cos(longitude);

        R_ENU_ECEF3(0,0) = -sLon;
        R_ENU_ECEF3(0,1) =  cLon;
        R_ENU_ECEF3(0,2) =  0.0;

        R_ENU_ECEF3(1,0) = -sLat*cLon;
        R_ENU_ECEF3(1,1) = -sLat * sLon;
        R_ENU_ECEF3(1,2) =  cLat;

        R_ENU_ECEF3(2,0) =  cLat * cLon;
        R_ENU_ECEF3(2,1) =  cLat * sLon;
        R_ENU_ECEF3(2,2) =  sLat;

        t_ENU_ECEF3 = -R_ENU_ECEF3*t_ECEF_ENU;

        // CHECK
        ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF2,1e-6);
        ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF2,1e-6);
        ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF3,1e-6);
        ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF3,1e-6);
    }
}

TEST(TransformationsTest, computeAzel)
{
    Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF;
    Eigen::Matrix3d R_ENU_ECEF;
    Eigen::Vector2d azel, azel2;
    double range;

    // random receiver position
    for (auto i = 0; i<100; i++)
    {
        t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
        t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);

        // random elevation and heading
        for (auto j = 0; j<100; j++)
        {
            computeRandomVisibleSatellite(t_ENU_latlonalt, sat_ENU, sat_ECEF, azel, range);

            azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);

            ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
        }
    }
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}