#include "planet_overhead.h"

#include <math.h>
#include <stddef.h>
#include <string.h>


/*
    This should run with the python autogenerated ephemeris builder
                                :)
*/
#include "master_ephemeris.c"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#define SIDEREAL_DAY_SECONDS 86164.0905
#define EARTH_OMEGA          (2.0 * M_PI / SIDEREAL_DAY_SECONDS)

#define SECONDS_PER_DAY      86400.0

/* Approximate distances used only for the Moon topocentric correction. */
#define AU_KM                149597870.7
#define MOON_DISTANCE_AU     (384400.0 / AU_KM)
#define EARTH_RADIUS_AU      (6371.0 / AU_KM)

static bool ephemeris_elapsed_seconds(
    const SAM_M10Q_Data *gnss,
    double *elapsed_seconds_out)
{
    if ((gnss == NULL) || (elapsed_seconds_out == NULL)){return false;}

    if ((gnss->year < EPHEMERIS_START_YEAR) ||
        (gnss->year > EPHEMERIS_END_YEAR) ||
        (gnss->month < 1U) || (gnss->month > 12U) ||
        (gnss->day < 1U) || (gnss->day > 31U) ||
        (gnss->hour > 23U) ||
        (gnss->minute > 59U) ||
        (gnss->second > 60U))
    {
        return false;
    }

    uint16_t year_index = (uint16_t)(gnss->year - EPHEMERIS_START_YEAR);
    int32_t days = (int32_t)ephemeris_month_start_days[year_index][gnss->month - 1U] + ((int32_t)gnss->day - 1);

    double elapsed_seconds =
        ((double)days * SECONDS_PER_DAY) +
        ((double)gnss->hour * 3600.0) +
        ((double)gnss->minute * 60.0) +
        (double)gnss->second +
        ((double)gnss->millisecond / 1000.0);

    /* i think negative would mean they entered a time before the ephemeris */
    if (elapsed_seconds < 0.0){return false;}
    *elapsed_seconds_out = elapsed_seconds;
    return true;
}

static double chebyshev_eval_int16(const int16_t *coefficients, uint8_t coeff_count, double u)
{
    double b1 = 0.0;
    double b2 = 0.0;
    for (int i = (int)coeff_count - 1; i > 0; i--)
    {
        double c = (double)coefficients[i] / EPHEMERIS_INT16_SCALE;
        double b0 = (2.0 * u * b1) - b2 + c;
        b2 = b1;
        b1 = b0;
    }
    double c0 = (double)coefficients[0] / EPHEMERIS_INT16_SCALE;
    return (u * b1) - b2 + c0;
}

static void icrf_vector_from_body(
    const ephemeris_body_t *body,
    double elapsed_seconds,
    double *x_out,
    double *y_out,
    double *z_out)
{
    const uint8_t coeff_count = (uint8_t)(body->cheb_degree + 1U);

    const double segment_seconds = (double)body->segment_days * SECONDS_PER_DAY;
    uint32_t segment_index = (uint32_t)(elapsed_seconds / segment_seconds);

    if (segment_index >= body->segment_count){segment_index = body->segment_count - 1U;}

    double segment_start_seconds = (double)segment_index * segment_seconds;
    double seconds_into_segment = elapsed_seconds - segment_start_seconds;
    double fraction = seconds_into_segment / segment_seconds;
    double u = (2.0 * fraction) - 1.0;

    /*
      for five coeffs, it's stored like this in c
      X0 X1 X2 X3 X4 X5 Y0 Y1 Y2 Y3 Y4 Y5 Z0 Z1 Z2 Z3 Z4 Z5
     */
    const int16_t *ephemeris = (const int16_t *)body->coefficients;

    uint32_t segment_offset = segment_index * 3U * (uint32_t)coeff_count;

    const int16_t *coeff_x = &ephemeris[segment_offset];

    const int16_t *coeff_y = &ephemeris[segment_offset + coeff_count];

    const int16_t *coeff_z = &ephemeris[segment_offset + (2U * coeff_count)];

    double x = chebyshev_eval_int16(coeff_x, coeff_count, u);

    double y = chebyshev_eval_int16(coeff_y, coeff_count, u);

    double z = chebyshev_eval_int16(coeff_z, coeff_count, u);

    double length = sqrt((x * x) + (y * y) + (z * z));

    if (length > 0.0)
    {
        x /= length;
        y /= length;
        z /= length;
    }

    *x_out = x;
    *y_out = y;
    *z_out = z;
}

static void icrf_to_ecef(
    double icrf_x,
    double icrf_y,
    double icrf_z,
    double elapsed_seconds,
    double *ecef_x,
    double *ecef_y,
    double *ecef_z)
{
    /*
        from python code:
        ## get earth's rotation for ECEF calculations
        ## R0_full is earth rotation matrix that would take a ECI vector
        ## and rotate it to its ECEF spot on earth
        ## But we cant store this many rotation matrices
        ## SIDEREAL_DAY_SECONDS = 86164.0905
        ## 1) Get total seconds, say dt_seconds and use OMEGA = 2.0 * np.pi / SIDEREAL_DAY_SECONDS
        ## 2) Use theta0 to get theta = theta0 + OMEGA * dt_seconds, then c = cos(theta), s = sin(theta)
        ## 2.5) theta0 is how far has the Earth-fixed X axis rotated around the Earth’s ECI spin axis
        ## 3) Build a Rz matrix, Rz = [[c, s, 0.0],[-s, c, 0.0],[0.0, 0.0, 1.0]]
        ## 4) Tilt the ECI vector by R_slow, then spin fast fast with Rz
        ## 4.5) ecef_approx = Rz @ R_slow @ eci
    */
    double slow_x =
        earth_r_slow[0][0] * icrf_x +
        earth_r_slow[0][1] * icrf_y +
        earth_r_slow[0][2] * icrf_z;

    double slow_y =
        earth_r_slow[1][0] * icrf_x +
        earth_r_slow[1][1] * icrf_y +
        earth_r_slow[1][2] * icrf_z;

    double slow_z =
        earth_r_slow[2][0] * icrf_x +
        earth_r_slow[2][1] * icrf_y +
        earth_r_slow[2][2] * icrf_z;

    double theta = (double)earth_theta0_rad + EARTH_OMEGA * elapsed_seconds;

    theta = fmod(theta, 2.0 * M_PI);

    if (theta < 0.0){theta += 2.0 * M_PI;}

    double c = cos(theta);
    double s = sin(theta);

    double x = (c * slow_x) + (s * slow_y);
    double y = (-s * slow_x) + (c * slow_y);
    double z = slow_z;

    double length = sqrt((x * x) + (y * y) + (z * z));

    if (length > 0.0)
    {
        x /= length;
        y /= length;
        z /= length;
    }

    *ecef_x = x;
    *ecef_y = y;
    *ecef_z = z;
}

static void moon_topocentric_ecef(
    double moon_icrf_x,
    double moon_icrf_y,
    double moon_icrf_z,
    double elapsed_seconds,
    double latitude_deg,
    double longitude_deg,
    double *out_x,
    double *out_y,
    double *out_z)
{
    double moon_ecef_x;
    double moon_ecef_y;
    double moon_ecef_z;

    icrf_to_ecef(
        moon_icrf_x,
        moon_icrf_y,
        moon_icrf_z,
        elapsed_seconds,
        &moon_ecef_x,
        &moon_ecef_y,
        &moon_ecef_z);

    // extended moon to essentially eliminate parallax moon issue

    moon_ecef_x *= MOON_DISTANCE_AU;
    moon_ecef_y *= MOON_DISTANCE_AU;
    moon_ecef_z *= MOON_DISTANCE_AU;

    double lat = latitude_deg * M_PI / 180.0;
    double lon = longitude_deg * M_PI / 180.0;

    double cos_lat = cos(lat);
    double sin_lat = sin(lat);
    double cos_lon = cos(lon);
    double sin_lon = sin(lon);


    double observer_x = EARTH_RADIUS_AU * cos_lat * cos_lon;

    double observer_y = EARTH_RADIUS_AU * cos_lat * sin_lon;

    double observer_z = EARTH_RADIUS_AU * sin_lat;

    double x = moon_ecef_x - observer_x;
    double y = moon_ecef_y - observer_y;
    double z = moon_ecef_z - observer_z;

    double length = sqrt((x * x) + (y * y) + (z * z));

    if (length > 0.0)
    {
        x /= length;
        y /= length;
        z /= length;
    }

    *out_x = x;
    *out_y = y;
    *out_z = z;
}

static double ecef_to_altitude(
    double ecef_x,
    double ecef_y,
    double ecef_z,
    double latitude_deg,
    double longitude_deg)
{
    double lat = latitude_deg * M_PI / 180.0;
    double lon = longitude_deg * M_PI / 180.0;

    double cos_lat = cos(lat);
    double sin_lat = sin(lat);
    double cos_lon = cos(lon);
    double sin_lon = sin(lon);

    double up_x = cos_lat * cos_lon;
    double up_y = cos_lat * sin_lon;
    double up_z = sin_lat;

    double up_value = (ecef_x * up_x) + (ecef_y * up_y) + (ecef_z * up_z);

    if (up_value > 1.0){up_value = 1.0;}
    else if (up_value < -1.0){up_value = -1.0;}
    return asin(up_value) * 180.0 / M_PI;
}

bool PlanetOverhead_Calculate(
    const SAM_M10Q_Data *gnss,
    planet_overhead_result_t *result)
{
    if ((gnss == NULL) || (result == NULL)){return false;}

    memset(result, 0, sizeof(*result));

    double elapsed_seconds;

    if (!ephemeris_elapsed_seconds(gnss, &elapsed_seconds)){return false;}

    for (uint32_t planet = 0; planet < PLANET_COUNT; planet++)
    {
        double icrf_x;
        double icrf_y;
        double icrf_z;

        icrf_vector_from_body(
            &ephemeris_bodies[planet],
            elapsed_seconds,
            &icrf_x,
            &icrf_y,
            &icrf_z);

        double ecef_x;
        double ecef_y;
        double ecef_z;

        if (planet == PLANET_MOON)
        {
            moon_topocentric_ecef(
                icrf_x,
                icrf_y,
                icrf_z,
                elapsed_seconds,
                gnss->latitude,
                gnss->longitude,
                &ecef_x,
                &ecef_y,
                &ecef_z);
        }
        else
        {
            icrf_to_ecef(
                icrf_x,
                icrf_y,
                icrf_z,
                elapsed_seconds,
                &ecef_x,
                &ecef_y,
                &ecef_z);
        }

        double altitude_deg =
            ecef_to_altitude(
                ecef_x,
                ecef_y,
                ecef_z,
                gnss->latitude,
                gnss->longitude);

        result->altitude_deg[planet] = altitude_deg;
        result->overhead[planet] = (altitude_deg > PLANET_OVERHEAD_ALT_DEG);
    }

    return true;
}