#include "maneuver.h"
#include "physics.h"
#include "orbital_objects.h"
#include "simulation.h"
#include "orbital_mechanics.h"
#include <cmath>
#include <cstdio>
#include <cstring>

Vec3 calculate_prograde_dir(Vec3 local_velocity) {
    return vec3_normalize(local_velocity);
}

Vec3 calculate_retrograde_dir(Vec3 local_velocity) {
    Vec3 prograde = calculate_prograde_dir(local_velocity);
    return vec3_scale(prograde, -1.0);
}

Vec3 calculate_normal_dir(Vec3 local_position, Vec3 local_velocity) {
    Vec3 angular_momentum = vec3_cross(local_position, local_velocity);
    return vec3_normalize(angular_momentum);
}

Vec3 calculate_antinormal_dir(Vec3 local_position, Vec3 local_velocity) {
    Vec3 normal = calculate_normal_dir(local_position, local_velocity);
    return vec3_scale(normal, -1.0);
}

Vec3 calculate_radial_in_dir(Vec3 local_position) {
    Vec3 radial = vec3_normalize(local_position);
    return vec3_scale(radial, -1.0);
}

Vec3 calculate_radial_out_dir(Vec3 local_position) {
    return vec3_normalize(local_position);
}

Vec3 get_burn_direction_vector(BurnDirection direction, Vec3 local_pos, Vec3 local_vel) {
    switch (direction) {
        case BURN_PROGRADE:
            return calculate_prograde_dir(local_vel);
        case BURN_RETROGRADE:
            return calculate_retrograde_dir(local_vel);
        case BURN_NORMAL:
            return calculate_normal_dir(local_pos, local_vel);
        case BURN_ANTINORMAL:
            return calculate_antinormal_dir(local_pos, local_vel);
        case BURN_RADIAL_IN:
            return calculate_radial_in_dir(local_pos);
        case BURN_RADIAL_OUT:
            return calculate_radial_out_dir(local_pos);
        case BURN_CUSTOM:
        default:
            return {0.0, 0.0, 0.0};
    }
}

const char* get_burn_direction_name(BurnDirection direction) {
    switch (direction) {
        case BURN_PROGRADE:
            return "Prograde";
        case BURN_RETROGRADE:
            return "Retrograde";
        case BURN_NORMAL:
            return "Normal";
        case BURN_ANTINORMAL:
            return "Antinormal";
        case BURN_RADIAL_IN:
            return "Radial In";
        case BURN_RADIAL_OUT:
            return "Radial Out";
        case BURN_CUSTOM:
            return "Custom";
        default:
            return "Unknown";
    }
}

void apply_impulsive_burn(Spacecraft* craft, BurnDirection direction, double delta_v) {
    Vec3 dir = get_burn_direction_vector(direction, craft->local_position, craft->local_velocity);

    Vec3 delta_v_vec = vec3_scale(dir, delta_v);

    craft->local_velocity = vec3_add(craft->local_velocity, delta_v_vec);
}

void apply_custom_burn(Spacecraft* craft, Vec3 delta_v_local) {
    craft->local_velocity = vec3_add(craft->local_velocity, delta_v_local);
    craft->global_velocity = vec3_add(craft->global_velocity, delta_v_local);
}

OrbitalElements preview_burn_result(const Spacecraft* craft, BurnDirection direction, double delta_v, const SimulationState* sim) {
    OrbitalElements current_elements = craft->orbit;

    if (craft->parent_index < 0 || craft->parent_index >= sim->body_count) {
        return current_elements;
    }

    CelestialBody* parent = &sim->bodies[craft->parent_index];
    double parent_mass = parent->mass;

    Vec3 pos;
    Vec3 vel;
    orbital_elements_to_cartesian(current_elements, parent_mass, &pos, &vel);

    Vec3 burn_dir = get_burn_direction_vector(direction, pos, vel);
    Vec3 delta_v_vec = vec3_scale(burn_dir, delta_v);
    Vec3 new_vel = vec3_add(vel, delta_v_vec);

    return cartesian_to_orbital_elements(pos, new_vel, parent_mass);
}

// Elliptical orbits only: uses analytical mean anomaly delta to compute
// exact time to target true anomaly, eliminating per-frame propagation.
// TODO: add parabolic (Barker's equation) and hyperbolic branches.
bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim) {
    switch (maneuver->trigger_type) {
        case TRIGGER_TIME: {
            // Fire at the step that contains the trigger time.
            // The orbit state is at sim->time (start of current step).
            // We propagate forward to trigger_value, burn, then propagate
            // the remaining time to reach sim->time + sim->dt.
            if (sim->time > maneuver->trigger_value) {
                // Trigger is before the start of this step — clamp to 0
                // (should have fired in an earlier step; fire immediately)
                maneuver->scheduled_dt = 0.0;
                return true;
            }
            if (sim->time + sim->dt <= maneuver->trigger_value) {
                return false;
            }

            double dt_to_burn = maneuver->trigger_value - sim->time;

            // Clamp to valid range [0, sim->dt]
            if (dt_to_burn < 0.0) {
                dt_to_burn = 0.0;
            }
            if (dt_to_burn > sim->dt) {
                dt_to_burn = sim->dt;
            }

            maneuver->scheduled_dt = dt_to_burn;
            return true;
        }

        case TRIGGER_TRUE_ANOMALY: {
            if (craft->parent_index < 0 || craft->parent_index >= sim->body_count) {
                return false;
            }

            CelestialBody* parent = &sim->bodies[craft->parent_index];
            double current_nu = normalize_angle(craft->orbit.true_anomaly);
            double target_nu = normalize_angle(maneuver->trigger_value);
            double current_diff = angular_distance(current_nu, target_nu);

            if (current_diff < 0.01) {
                maneuver->scheduled_dt = 0.0;
                return true;
            }

            double a = craft->orbit.semi_major_axis;
            double e = craft->orbit.eccentricity;
            double mu = G * parent->mass;
            double n = sqrt(mu / (a * a * a));

            double E_current = true_anomaly_to_eccentric_anomaly(current_nu, e);
            double E_target = true_anomaly_to_eccentric_anomaly(target_nu, e);

            double M_current = E_current - e * sin(E_current);
            double M_target = E_target - e * sin(E_target);

            double M_delta = M_target - M_current;
            double dt_needed = M_delta / n;

            if (dt_needed < 0) {
                double M_period = 2.0 * M_PI;
                dt_needed += M_period / n;
            }

            if (dt_needed <= 0.0 || dt_needed > sim->dt) {
                return false;
            }

            maneuver->scheduled_dt = dt_needed;
            return true;
        }

        default:
            return false;
    }
}

Maneuver create_maneuver(const char* name, int craft_index, BurnDirection direction, double delta_v, TriggerType trigger_type, double trigger_value) {
    Maneuver m;
    strncpy(m.name, name, 63);
    m.name[63] = '\0';
    m.craft_index = craft_index;
    m.direction = direction;
    m.delta_v = delta_v;
    m.trigger_type = trigger_type;
    m.trigger_value = trigger_value;
    m.scheduled_dt = 0.0;
    m.executed = false;
    m.executed_time = 0.0;
    return m;
}

void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim, double current_time) {
    apply_impulsive_burn(craft, maneuver->direction, maneuver->delta_v);

    if (craft->parent_index >= 0 && craft->parent_index < sim->body_count) {
        CelestialBody* parent = &sim->bodies[craft->parent_index];
        craft->orbit = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, parent->mass);
    }

    maneuver->executed = true;
    maneuver->executed_time = current_time;
}

int add_maneuver_to_simulation(SimulationState* sim, Maneuver* maneuver) {
    if (sim->maneuver_count >= sim->max_maneuvers) {
        return -1;
    }

    for (int i = 0; i < sim->maneuver_count; i++) {
        if (strcmp(sim->maneuvers[i].name, maneuver->name) == 0) {
            return -1;
        }
    }

    if (maneuver->craft_index < 0 || maneuver->craft_index >= sim->craft_count) {
        return -1;
    }

    int new_idx = sim->maneuver_count;
    sim->maneuvers[new_idx] = *maneuver;
    sim->maneuvers[new_idx].scheduled_dt = 0.0;
    sim->maneuvers[new_idx].executed = false;
    sim->maneuvers[new_idx].executed_time = 0.0;
    sim->maneuver_count++;

    return new_idx;
}

bool remove_maneuver_by_index(SimulationState* sim, int index) {
    if (index < 0 || index >= sim->maneuver_count) {
        return false;
    }

    int elements_to_move = sim->maneuver_count - index - 1;
    if (elements_to_move > 0) {
        memmove(&sim->maneuvers[index], &sim->maneuvers[index + 1],
                elements_to_move * sizeof(Maneuver));
    }

    sim->maneuver_count--;
    return true;
}

HohmannTransfer calculate_hohmann_transfer(double r1, double r2, double central_mass) {
    HohmannTransfer result;
    double a_transfer = (r1 + r2) / 2.0;
    double mu = G * central_mass;

    double v1 = sqrt(mu / r1);
    double v_transfer1 = sqrt(mu * (2.0 / r1 - 1.0 / a_transfer));
    result.dv1 = v_transfer1 - v1;

    double v2 = sqrt(mu / r2);
    double v_transfer2 = sqrt(mu * (2.0 / r2 - 1.0 / a_transfer));
    result.dv2 = v2 - v_transfer2;

    result.transfer_time = M_PI * sqrt(pow(a_transfer, 3.0) / mu);
    result.true_anomaly_2 = M_PI;

    return result;
}

bool validate_burn_parameters(const Spacecraft*, BurnDirection, double delta_v, double) {
    if (delta_v < 0) {
        return false;
    }

    if (delta_v > 50000.0) {
        return false;
    }

    return true;
}