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

#include "defs.h"
#include "sdp4.h"
#include "sgp4.h"
#include "sun.h"
#include "unsorted.h"

bool is_eclipsed(const double pos[3], const double sol[3], double *depth);
bool predict_decayed(const predict_orbital_elements_t *orbital_elements, predict_julian_date_t time);

// length of buffer used for extracting subsets of TLE strings for parsing
#define SUBSTRING_BUFFER_LENGTH 50

predict_orbital_elements_t *predict_parse_tle(const char *tle_line_1, const char *tle_line_2)
{
    double tempnum;
    predict_orbital_elements_t *m = (predict_orbital_elements_t *)malloc(sizeof(predict_orbital_elements_t));
    if (m == NULL)
        return NULL;

    char substring_buffer[SUBSTRING_BUFFER_LENGTH];
    m->satellite_number = atol(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 2, 6));
    m->element_number = atol(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 64, 67));
    m->epoch_year = atoi(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 18, 19));
    strncpy(m->designator, SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 9, 16), 8);
    m->epoch_day = atof(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 20, 31));
    m->inclination = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 8, 15));
    m->right_ascension = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 17, 24));
    m->eccentricity = 1.0e-07 * atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 26, 32));
    m->argument_of_perigee = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 34, 41));
    m->mean_anomaly = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 43, 50));
    m->mean_motion = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 52, 62));
    m->derivative_mean_motion = atof(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 33, 42));
    tempnum = 1.0e-5 * atof(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 44, 49));
    m->second_derivative_mean_motion = tempnum / pow(10.0, (tle_line_1[51] - '0'));
    tempnum = 1.0e-5 * atof(SubString(tle_line_1, SUBSTRING_BUFFER_LENGTH, substring_buffer, 53, 58));
    m->bstar_drag_term = tempnum / pow(10.0, (tle_line_1[60] - '0'));
    m->revolutions_at_epoch = atof(SubString(tle_line_2, SUBSTRING_BUFFER_LENGTH, substring_buffer, 63, 67));

    /* Period > 225 minutes is deep space */
    double ao, xnodp, dd1, dd2, delo, a1, del1, r1;
    double temp = TWO_PI / MINUTES_PER_DAY / MINUTES_PER_DAY;
    double xno = m->mean_motion * temp * MINUTES_PER_DAY; // from old TLE struct
    dd1 = (XKE / xno);
    dd2 = TWO_THIRD;
    a1 = pow(dd1, dd2);
    r1 = cos(m->inclination * M_PI / 180.0);
    dd1 = (1.0 - m->eccentricity * m->eccentricity);
    temp = CK2 * 1.5f * (r1 * r1 * 3.0 - 1.0) / pow(dd1, 1.5);
    del1 = temp / (a1 * a1);
    ao = a1 * (1.0 - del1 * (TWO_THIRD * .5 + del1 * (del1 * 1.654320987654321 + 1.0)));
    delo = temp / (ao * ao);
    xnodp = xno / (delo + 1.0);

    /* Select a deep-space/near-earth ephemeris */
    if (TWO_PI / xnodp / MINUTES_PER_DAY >= 0.15625)
    {
        m->ephemeris = EPHEMERIS_SDP4;

        // Allocate memory for ephemeris data
        m->ephemeris_data = malloc(sizeof(struct _sdp4));

        if (m->ephemeris_data == NULL)
        {
            predict_destroy_orbital_elements(m);
            return NULL;
        }
        // Initialize ephemeris data structure
        sdp4_init(m, (struct _sdp4 *)m->ephemeris_data);
    }
    else
    {
        m->ephemeris = EPHEMERIS_SGP4;

        // Allocate memory for ephemeris data
        m->ephemeris_data = malloc(sizeof(struct _sgp4));

        if (m->ephemeris_data == NULL)
        {
            predict_destroy_orbital_elements(m);
            return NULL;
        }
        // Initialize ephemeris data structure
        sgp4_init(m, (struct _sgp4 *)m->ephemeris_data);
    }

    return m;
}

char **str_split(char *a_str, const char a_delim)
{
    char **result = 0;
    size_t count = 0;
    char *tmp = a_str;
    char *last_comma = 0;
    char delim[2];
    delim[0] = a_delim;
    delim[1] = 0;

    /* Count how many elements will be extracted. */
    while (*tmp)
    {
        if (a_delim == *tmp)
        {
            count++;
            last_comma = tmp;
        }
        tmp++;
    }

    /* Add space for trailing token. */
    count += last_comma < (a_str + strlen(a_str) - 1);

    /* Add space for terminating null string so caller
       knows where the list of returned strings ends. */
    count++;

    result = (char **)malloc(sizeof(char *) * count);

    if (result)
    {
        size_t idx = 0;
        char *token = strtok(a_str, delim);

        while (token)
        {
            // assert(idx < count);
            *(result + idx++) = strdup(token);
            token = strtok(0, delim);
        }
        // assert(idx == count - 1);
        *(result + idx) = 0;
    }

    return result;
}

predict_orbital_elements_t *predict_parse_omm(const char *omm_str)
{
    predict_orbital_elements_t *m = (predict_orbital_elements_t *)malloc(sizeof(predict_orbital_elements_t));
    if (m == NULL)
        return NULL;

    char **tokens = str_split((char *)omm_str, ',');

    if (tokens)
    {
        int ntokens;
        for (ntokens = 0; *(tokens + ntokens); ntokens++)
            ;

        if (ntokens >= 17)
        {
            struct tm timeS;
            double seconds;
            memset(&timeS, 0, sizeof(struct tm));
            if (sscanf(tokens[2], "%4d-%2d-%2dT%2d:%2d:%lf", &timeS.tm_year, &timeS.tm_mon, &timeS.tm_mday,
                       &timeS.tm_hour, &timeS.tm_min, &seconds) == 6)
            {
                timeS.tm_year -= 1900;
                timeS.tm_mon -= 1;
                double timestamp_full = timegm(&timeS) + seconds;

                struct tm timeS2;
                memset(&timeS2, 0, sizeof(struct tm));
                timeS2.tm_year = timeS.tm_year;
                double timestamp_year = timegm(&timeS2);

                m->epoch_year = timeS.tm_year - 100;
                m->epoch_day = ((timestamp_full - timestamp_year) / (3600. * 24.));
            }

            m->mean_motion = atof(tokens[3]);
            m->eccentricity = atof(tokens[4]);
            m->inclination = atof(tokens[5]);
            m->right_ascension = atof(tokens[6]);
            m->argument_of_perigee = atof(tokens[7]);
            m->mean_anomaly = atof(tokens[8]);

            m->satellite_number = atof(tokens[11]);
            m->element_number = atof(tokens[12]);
            m->revolutions_at_epoch = atof(tokens[13]);

            m->bstar_drag_term = atof(tokens[14]);
            m->derivative_mean_motion = atof(tokens[15]);
            m->second_derivative_mean_motion = atof(tokens[16]);

            memcpy(m->designator, "UNKNOW", 6);
        }

        for (int i = 0; i < ntokens; i++)
            free(tokens[i]);
        free(tokens);
        free(m);
    }

    /* Period > 225 minutes is deep space */
    double ao, xnodp, dd1, dd2, delo, a1, del1, r1;
    double temp = TWO_PI / MINUTES_PER_DAY / MINUTES_PER_DAY;
    double xno = m->mean_motion * temp * MINUTES_PER_DAY; // from old TLE struct
    dd1 = (XKE / xno);
    dd2 = TWO_THIRD;
    a1 = pow(dd1, dd2);
    r1 = cos(m->inclination * M_PI / 180.0);
    dd1 = (1.0 - m->eccentricity * m->eccentricity);
    temp = CK2 * 1.5f * (r1 * r1 * 3.0 - 1.0) / pow(dd1, 1.5);
    del1 = temp / (a1 * a1);
    ao = a1 * (1.0 - del1 * (TWO_THIRD * .5 + del1 * (del1 * 1.654320987654321 + 1.0)));
    delo = temp / (ao * ao);
    xnodp = xno / (delo + 1.0);

    /* Select a deep-space/near-earth ephemeris */
    if (TWO_PI / xnodp / MINUTES_PER_DAY >= 0.15625)
    {
        m->ephemeris = EPHEMERIS_SDP4;

        // Allocate memory for ephemeris data
        m->ephemeris_data = malloc(sizeof(struct _sdp4));

        if (m->ephemeris_data == NULL)
        {
            predict_destroy_orbital_elements(m);
            return NULL;
        }
        // Initialize ephemeris data structure
        sdp4_init(m, (struct _sdp4 *)m->ephemeris_data);
    }
    else
    {
        m->ephemeris = EPHEMERIS_SGP4;

        // Allocate memory for ephemeris data
        m->ephemeris_data = malloc(sizeof(struct _sgp4));

        if (m->ephemeris_data == NULL)
        {
            predict_destroy_orbital_elements(m);
            return NULL;
        }
        // Initialize ephemeris data structure
        sgp4_init(m, (struct _sgp4 *)m->ephemeris_data);
    }

    return m;
}

void predict_destroy_orbital_elements(predict_orbital_elements_t *m)
{
    if (m == NULL)
        return;

    if (m->ephemeris_data != NULL)
    {
        free(m->ephemeris_data);
    }

    free(m);
}

bool predict_is_geosynchronous(const predict_orbital_elements_t *m)
{
    return (m->mean_motion >= GEOSYNCHRONOUS_LOWER_MEAN_MOTION) &&
           (m->mean_motion <= GEOSYNCHRONOUS_UPPER_MEAN_MOTION) &&
           (fabs(m->eccentricity) <= GEOSYNCHRONOUS_ECCENTRICITY_THRESHOLD) &&
           (fabs(m->inclination) <= GEOSYNCHRONOUS_INCLINATION_THRESHOLD_DEGREES);
}

double predict_apogee(const predict_orbital_elements_t *m)
{
    double sma = 331.25 * exp(log(1440.0 / m->mean_motion) * (2.0 / 3.0));
    return sma * (1.0 + m->eccentricity) - EARTH_RADIUS_KM_WGS84;
}

double predict_perigee(const predict_orbital_elements_t *m)
{
    double xno = m->mean_motion * TWO_PI / MINUTES_PER_DAY;
    double a1 = pow(XKE / xno, TWO_THIRD);
    double cosio = cos(m->inclination * M_PI / 180.0);
    double theta2 = cosio * cosio;
    double x3thm1 = 3 * theta2 - 1.0;
    double eosq = m->eccentricity * m->eccentricity;
    double betao2 = 1.0 - eosq;
    double betao = sqrt(betao2);
    double del1 = 1.5 * CK2 * x3thm1 / (a1 * a1 * betao * betao2);
    double ao = a1 * (1.0 - del1 * (0.5 * TWO_THIRD + del1 * (1.0 + 134.0 / 81.0 * del1)));
    double delo = 1.5 * CK2 * x3thm1 / (ao * ao * betao * betao2);
    double aodp = ao / (1.0 - delo);

    return (aodp * (1 - m->eccentricity) - AE) * EARTH_RADIUS_KM_WGS84;
}

bool predict_aos_happens(const predict_orbital_elements_t *m, double latitude)
{
    /* This function returns true if the satellite pointed to by
       "x" can ever rise above the horizon of the ground station. */

    double lin, apogee;

    if (m->mean_motion == 0.0)
        return false;
    else
    {
        lin = m->inclination;

        if (lin >= 90.0)
            lin = 180.0 - lin;

        apogee = predict_apogee(m);

        if ((acos(EARTH_RADIUS_KM_WGS84 / (apogee + EARTH_RADIUS_KM_WGS84)) + (lin * M_PI / 180.0)) > fabs(latitude))
            return true;
        else
            return false;
    }
}

/* This is the stuff we need to do repetitively while tracking. */
/* This is the old Calc() function. */
int predict_orbit(const predict_orbital_elements_t *orbital_elements, struct predict_position *m, double utc)
{
    /* Set time to now if now time is provided: */
    if (utc == 0)
        utc = predict_to_julian(time(NULL));

    /* Satellite position and velocity vectors */
    vec3_set(m->position, 0, 0, 0);
    vec3_set(m->velocity, 0, 0, 0);

    m->time = utc;
    double julTime = utc + JULIAN_TIME_DIFF;

    /* Convert satellite's epoch time to Julian  */
    /* and calculate time since epoch in minutes */
    double epoch = 1000.0 * orbital_elements->epoch_year + orbital_elements->epoch_day;
    double jul_epoch = Julian_Date_of_Epoch(epoch);
    double tsince = (julTime - jul_epoch) * MINUTES_PER_DAY;

    /* Call NORAD routines according to deep-space flag. */
    struct model_output output;
    switch (orbital_elements->ephemeris)
    {
    case EPHEMERIS_SDP4:
        sdp4_predict((struct _sdp4 *)orbital_elements->ephemeris_data, tsince, &output);
        break;
    case EPHEMERIS_SGP4:
        sgp4_predict((struct _sgp4 *)orbital_elements->ephemeris_data, tsince, &output);
        break;
    default:
        // Panic!
        return -1;
    }
    m->position[0] = output.pos[0];
    m->position[1] = output.pos[1];
    m->position[2] = output.pos[2];
    m->velocity[0] = output.vel[0];
    m->velocity[1] = output.vel[1];
    m->velocity[2] = output.vel[2];
    m->phase = output.phase;
    m->argument_of_perigee = output.omgadf;
    m->inclination = output.xinck;
    m->right_ascension = output.xnodek;

    /* TODO: Remove? Scale position and velocity vectors to km and km/sec */
    Convert_Sat_State(m->position, m->velocity);

    /* Calculate satellite Lat North, Lon East and Alt. */
    geodetic_t sat_geodetic;
    Calculate_LatLonAlt(utc, m->position, &sat_geodetic);

    m->latitude = sat_geodetic.lat;
    m->longitude = sat_geodetic.lon;
    m->altitude = sat_geodetic.alt;

    // Calculate solar position
    double solar_vector[3];
    sun_predict(m->time, solar_vector);

    // Find eclipse depth and if sat is eclipsed
    m->eclipsed = is_eclipsed(m->position, solar_vector, &m->eclipse_depth);

    // Calculate footprint
    m->footprint = 2.0 * EARTH_RADIUS_KM_WGS84 * acos(EARTH_RADIUS_KM_WGS84 / (EARTH_RADIUS_KM_WGS84 + m->altitude));

    // Calculate current number of revolutions around Earth
    double temp = TWO_PI / MINUTES_PER_DAY / MINUTES_PER_DAY;
    double age = julTime - jul_epoch;
    double xno = orbital_elements->mean_motion * temp * MINUTES_PER_DAY;
    double xmo = orbital_elements->mean_anomaly * M_PI / 180.0;
    m->revolutions =
        (long)floor((xno * MINUTES_PER_DAY / (M_PI * 2.0) + age * orbital_elements->bstar_drag_term) * age +
                    xmo / (2.0 * M_PI)) +
        orbital_elements->revolutions_at_epoch;

    // calculate whether orbit is decayed
    m->decayed = predict_decayed(orbital_elements, utc);

    return 0;
}

bool predict_decayed(const predict_orbital_elements_t *orbital_elements, predict_julian_date_t time)
{
    double satepoch;
    satepoch = DayNum(1, 0, orbital_elements->epoch_year) + orbital_elements->epoch_day;

    bool has_decayed = false;
    if (satepoch +
            ((16.666666 - orbital_elements->mean_motion) / (10.0 * fabs(orbital_elements->derivative_mean_motion))) <
        time)
    {
        has_decayed = true;
    }
    return has_decayed;
}

/* Calculates if a position is eclipsed.  */
bool is_eclipsed(const double pos[3], const double sol[3], double *depth)
{
    double Rho[3], earth[3];

    /* Determine partial eclipse */
    double sd_earth = asin_(EARTH_RADIUS_KM_WGS84 / vec3_length(pos));
    vec3_sub(sol, pos, Rho);
    double sd_sun = asin_(SOLAR_RADIUS_KM / vec3_length(Rho));
    vec3_mul_scalar(pos, -1, earth);

    double delta = acos_(vec3_dot(sol, earth) / vec3_length(sol) / vec3_length(earth));
    *depth = sd_earth - sd_sun - delta;

    if (sd_earth < sd_sun)
        return false;
    else if (*depth >= 0)
        return true;
    else
        return false;
}

double predict_squint_angle(const predict_observer_t *observer, const struct predict_position *orbit, double alon,
                            double alat)
{
    double bx = cos(alat) * cos(alon + orbit->argument_of_perigee);
    double by = cos(alat) * sin(alon + orbit->argument_of_perigee);
    double bz = sin(alat);

    double cx = bx;
    double cy = by * cos(orbit->inclination) - bz * sin(orbit->inclination);
    double cz = by * sin(orbit->inclination) + bz * cos(orbit->inclination);
    double ax = cx * cos(orbit->right_ascension) - cy * sin(orbit->right_ascension);
    double ay = cx * sin(orbit->right_ascension) + cy * cos(orbit->right_ascension);
    double az = cz;

    struct predict_observation obs;
    predict_observe_orbit(observer, orbit, &obs);
    double squint = acos(-(ax * obs.range_x + ay * obs.range_y + az * obs.range_z) / obs.range);

    return squint;
}
