#include "mission_planning.h"
#include <cstdio>
#include <cmath>

TransferParameters calculate_hohmann_transfer(double r_departure, double r_arrival,
                                             double central_mass) {
    TransferParameters params;

    params.periapsis = r_departure;
    params.apoapsis = r_arrival;
    params.semi_major_axis = (r_departure + r_arrival) / 2.0;
    params.eccentricity = (r_arrival - r_departure) / (r_arrival + r_departure);

    params.transfer_time = M_PI * sqrt(pow(params.semi_major_axis, 3) / (G * central_mass));

    params.departure_velocity = sqrt(G * central_mass * (2.0/r_departure - 1.0/params.semi_major_axis));
    params.arrival_velocity = sqrt(G * central_mass * (2.0/r_arrival - 1.0/params.semi_major_axis));

    double circular_velocity = sqrt(G * central_mass / r_departure);
    params.delta_v_injection = params.departure_velocity - circular_velocity;

    params.delta_v_capture = 0.0;

    double departure_period = 2.0 * M_PI * sqrt(pow(r_departure, 3) / (G * central_mass));
    double arrival_period = 2.0 * M_PI * sqrt(pow(r_arrival, 3) / (G * central_mass));
    params.phase_angle_deg = calculate_required_phase_angle(params.transfer_time, arrival_period);

    return params;
}

double calculate_angular_position(CelestialBody* body, CelestialBody* center) {
    Vec3 rel_pos = vec3_sub(body->position, center->position);

    double angle = atan2(rel_pos.y, rel_pos.x);

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

    return angle;
}

double calculate_required_phase_angle(double transfer_time, double arrival_period) {
    double omega_arrival = 2.0 * M_PI / arrival_period;

    double alpha_arrival = omega_arrival * transfer_time;

    double phase_angle_rad = M_PI - alpha_arrival;

    double phase_angle_deg = phase_angle_rad * 180.0 / M_PI;

    while (phase_angle_deg < 0.0) {
        phase_angle_deg += 360.0;
    }
    while (phase_angle_deg >= 360.0) {
        phase_angle_deg -= 360.0;
    }

    return phase_angle_deg;
}

bool check_launch_window(SimulationState* sim, int departure_idx, int arrival_idx,
                         double required_phase_angle_deg, double tolerance_deg) {
    if (departure_idx < 0 || departure_idx >= sim->body_count) {
        return false;
    }
    if (arrival_idx < 0 || arrival_idx >= sim->body_count) {
        return false;
    }

    CelestialBody* departure = &sim->bodies[departure_idx];
    CelestialBody* arrival = &sim->bodies[arrival_idx];
    CelestialBody* sun = &sim->bodies[0];

    double theta_depart = calculate_angular_position(departure, sun);
    double theta_arrival = calculate_angular_position(arrival, sun);

    double current_phase_rad = theta_arrival - theta_depart;
    if (current_phase_rad < 0.0) {
        current_phase_rad += 2.0 * M_PI;
    }

    double current_phase_deg = current_phase_rad * 180.0 / M_PI;

    double error = fabs(current_phase_deg - required_phase_angle_deg);
    if (error > 180.0) {
        error = fabs(error - 360.0);
    }

    return error <= tolerance_deg;
}

void wait_for_launch_window(SimulationState* sim, int departure_idx, int arrival_idx,
                           double required_phase_angle_deg, double tolerance_deg) {
    const double TIME_STEP = 60.0;
    const int STEPS_PER_DAY = (int)(86400.0 / TIME_STEP);

    while (!check_launch_window(sim, departure_idx, arrival_idx,
                              required_phase_angle_deg, tolerance_deg)) {
        for (int i = 0; i < STEPS_PER_DAY; i++) {
            update_simulation(sim);
        }
    }

    printf("Launch window opened at t = %.2f days\n", sim->time / 86400.0);
}

int spawn_spacecraft_on_transfer(SimulationState* sim, int departure_idx,
                                 TransferParameters* params) {
    if (departure_idx < 0 || departure_idx >= sim->body_count) {
        return -1;
    }

    CelestialBody* departure = &sim->bodies[departure_idx];
    CelestialBody* sun = &sim->bodies[0];

    CelestialBody spacecraft;
    spacecraft.name[0] = 'S';
    spacecraft.name[1] = 'p';
    spacecraft.name[2] = 'a';
    spacecraft.name[3] = 'c';
    spacecraft.name[4] = 'e';
    spacecraft.name[5] = 'c';
    spacecraft.name[6] = 'r';
    spacecraft.name[7] = 'a';
    spacecraft.name[8] = 'f';
    spacecraft.name[9] = 't';
    spacecraft.name[10] = '\0';
    spacecraft.mass = 1.0;
    spacecraft.radius = 1.0e3;
    spacecraft.eccentricity = params->eccentricity;
    spacecraft.semi_major_axis = params->semi_major_axis;
    spacecraft.color[0] = 1.0f;
    spacecraft.color[1] = 0.0f;
    spacecraft.color[2] = 0.5f;

    spacecraft.position = departure->position;

    Vec3 orbit_dir = vec3_normalize(departure->velocity);
    Vec3 delta_v = vec3_scale(orbit_dir, params->delta_v_injection);
    spacecraft.velocity = vec3_add(departure->velocity, delta_v);

    spacecraft.parent_index = 0;

    return add_body_to_simulation(sim, &spacecraft);
}