#include "rendezvous.h"
#include <math.h>
#include <float.h>

// Mean motion: n = sqrt(mu / a^3)
static double calc_mean_motion(double radius, double mass) {
    double mu = G * mass;
    return sqrt(mu / pow(radius, 3));
}

// Hohmann transfer time (half orbit of transfer ellipse)
static double hohmann_transfer_time(double r1, double r2, double mass) {
    double mu = G * mass;
    double a_transfer = (r1 + r2) / 2.0;
    double T_transfer = 2.0 * M_PI * sqrt(pow(a_transfer, 3) / mu);
    return T_transfer / 2.0;
}

// Calculate required angular separation at first burn
// For Hohmann transfer: target should be at specific angle when chaser burns
// Returns: required angular separation (chaser - target) in radians
// Negative value means chaser should be behind target, positive means ahead
static double required_separation(double r1, double r2, double mass) {
    double transfer_time = hohmann_transfer_time(r1, r2, mass);
    double n2 = calc_mean_motion(r2, mass);
    double target_angle = n2 * transfer_time;
    // Chaser travels π radians in transfer orbit, target travels target_angle
    // For rendezvous: chaser_pos + π = target_pos + target_angle
    // Therefore: chaser_pos - target_pos = target_angle - π
    // Negative value means chaser should be behind target
    return target_angle - M_PI;
}



// Normalize angle to [0, 2π)
static double normalize_angle_2pi(double angle) {
    while (angle < 0.0) {
        angle += 2.0 * M_PI;
    }
    while (angle >= 2.0 * M_PI) {
        angle -= 2.0 * M_PI;
    }
    return angle;
}

// Normalize angle to [-π, π] for shortest path
static double normalize_angle_pi(double angle) {
    angle = normalize_angle_2pi(angle);
    while (angle > M_PI) {
        angle -= 2.0 * M_PI;
    }
    while (angle < -M_PI) {
        angle += 2.0 * M_PI;
    }
    return angle;
}

// Calculate wait time before starting Hohmann transfer
// Determines how long to wait before executing the first burn so that
// both chaser and target arrive at the interception point simultaneously.
// Returns: wait time in seconds. Positive = wait, negative = transfer already late
double calculate_wait_time_for_hohmann(
    double initial_orbit_radius,
    double target_orbit_radius,
    double angular_separation,
    double central_mass
) {
    double required_sep = required_separation(initial_orbit_radius, target_orbit_radius, central_mass);
    double n1 = calc_mean_motion(initial_orbit_radius, central_mass);
    double n2 = calc_mean_motion(target_orbit_radius, central_mass);
    double rel_angular_vel = n1 - n2;

    // Normalize current separation to [-pi, pi]
    // Positive = chaser ahead of target, negative = chaser behind target
    double current_sep = normalize_angle_pi(angular_separation);

    // Normalize required separation to [-pi, pi]
    required_sep = normalize_angle_pi(required_sep);

    // Angle to close: difference between required and current separation
    // If current_sep > required_sep, chaser is too far ahead (negative wait time)
    // If current_sep < required_sep, chaser is too far behind (positive wait time)
    double angle_to_close = required_sep - current_sep;

    // Wait time = angle_to_close / relative_angular_velocity
    return angle_to_close / rel_angular_vel;
}

// Calculate required angular separation for Hohmann transfer
// Computes the ideal angle between chaser and target at the moment
// of first burn to ensure simultaneous arrival at target orbit.
// Returns: required angular separation in radians (-2π, 2π)
double calculate_required_separation_for_hohmann(
    double initial_orbit_radius,
    double target_orbit_radius,
    double central_mass
) {
    double required_sep = required_separation(initial_orbit_radius, target_orbit_radius, central_mass);
    return normalize_angle_pi(required_sep);
}

// Verify spacecraft is on correct Hohmann transfer orbit
// Checks if current orbit matches expected Hohmann transfer parameters.
// Returns: true if orbit is on Hohmann transfer, false otherwise
bool verify_hohmann_transfer_orbit(
    const OrbitalElements* orbit,
    double r1,
    double r2,
    double tolerance
) {
    double expected_a = (r1 + r2) / 2.0;
    double actual_a = orbit->semi_major_axis;
    double diff = fabs(actual_a - expected_a);
    return diff < tolerance;
}

// Check if Hohmann transfer is complete
// Determines if transfer time has elapsed and spacecraft is at target radius.
// Returns: true if transfer is complete, false otherwise
bool hohmann_transfer_complete(
    double transfer_start_time,
    double current_time,
    double transfer_time,
    double target_radius,
    double current_radius,
    double tolerance
) {
    // Check if enough time has elapsed
    if (current_time < transfer_start_time + transfer_time - tolerance) {
        return false;
    }

    // Check if at target radius
    double radius_diff = fabs(current_radius - target_radius);
    return radius_diff < tolerance;
}

// Validate parameters for Hohmann transfer
// Checks if the transfer parameters are valid before calculation.
// Returns: true if parameters are valid, false otherwise
bool validate_hohmann_transfer_parameters(
    double initial_orbit_radius,
    double target_orbit_radius,
    double central_mass
) {
    // Check for positive radii
    if (initial_orbit_radius <= 0.0 || target_orbit_radius <= 0.0) {
        return false;
    }

    // Check for positive mass
    if (central_mass <= 0.0) {
        return false;
    }

    // Check for different orbits (no relative motion if equal)
    if (initial_orbit_radius == target_orbit_radius) {
        return false;
    }

    return true;
}

// Calculate relative orbit period
// Computes the time for the chaser to complete one full relative orbit
// with respect to the target (time between consecutive phasing opportunities).
// Returns: relative orbit period in seconds
double calculate_relative_orbit_period(
    double initial_orbit_radius,
    double target_orbit_radius,
    double central_mass
) {
    double n1 = calc_mean_motion(initial_orbit_radius, central_mass);
    double n2 = calc_mean_motion(target_orbit_radius, central_mass);
    double rel_angular_vel = fabs(n1 - n2);

    return 2.0 * M_PI / rel_angular_vel;
}

// Calculate next valid wait time for Hohmann transfer
// Like calculate_wait_time_for_hohmann(), but always returns a non-negative
// value by advancing to the next phasing opportunity if needed.
// Returns: non-negative wait time in seconds (time to wait before executing transfer)
double calculate_next_hohmann_wait_time(
    double initial_orbit_radius,
    double target_orbit_radius,
    double angular_separation,
    double central_mass,
    double min_wait_time
) {
    double wait_time = calculate_wait_time_for_hohmann(
        initial_orbit_radius, target_orbit_radius, angular_separation, central_mass
    );

    double rel_period = calculate_relative_orbit_period(
        initial_orbit_radius, target_orbit_radius, central_mass
    );

    // Add relative orbit periods until wait_time >= min_wait_time
    while (wait_time < min_wait_time) {
        wait_time += rel_period;
    }

    return wait_time;
}