// SPDX-License-Identifier: LGPL-2.1-or-later
//
// SPDX-FileCopyrightText: 2012 Gerhard HOLTKAMP
//

/***************************************************************************
 * Calculate Spacecraft around other planets                                *
 *                                                                          *
 *                                                                          *
 * Open Source Code. License: GNU LGPL Version 2+                          *
 *                                                                          *
 * Author: Gerhard HOLTKAMP,        26-AUG-2012                              *
 ***************************************************************************/

/*------------ include files and definitions -----------------------------*/
#include "planetarySats.h"
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iostream>
using namespace std;

#include "astrolib.h"

// ################ Planetary Sats Class ####################

PlanetarySats::PlanetarySats()
{
    plsatinit();
}

PlanetarySats::~PlanetarySats() = default;

double PlanetarySats::atan23(double y, double x)
{
    // redefine atan2 so that it doesn't crash when both x and y are 0
    double result;

    if ((x == 0) && (y == 0))
        result = 0;
    else
        result = atan2(y, x);

    return result;
}

void PlanetarySats::plsatinit()
{
    // initialize planetary sat data
    pls_moonflg = false;
    pls_day = 1;
    pls_month = 1;
    pls_year = 2012;
    pls_hour = 0;
    pls_minute = 0;
    pls_second = 0;
    pls_del_auto = 1;
    pls_step = 60.0;
    pls_delta_rt = 0.0;
    getTime();
    getMars();

    strcpy(pls_satelmfl, "./planetarysats.txt");
}

void PlanetarySats::getTime() // Get System Time and Date
{
    time_t tt;
    int hh, mm, ss;
    double jd, hr;

    tt = time(NULL);
    jd = 40587.0 + tt / 86400.0; // seconds since 1-JAN-1970

    jd = jd + pls_delta_rt / 24.0;
    caldat(jd, hh, mm, ss, hr);
    pls_year = ss;
    pls_month = mm;
    pls_day = hh;

    dms(hr, hh, mm, jd);
    pls_hour = hh;
    pls_minute = mm;
    pls_second = int(jd);
    if (pls_del_auto)
        pls_del_tdut = DefTdUt(pls_year);
    setMJD(pls_year, pls_month, pls_day, hh, mm, jd);
};

void PlanetarySats::setStepWidth(double s)
{
    // set the step width (in seconds) used for calculations
    if (s < 0.01)
        pls_step = 0.01;
    else
        pls_step = s;
}

void PlanetarySats::setDeltaRT(double drt)
{
    pls_delta_rt = drt;
}

void PlanetarySats::setDeltaTAI_UTC(double d)
{
    // c is the difference between TAI and UTC according to the IERS
    // we have to add 32.184 sec to get to the difference TT - UT
    // which is used in the calculations here

    pls_del_tdut = d + 32.184;
    pls_del_auto = 0;
}

void PlanetarySats::setAutoTAI_UTC()
{
    // set the difference between TAI and UTC according to the IERS
    pls_del_auto = true;
    pls_del_tdut = DefTdUt(pls_year);
}

void PlanetarySats::setMJD(int year, int month, int day, int hour, int min, double sec)
{
    // set the (MJD-) time currently used for calculations to year, month, day, hour, min, sec
    double jd;

    pls_year = year;
    pls_month = month;
    pls_day = day;
    pls_hour = hour;
    pls_minute = min;
    pls_second = sec;

    jd = ddd(hour, min, sec);
    jd = mjd(day, month, year, jd);

    pls_time = jd;

    if (pls_del_auto)
        pls_del_tdut = DefTdUt(pls_year);
}

void PlanetarySats::getDatefromMJD(double mjd, int &year, int &month, int &day, int &hour, int &min, double &sec)
{
    // convert times given in Modified Julian Date (MJD) into conventional date and time

    double magn;

    caldat((mjd), day, month, year, magn);
    dms(magn, hour, min, sec);
    if (sec > 30.0)
        min++;
    if (min > 59) {
        hour++;
        min = 0;
    };
}

void PlanetarySats::setSatFile(char *fname)
{
    strcpy(pls_satelmfl, fname);
}

void PlanetarySats::setStateVector(double mjd, double x, double y, double z, double vx, double vy, double vz)
{
    pls_rep[0] = x;
    pls_rep[1] = y;
    pls_rep[2] = z;
    pls_vep[0] = vx;
    pls_vep[1] = vy;
    pls_vep[2] = vz;

    int year = 0;
    int month = 0;
    int day = 0;
    int hour = 0;
    int min = 0;
    double sec = 0;
    getDatefromMJD(mjd, year, month, day, hour, min, sec);
    setMJD(year, month, day, hour, min, sec);
    pls_tepoch = pls_time;
    // pls_tepoch = pls_time + pls_del_tdut / 86400.0;  // epoch in TT
}

int PlanetarySats::getStateVector(int nsat)
{
    // read the state vector from the planetary sat file
    // nsat = number of eligible sat to select (1 if first or only sat)
    // RETURN number of eligible sat selected, 0 if no suitable sats of file problems

    int fsc, j, k, nst, utc;
    int yr, month, day, hour, min;
    double sec;
    bool searching;
    ifstream cfle;
    char satname[40]; // name of satellite
    char plntname[40]; // name of planet

    strcpy(satname, "");
    strcpy(plntname, "");
    nst = 0;

    searching = true;
    fsc = 0;

    cfle.open(pls_satelmfl, ios::in);
    if (!cfle) {
        searching = false;
        cfle.close();
    };
    if (searching) {
        while (searching) {
            fsc = 1;

            if (!cfle.getline(satname, 40))
                fsc = 0;
            else {
                k = strlen(satname);
                if ((k > 1) && (satname[0] == '#')) {
                    for (j = 1; j < k; ++j) {
                        pls_satname[j - 1] = satname[j];
                        if (pls_satname[j - 1] == '\n')
                            pls_satname[j - 1] = '\0';
                    };
                    pls_satname[k - 1] = '\0';
                } else
                    fsc = 0;
            };

            if (cfle.eof()) {
                fsc = 0;
                searching = false;
            };

            if (fsc) {
                if (!cfle.getline(plntname, 40))
                    fsc = 0;
                else {
                    k = strlen(plntname);
                    if (k > 0) {
                        if (plntname[k - 1] == '\n')
                            plntname[k - 1] = '\0';
                        if ((k > 1) && (plntname[k - 2] == '\r'))
                            plntname[k - 2] = '\0';
                    };
                };
            };

            if (fsc) {
                cfle >> yr >> month >> day >> hour >> min >> sec >> utc;
                if (cfle.bad())
                    fsc = 0;
                if (cfle.eof()) {
                    fsc = 0;
                    searching = false;
                };
            };

            if (fsc) {
                cfle >> pls_rep[0] >> pls_rep[1] >> pls_rep[2];
                if (cfle.bad())
                    fsc = 0;
                if (cfle.eof()) {
                    fsc = 0;
                    searching = false;
                };
            };

            if (fsc) {
                cfle >> pls_vep[0] >> pls_vep[1] >> pls_vep[2];
                if (cfle.bad())
                    fsc = 0;
            };

            if (fsc) {
                if (strncmp(pls_plntname, plntname, 4) == 0)
                    nst++;
                if (nst == nsat) {
                    searching = false;

                    setMJD(yr, month, day, hour, min, sec);
                    pls_tepoch = pls_time;
                    if (utc)
                        pls_tepoch = pls_tepoch + pls_del_tdut / 86400.0; // epoch in TT
                };
            };
        };
        cfle.close();
    };

    if (fsc == 0)
        nst = 0;

    return nst;
}

void PlanetarySats::setPlanet(char *pname)
{
    pls_moonflg = false;
    strcpy(pls_plntname, pname);
    if (strncmp("Mars", pname, 4) == 0)
        getMars();
    if (strncmp("Venus", pname, 4) == 0)
        getVenus();
    if (strncmp("Mercury", pname, 4) == 0)
        getMercury();
    if (strncmp("Moon", pname, 4) == 0)
        getMoon();
}

void PlanetarySats::stateToKepler()
{
    // convert state vector (mean equatorial J2000.0) into planetary Kepler elements

    double t, dt, ag, gm, re, j2;
    double n, c, w, a, ecc, inc;
    Vec3 r1, v1;
    Mat3 mx;

    dt = (pls_tepoch - 51544.5) / 36525.0;
    gm = pls_GM * 7.4649600000; // convert from m^3/s^2 into km^3/d^2
    re = pls_R0;
    j2 = pls_J2;

    // convert into planet equatorial reference frame
    if (pls_moonflg) {
        mx = mxidn();
        r1 = mxvct(mx, pls_rep);
        v1 = mxvct(mx, pls_vep);

    } else {
        ag = (pls_axl0 + pls_axl1 * dt) * M_PI / 180.0;
        mx = zrot(ag + M_PI / 2.0);
        r1 = mxvct(mx, pls_rep);
        v1 = mxvct(mx, pls_vep);

        ag = (pls_axb0 + pls_axb1 * dt) * M_PI / 180.0;
        mx = xrot(M_PI / 2.0 - ag);
        r1 = mxvct(mx, r1);
        v1 = mxvct(mx, v1);
    };

    v1 *= 86400.0; // convert into km / day

    oscelm(gm, pls_tepoch, r1, v1, t, pls_m0, pls_a, pls_ecc, pls_ra, pls_per, pls_inc);

    // now the mean motion
    a = pls_a;
    ecc = pls_ecc;
    inc = pls_inc;

    //  preliminary n
    if (a == 0)
        a = 1.0; // just in case
    if (a < 0)
        a = -a; // just in case
    n = sqrt(gm / (a * a * a));

    // correct for J2 term
    w = 1.0 - ecc * ecc;
    if (w > 0) {
        w = pow(w, -1.5);
        c = sin(inc * M_PI / 180.0);
        n = n * (1.0 + 1.5 * j2 * re * re / (a * a) * w * (1.0 - 1.5 * c * c));
    } else
        n = 1.0; // do something to avoid a domain error

    n = n / (2.0 * M_PI);
    if (n > 1000.0)
        n = 1000.0; // avoid possible errors

    pls_n0 = n;
}

void PlanetarySats::getKeplerElements(double &perc, double &apoc, double &inc, double &ecc, double &ra, double &tano, double &m0, double &a, double &n0)
{
    // get Kepler elements of orbit with regard to the planetary equator and prime meridian.

    double t, gm;
    Vec3 r1, v1;
    Mat3 mx;

    if (pls_moonflg) // for the Moon we normally work in J2000. Now get it into planetary
    {
        gm = pls_GM * 7.4649600000; // convert from m^3/s^2 into km^3/d^2

        mx = getSelenographic(pls_tepoch);
        r1 = mxvct(mx, pls_rep);
        v1 = mxvct(mx, pls_vep);
        v1 *= 86400.0; // convert into km / day

        oscelm(gm, pls_tepoch, r1, v1, t, m0, a, ecc, ra, tano, inc);

        // now the mean motion
        if (a == 0)
            a = 1.0; // just in case
        if (a < 0)
            a = -a; // just in case
        n0 = sqrt(gm / (a * a * a));
        n0 = n0 / (2.0 * M_PI);
    } else {
        a = pls_a;
        n0 = pls_n0;
        m0 = pls_m0;
        tano = pls_per;
        ra = pls_ra;
        ecc = pls_ecc;
        inc = pls_inc;
    };

    perc = pls_a * (1.0 - pls_ecc) - pls_R0;
    apoc = pls_a * (1.0 + pls_ecc) - pls_R0;
}

int PlanetarySats::selectSat(char *sname)
{
    // select specified satellite
    // RETURN 1 if successful, 0 if no suitable satellite found

    int nst, res, sl;
    bool searching;

    searching = true;
    nst = 1;
    sl = strlen(sname);

    while (searching) {
        res = getStateVector(nst);
        if (res) {
            if (strncmp(pls_satname, sname, sl) == 0)
                searching = false;
        } else
            searching = false;
        nst++;
    };

    return res;
}

void PlanetarySats::getSatName(char *sname) const
{
    strcpy(sname, pls_satname);
}

void PlanetarySats::currentPos()
{
    getSatPos(pls_time);
}

void PlanetarySats::nextStep()
{
    pls_time = pls_time + pls_step / 86400.0;
    getSatPos(pls_time);
}

double PlanetarySats::getLastMJD() const
{
    return pls_time;
}

void PlanetarySats::getPlanetographic(double &lng, double &lat, double &height) const
{
    // planetographic coordinates from current state vector

    lng = pls_lng;
    lat = pls_lat;
    height = pls_height;
}

void PlanetarySats::getFixedFrame(double &x, double &y, double &z, double &vx, double &vy, double &vz)
{
    // last state vector coordinates in planetary fixed frame

    x = pls_r[0];
    y = pls_r[1];
    z = pls_r[2];
    vx = pls_v[0];
    vy = pls_v[1];
    vz = pls_v[2];
}

void PlanetarySats::getSatPos(double tutc)
{
    // Get Position of Satellite at MJD-time t (UTC)

    const double mp2 = 2.0 * M_PI;

    double t, dt, m0, ran, aper, inc, a, ecc, n0, re;
    double f, e, c, s, k, sh, j2, gm, fac, b1, b2, b3;
    int j;

    Vec3 r1, v1, rg1, s2;
    Mat3 m1, m2;

    // prepare orbit calculation

    t = tutc + pls_del_tdut / 86400.0;
    dt = t - pls_tepoch;

    ecc = pls_ecc;
    if (ecc >= 1.0)
        ecc = 0.999; // to avoid crashes
    a = pls_a;
    n0 = mp2 * pls_n0;
    if (a < 1.0)
        a = 1.0; // avoid possible crashes later on
    re = pls_R0;
    f = pls_flat;
    j2 = pls_J2;
    gm = pls_GM * 7.4649600000; // convert from m^3/s^2 into km^3/d^2

    // get current orbit elements ready
    aper = (re / a) / (1.0 - ecc * ecc);
    aper = 1.5 * j2 * aper * aper * n0;
    m0 = pls_inc * M_PI / 180.0;
    ran = -aper * cos(m0) * dt;
    m0 = sin(m0);
    aper = aper * (2.0 - 2.5 * m0 * m0) * dt;
    ran = pls_ra * M_PI / 180.0 + ran;
    aper = pls_per * M_PI / 180.0 + aper;
    m0 = pls_m0 * M_PI / 180.0 + n0 * dt;
    inc = pls_inc * M_PI / 180.0;

    // solve Kepler equation
    if (a < 1.0)
        a = 1.0; // avoid possible crashes later on
    e = eccanom(m0, ecc);
    fac = sqrt(1.0 - ecc * ecc);
    c = cos(e);
    s = sin(e);
    r1.assign(a * (c - ecc), a * fac * s, 0.0);

    m0 = 1.0 - ecc * c;
    k = sqrt(gm / a);
    v1.assign(-k * s / m0, k * fac * c / m0, 0.0);

    // convert into reference plane
    m1 = zrot(-aper);
    m2 = xrot(-inc);
    m1 *= m2;
    m2 = zrot(-ran);
    m2 = m2 * m1;
    r1 = mxvct(m2, r1);
    v1 = mxvct(m2, v1);
    v1 /= 86400.0;

    // save state vector in planet-fixed frame

    if (pls_moonflg)
        m1 = getSelenographic(t);
    else
        m1 = zrot((pls_W + pls_Wd * (t - 51544.5)) * (M_PI / 180.0));
    pls_r = mxvct(m1, r1);
    pls_v = mxvct(m1, v1);
    pls_r *= 1000.0;
    pls_v *= 1000.0;

    // get the groundtrack coordinates

    rg1 = mxvct(m1, r1);

    s2 = carpol(rg1);
    pls_lat = s2[2]; // just preliminary
    pls_lng = s2[1]; // measured with the motion of rotation!
    if (pls_lng > mp2)
        pls_lng -= mp2;
    if (pls_lng < -M_PI)
        pls_lng += mp2;
    if (pls_lng > M_PI)
        pls_lng -= mp2;

    // get height above ellipsoid and geodetic latitude
    if (abs(r1) > 0.1) {
        if (f == 0)
            pls_height = abs(r1) - re;
        else {
            c = f * (2.0 - f);
            s = r1[0] * r1[0] + r1[1] * r1[1];
            sh = c * r1[2];

            for (j = 0; j < 4; ++j) {
                b1 = r1[2] + sh;
                b3 = sqrt(s + b1 * b1);
                if (b3 < 1e-5)
                    b3 = sin(pls_lat); // just in case
                else
                    b3 = b1 / b3;
                b2 = re / sqrt(1.0 - c * b3 * b3);
                sh = b2 * c * b3;
            };

            sh = r1[2] + sh;
            pls_lat = atan20(sh, sqrt(s));
            sh = sqrt(s + sh * sh) - b2;
            pls_height = sh;
        }
    } else
        pls_height = 0; // this should never happen

    pls_lat = pls_lat * 180.0 / M_PI;
    pls_lng = pls_lng * 180.0 / M_PI;
}

void PlanetarySats::getMars() // Mars planetary constants
{
    pls_J2 = 1.964e-3;
    pls_R0 = 3397.2;
    pls_flat = 0.00647630;
    pls_axl0 = 317.681;
    pls_axl1 = -0.108;
    pls_axb0 = 52.886;
    pls_axb1 = -0.061;
    pls_W = 176.868;
    pls_Wd = 350.8919830;
    pls_GM = 4.282828596416e+13; // 4.282837405582e+13
}

void PlanetarySats::getVenus() // Venus planetary constants
{
    pls_J2 = 0.027e-3;
    pls_R0 = 6051.9;
    pls_flat = 0.0;
    pls_axl0 = 272.72;
    pls_axl1 = 0.0;
    pls_axb0 = 67.15;
    pls_axb1 = 0.0;
    pls_W = 160.26;
    pls_Wd = -1.4813596;
    pls_GM = 3.24858761e+14;
}

void PlanetarySats::getMercury() // Mercury planetary constants
{
    pls_J2 = 0.0;
    pls_R0 = 2439.7;
    pls_flat = 0.0;
    pls_axl0 = 281.01;
    pls_axl1 = -0.033;
    pls_axb0 = 61.45;
    pls_axb1 = -0.005;
    pls_W = 329.71;
    pls_Wd = 6.1385025;
    pls_GM = 2.20320802e+13;
}

void PlanetarySats::getMoon() // Moon planetary constants
{
    pls_moonflg = true;
    pls_J2 = 0.2027e-3;
    pls_R0 = 1738.0;
    pls_flat = 0.0;
    pls_axl0 = 0.0;
    pls_axl1 = 0.0;
    pls_axb0 = 90.0;
    pls_axb1 = 0.0;
    pls_W = 0.0;
    pls_Wd = 13.17635898;
    pls_GM = 4.90279412e+12;
}

Mat3 PlanetarySats::getSelenographic(double jd)
{
    // Calculate the Matrix to transform from Mean of J2000 into selenographic
    // coordinates at MJD time jd.

    double t, gam, gmp, l, omg, mln;
    double a, b, c, ic, gn, gp, omp;
    double const degrad = M_PI / 180.0;
    Mat3 m1, m2;

    t = (jd - 15019.5) / 36525.0;
    gam = 281.2208333 + ((0.33333333e-5 * t + 0.45277778e-3) * t + 1.7191750) * t;
    gmp = 334.3295556 + ((-0.125e-4 * t - 0.10325e-1) * t + 4069.0340333) * t;
    l = 279.6966778 + (0.3025e-3 * t + 36000.768925) * t;
    omg = 259.1832750 + ((0.22222222e-5 * t + 0.20777778e-2) * t - 1934.1420083) * t;
    b = 23.45229444 + ((0.50277778e-6 * t - 0.16388889e-5) * t - 0.130125e-1) * t;
    mln = 270.4341639 + ((0.1888889e-5 * t - 0.11333e-2) * t + 481267.8831417) * t;
    ic = 1.535 * degrad;
    gn = (l - gam) * degrad;
    gp = (mln - gmp) * degrad;
    omp = (gmp - omg) * degrad;
    a = -107.0 * cos(gp) + 37.0 * cos(gp + 2.0 * omp) - 11.0 * cos(2.0 * (gp + omp));
    a = a * 0.000277778 * degrad + ic;
    c = (-109.0 * sin(gp) + 37.0 * sin(gp + 2.0 * omp) - 11.0 * sin(2.0 * (gp + omp))) / sin(ic);
    c = (c * 0.000277778 + omg) * degrad;
    gn = -12.0 * sin(gp) + 59.0 * sin(gn) + 18.0 * sin(2.0 * omp);
    gn = gn * 0.000277778 * degrad; // tau

    b *= degrad;
    gam = cos(a) * cos(b) + sin(a) * sin(b) * cos(c);
    gmp = gam * gam;
    if (gmp > 1.0)
        gmp = 0;
    else
        gmp = sqrt(1.0 - gmp);
    gam = atan23(gmp, gam); // theta
    t = cos(a) * sin(b) - sin(a) * sin(b) * cos(c);
    l = -sin(a) * sin(c);

    gmp = atan23(l, t); // phi
    t = sin(a) * cos(b) - cos(a) * sin(b) * cos(c);
    l = -sin(b) * sin(c);
    a = atan23(l, t); // delta
    c = a + mln * degrad + gn - c; // psi

    // libration rotation matrix from Mean equator to true selenographic
    m1 = zrot(gmp);
    m2 = xrot(gam);
    m1 = m2 * m1;
    m2 = zrot(c);
    m1 = m2 * m1;

    t = julcent(jd);
    m2 = pmatequ(0, t); // convert from mean of J2000 to mean of epoch
    m1 = m1 * m2;

    return m1;
}
