#include "maneuver.h"
#include "physics.h"
#include "spacecraft.h"
#include "simulation.h"
#include <cmath>
#include <cstdio>

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

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

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);
            double v_mag = vec3_magnitude(v);

            if (r_mag < 1.0) {
                return false;
            }

            Vec3 r_unit = vec3_scale(r, 1.0 / r_mag);

            Vec3 h = vec3_cross(r, v);
            double h_mag = vec3_magnitude(h);

            if (h_mag < 1e-10) {
                return false;
            }

            Vec3 e_vec = {0.0, 0.0, 0.0};
            double mu = 0.0;

            if (craft->parent_index >= 0 && craft->parent_index < sim->body_count) {
                CelestialBody* parent = &sim->bodies[craft->parent_index];
                mu = G * parent->mass;

                Vec3 v_cross_h = vec3_cross(v, h);
                Vec3 v_cross_h_over_mu = vec3_scale(v_cross_h, 1.0 / mu);
                Vec3 r_over_mag = vec3_scale(r, 1.0 / r_mag);
                e_vec = vec3_sub(v_cross_h_over_mu, r_over_mag);
            } else {
                return false;
            }

            double e_mag = vec3_magnitude(e_vec);

            if (e_mag < 1e-10) {
                Vec3 v_unit = vec3_scale(v, 1.0 / v_mag);
                double cos_nu = vec3_dot(r_unit, v_unit);
                cos_nu = fmax(-1.0, fmin(1.0, cos_nu));
                double nu = acos(cos_nu);

                double target_nu = maneuver->trigger_value;
                while (target_nu < 0) target_nu += 2.0 * M_PI;
                while (target_nu >= 2.0 * M_PI) target_nu -= 2.0 * M_PI;

                double nu_normalized = nu;
                while (nu_normalized < 0) nu_normalized += 2.0 * M_PI;
                while (nu_normalized >= 2.0 * M_PI) nu_normalized -= 2.0 * M_PI;

                double angle_diff = fabs(nu_normalized - target_nu);
                if (angle_diff > M_PI) {
                    angle_diff = 2.0 * M_PI - angle_diff;
                }

                return angle_diff < 0.01;
            }

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

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

            double target_nu = maneuver->trigger_value;
            while (target_nu < 0) target_nu += 2.0 * M_PI;
            while (target_nu >= 2.0 * M_PI) target_nu -= 2.0 * M_PI;

            double nu_normalized = nu;
            while (nu_normalized < 0) nu_normalized += 2.0 * M_PI;
            while (nu_normalized >= 2.0 * M_PI) nu_normalized -= 2.0 * M_PI;

            double angle_diff = fabs(nu_normalized - target_nu);
            if (angle_diff > M_PI) {
                angle_diff = 2.0 * M_PI - angle_diff;
            }

            return angle_diff < 0.01;
        }

        default:
            return false;
    }
}

void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, double current_time) {
    apply_impulsive_burn(craft, maneuver->direction, maneuver->delta_v);
    maneuver->executed = true;
    maneuver->executed_time = current_time;
}