#include "maneuver.h"
#include "physics.h"
#include "spacecraft.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);
}

static double normalize_angle(double angle) {
    while (angle < 0.0) angle += 2.0 * M_PI;
    while (angle >= 2.0 * M_PI) angle -= 2.0 * M_PI;
    return angle;
}

static double angular_distance(double a, double b) {
    double diff = fabs(normalize_angle(a) - normalize_angle(b));
    return (diff > M_PI) ? (2.0 * M_PI - diff) : diff;
}

static bool angle_between(double current, double next, double target) {
    double curr_norm = normalize_angle(current);
    double next_norm = normalize_angle(next);
    double target_norm = normalize_angle(target);

    if (curr_norm <= next_norm) {
        return (target_norm >= curr_norm) && (target_norm <= next_norm);
    } else {
        return (target_norm >= curr_norm) || (target_norm <= next_norm);
    }
}

static double calculate_true_anomaly(Vec3 r, Vec3 v, Vec3 e_vec, double e_mag, double r_mag) {
    // For near-circular orbits, eccentricity vector is near-zero
    // Compute true anomaly as the angle in the orbital plane
    if (e_mag < 1e-10) {
        Vec3 h = vec3_cross(r, v);
        double h_mag = vec3_magnitude(h);
        if (h_mag < 1e-10) return 0.0;

        // Create a coordinate system in the orbital plane
        Vec3 z_hat = vec3_scale(h, 1.0 / h_mag);
        // Choose x-axis as cross product of Z (world up) and orbit normal
        // This gives a consistent reference direction in the orbital plane
        Vec3 world_z = {0.0, 0.0, 1.0};
        Vec3 x_hat = vec3_cross(world_z, z_hat);
        double x_hat_mag = vec3_magnitude(x_hat);
        if (x_hat_mag < 1e-10) {
            // Orbit is equatorial, use world X as reference
            x_hat = (Vec3){1.0, 0.0, 0.0};
        } else {
            x_hat = vec3_scale(x_hat, 1.0 / x_hat_mag);
        }
        Vec3 y_hat = vec3_cross(z_hat, x_hat);

        // Project position onto this orbital plane coordinate system
        double x_proj = vec3_dot(r, x_hat);
        double y_proj = vec3_dot(r, y_hat);

        // True anomaly is the angle in the orbital plane
        double nu = atan2(y_proj, x_proj);
        if (nu < 0) nu += 2.0 * M_PI;
        return nu;
    }

    // Standard calculation using eccentricity vector
    double cos_nu = vec3_dot(e_vec, r) / (e_mag * r_mag);
    cos_nu = fmax(-1.0, fmin(1.0, cos_nu));
    double nu = acos(cos_nu);

    // Determine correct quadrant using cross product
    Vec3 r_cross_v = vec3_cross(r, v);
    double r_cross_v_dot_e = vec3_dot(r_cross_v, e_vec);
    if (r_cross_v_dot_e < 0) {
        nu = 2.0 * M_PI - nu;
    }

    return nu;
}

static Vec3 calculate_eccentricity_vector(Vec3 r, Vec3 v, Vec3 h, double mu) {
    Vec3 v_cross_h = vec3_cross(v, h);
    Vec3 v_cross_h_over_mu = vec3_scale(v_cross_h, 1.0 / mu);
    double r_mag = vec3_magnitude(r);
    Vec3 r_over_mag = vec3_scale(r, 1.0 / r_mag);
    return vec3_sub(v_cross_h_over_mu, r_over_mag);
}

bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim) {
    switch (maneuver->trigger_type) {
        case TRIGGER_TIME:
            return sim->time >= maneuver->trigger_value;

        case TRIGGER_TRUE_ANOMALY: {
            Vec3 r = craft->local_position;
            Vec3 v = craft->local_velocity;
            double r_mag = vec3_magnitude(r);

            // Validate position magnitude (avoid division by zero)
            if (r_mag < 1.0) return false;

            // Calculate angular momentum
            Vec3 h = vec3_cross(r, v);
            double h_mag = vec3_magnitude(h);
            if (h_mag < 1e-10) return false;  // Near-linear trajectory

            // Get parent body for gravitational parameter
            if (craft->parent_index < 0 || craft->parent_index >= sim->body_count) {
                return false;
            }

            CelestialBody* parent = &sim->bodies[craft->parent_index];
            double mu = G * parent->mass;
            Vec3 e_vec = calculate_eccentricity_vector(r, v, h, mu);
            double e_mag = vec3_magnitude(e_vec);
            double target_nu = normalize_angle(maneuver->trigger_value);
            double current_nu = calculate_true_anomaly(r, v, e_vec, e_mag, r_mag);
            double current_nu_norm = normalize_angle(current_nu);
            double current_diff = angular_distance(current_nu_norm, target_nu);
            if (current_diff < 0.01) return true;

            // Propagate orbit forward by one time step to check if we'll cross trigger
            OrbitalElements future_elements = propagate_orbital_elements(craft->orbit, sim->dt, parent->mass);
            Vec3 future_r, future_v;
            orbital_elements_to_cartesian(future_elements, parent->mass, &future_r, &future_v);

            double future_r_mag = vec3_magnitude(future_r);
            if (future_r_mag < 1.0) return false;

            // Calculate future eccentricity vector for true anomaly calculation
            Vec3 future_h = vec3_cross(future_r, future_v);
            Vec3 future_e_vec = calculate_eccentricity_vector(future_r, future_v, future_h, mu);
            double future_e_mag = vec3_magnitude(future_e_vec);

            // Calculate future true anomaly
            double future_nu = calculate_true_anomaly(future_r, future_v, future_e_vec, future_e_mag, future_r_mag);
            double future_nu_norm = normalize_angle(future_nu);

            // Check if target lies between current and future positions
            return angle_between(current_nu_norm, future_nu_norm, target_nu);
        }

        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.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].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* craft, BurnDirection direction, double delta_v, double parent_mass) {
    if (delta_v < 0) {
        return false;
    }

    if (delta_v > 50000.0) {
        return false;
    }

    return true;
}