#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 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);
}

void initialize_spacecraft_leo(CelestialBody* spacecraft, CelestialBody* parent,
                              double altitude_m) {

    double orbital_radius = parent->radius + altitude_m;

    Vec3 sun_to_earth = vec3_sub(parent->position,
                                  (Vec3){0.0, 0.0, 0.0});
    Vec3 direction = vec3_normalize(sun_to_earth);

    Vec3 offset = vec3_scale(direction, orbital_radius);
    spacecraft->position = vec3_add(parent->position, offset);

    spacecraft->local_position = offset;

    double v_leo = sqrt(G * parent->mass / orbital_radius);

    Vec3 leo_tangent = (Vec3){direction.y, -direction.x, 0.0};
    Vec3 leo_velocity = vec3_scale(leo_tangent, v_leo);

    spacecraft->velocity = vec3_add(parent->velocity, leo_velocity);
    spacecraft->local_velocity = leo_velocity;

    spacecraft->semi_major_axis = orbital_radius;

    printf("Spacecraft LEO initialized:\n");
    printf("  Altitude: %.2f km\n", altitude_m / 1000.0);
    printf("  Orbital radius: %.2e m\n", orbital_radius);
    printf("  LEO velocity: %.2f m/s\n", v_leo);
    printf("  Parent: %s\n", parent->name);
}

// DEPRECATED: This function is no longer needed. Spacecraft positions and velocities
// are now set via TOML config files with semi_major_axis parameter. Use config-based
// initialization instead. This function is kept for reference only and will be
// removed in a future cleanup.

void apply_transfer_burn(SimulationState* sim, int spacecraft_idx,
                       int departure_idx, TransferParameters* params) {
    CelestialBody* spacecraft = &sim->bodies[spacecraft_idx];
    CelestialBody* departure = &sim->bodies[departure_idx];
    CelestialBody* sun = &sim->bodies[0];

    Vec3 sun_to_departure = vec3_sub(departure->position, sun->position);
    Vec3 sun_to_departure_norm = vec3_normalize(sun_to_departure);

    Vec3 transfer_dir = (Vec3){-sun_to_departure_norm.y, sun_to_departure_norm.x, 0.0};
    Vec3 v_transfer_helio = vec3_scale(transfer_dir, params->departure_velocity);

    Vec3 old_helio = spacecraft->velocity;
    Vec3 old_local = spacecraft->local_velocity;

    Vec3 v_transfer_local = vec3_sub(v_transfer_helio, departure->velocity);

    spacecraft->local_velocity = v_transfer_local;
    spacecraft->velocity = vec3_add(departure->velocity, spacecraft->local_velocity);

    Vec3 delta_v_local = vec3_sub(spacecraft->local_velocity, old_local);
    Vec3 delta_v_helio = vec3_sub(spacecraft->velocity, old_helio);

    printf("Transfer burn applied:\n");
    printf("  Current heliocentric velocity: (%.2f, %.2f, %.2f) m/s\n",
           old_helio.x, old_helio.y, old_helio.z);
    printf("  Target heliocentric velocity: (%.2f, %.2f, %.2f) m/s\n",
           v_transfer_helio.x, v_transfer_helio.y, v_transfer_helio.z);
    printf("  Delta-v (local): (%.2f, %.2f, %.2f) m/s\n",
           delta_v_local.x, delta_v_local.y, delta_v_local.z);
    printf("  Delta-v magnitude: %.2f m/s (%.3f km/s)\n",
           vec3_magnitude(delta_v_helio), vec3_magnitude(delta_v_helio) / 1000.0);
}

double calculate_phase_angle(SimulationState* sim, int departure_idx, int arrival_idx) {
    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 phase_rad = theta_arrival - theta_depart;

    while (phase_rad < 0.0) {
        phase_rad += 2.0 * M_PI;
    }
    while (phase_rad >= 2.0 * M_PI) {
        phase_rad -= 2.0 * M_PI;
    }

    return phase_rad * 180.0 / M_PI;
}