#ifndef RENDEZVOUS_H
#define RENDEZVOUS_H

#include "simulation.h"
#include "spacecraft.h"

/**
 * Rendezvous Module
 *
 * Provides Clohessy-Wiltshire (Hill's) equations-based guidance for spacecraft rendezvous.
 * Supports both spacecraft-to-spacecraft and spacecraft-to-body rendezvous in circular,
 * coplanar orbits.
 *
 * Validity Limits (dimensionless, scale with orbital radius):
 * - Spatial: 5% of orbital radius (x/r, y/r, z/r < 0.05)
 * - Time: 2.0 radians of orbital motion (n*dt < 2.0, ~1/3 orbit)
 */

// CW validity thresholds (dimensionless)
#define CW_SPATIAL_LIMIT_FRACTION  0.05  // 5% of orbital radius
#define CW_TIME_LIMIT_N_DT         2.0   // ~2 radians of orbital motion

// Relative state in LVLH frame
typedef struct {
    double radial;       // x: radial separation (positive outward)
    double along_track;  // y: along-track separation (positive in direction of motion)
    double cross_track;  // z: cross-track separation
    double v_radial;     // radial velocity
    double v_along_track;// along-track velocity
    double v_cross_track;// cross-track velocity
} LVLHRelativeState;

// CW validity result
typedef struct {
    bool spatial_valid;      // Within spatial limits
    bool time_valid;         // Within time limits
    bool overall_valid;      // Both spatial and time valid
    double spatial_fraction; // max(|x|,|y|,|z|) / orbital_radius
    double n_dt;             // n * time_since_linearization
    double expected_error;   // Estimated error percentage
} CWValidityResult;

// CW guidance solution
typedef struct {
    bool valid;                        // Whether solution is valid
    double delta_v_magnitude;          // Required delta-v (m/s)
    double burn_direction_radial;      // Radial component (unit vector)
    double burn_direction_along_track; // Along-track component
    double burn_direction_cross_track; // Cross-track component
    double time_to_intercept;          // Time to reach target (s)
} CWGuidanceSolution;

// ============================================================================
// Utility Functions
// ============================================================================

/**
 * Transform Cartesian position/velocity to LVLH (Local Vertical Local Horizontal) frame
 *
 * LVLH basis vectors:
 * - r_hat: Radial direction (from parent to object)
 * - v_hat: Along-track direction (velocity direction for circular orbit)
 * - h_hat: Cross-track direction (orbit normal)
 *
 * @param position Position vector in inertial frame
 * @param velocity Velocity vector in inertial frame
 * @param parent_mass Mass of central body
 * @param out_r_hat Output: radial unit vector
 * @param out_v_hat Output: along-track unit vector
 * @param out_h_hat Output: cross-track unit vector
 */
void cartesian_to_lvlh_basis(
    Vec3 position,
    Vec3 velocity,
    double parent_mass,
    Vec3* out_r_hat,
    Vec3* out_v_hat,
    Vec3* out_h_hat
);

/**
 * Project relative state onto LVLH basis
 *
 * @param rel_pos Relative position (target - chaser)
 * @param r_hat Radial unit vector
 * @param v_hat Along-track unit vector
 * @param h_hat Cross-track unit vector
 * @param rel_vel Relative velocity
 * @param out Relative state in LVLH frame
 */
void project_to_lvlh_frame(
    Vec3 rel_pos,
    Vec3 r_hat,
    Vec3 v_hat,
    Vec3 h_hat,
    Vec3 rel_vel,
    LVLHRelativeState* out
);

/**
 * Transform LVLH relative state back to Cartesian
 *
 * @param lvlh Relative state in LVLH frame
 * @param r_hat Radial unit vector
 * @param v_hat Along-track unit vector
 * @param h_hat Cross-track unit vector
 * @param chaser_pos Chaser position (for absolute position calculation)
 * @param out_r_cart Output: relative position in Cartesian
 * @param out_v_cart Output: relative velocity in Cartesian
 */
void lvlh_to_cartesian(
    LVLHRelativeState* lvlh,
    Vec3 r_hat,
    Vec3 v_hat,
    Vec3 h_hat,
    Vec3 chaser_pos,
    Vec3* out_r_cart,
    Vec3* out_v_cart
);

// ============================================================================
// CW Validity Functions
// ============================================================================

/**
 * Check if CW equations are valid for current relative state
 *
 * Validity criteria:
 * - Spatial: max(|x|,|y|,|z|) / orbital_radius < 0.05
 * - Time: n * dt < 2.0 (where dt is time since last linearization)
 *
 * @param chaser Chaser spacecraft
 * @param target Target (body or spacecraft)
 * @param parent Central body
 * @param sim Simulation state
 * @return CWValidityResult with validity flags and error estimates
 */
CWValidityResult check_cw_validity(
    Spacecraft* chaser,
    void* target,  // Can be Spacecraft* or CelestialBody*
    CelestialBody* parent,
    SimulationState* sim
);

/**
 * Compute mean motion for given orbital radius
 *
 * @param parent_mass Mass of central body
 * @param orbital_radius Orbital radius
 * @return Mean motion n = sqrt(mu / a^3)
 */
double compute_mean_motion(
    double parent_mass,
    double orbital_radius
);

// ============================================================================
// CW Guidance Functions
// ============================================================================

/**
 * Solve CW equations for rendezvous guidance
 *
 * Uses closed-form CW solutions to compute required delta-v for interception
 *
 * CW Equations (linearized relative motion):
 *   x'' - 2n*y' - 3n^2*x = 0
 *   y'' + 2n*x' = 0
 *   z'' + n^2*z = 0
 *
 * @param chaser Chaser spacecraft
 * @param target Target
 * @param parent Central body
 * @param time_to_intercept Desired time to intercept
 * @return CWGuidanceSolution with required delta-v
 */
CWGuidanceSolution solve_cw_guidance(
    Spacecraft* chaser,
    void* target,
    CelestialBody* parent,
    double time_to_intercept,
    SimulationState* sim
);

/**
 * Calculate optimal time to intercept for minimum delta-v
 *
 * For circular coplanar orbits, optimal intercept occurs at:
 * - Half the relative orbital period for along-track separation
 * - Adjusted for radial separation
 *
 * @param lvlh Relative state in LVLH frame
 * @param mean_motion Mean motion of reference orbit
 * @return Optimal time to intercept
 */
double calculate_optimal_intercept_time(
    LVLHRelativeState* lvlh,
    double mean_motion
);

// ============================================================================
// Rendezvous Target Management
// ============================================================================

/**
 * Initialize rendezvous target structure
 *
 * @param target Target structure to initialize
 * @param target_index Index of target object
 * @param is_spacecraft_target True if target is spacecraft, false if body
 * @param approach_distance Distance to start approach phase
 * @param capture_distance Distance for capture
 * @param max_relative_velocity Max closing speed for capture
 */
void initialize_rendezvous_target(
    RendezvousTarget* target,
    int target_index,
    bool is_spacecraft_target,
    double approach_distance,
    double capture_distance,
    double max_relative_velocity
);

/**
 * Get target object (spacecraft or body) from index
 *
 * @param sim Simulation state
 * @param target_index Index of target
 * @param is_spacecraft_target Output: whether target is spacecraft
 * @return Pointer to target (Spacecraft* or CelestialBody*)
 */
void* get_rendezvous_target(
    SimulationState* sim,
    int target_index,
    bool* is_spacecraft_target
);

/**
 * Update rendezvous state machine based on current relative state
 *
 * State transitions:
 * - PLANNING -> APPROACHING: when within approach_distance
 * - APPROACHING -> MATCHING: when relative velocity < threshold
 * - MATCHING -> COMPLETE: when within capture_distance AND relative velocity < max
 * - Any -> FAILED: if CW validity is lost or distance increases
 *
 * @param chaser Chaser spacecraft
 * @param target Rendezvous target
 * @param sim Simulation state
 */
void update_rendezvous_state(
    Spacecraft* chaser,
    SimulationState* sim
);

// ============================================================================
// Burn Application Functions
// ============================================================================

/**
 * Apply CW guidance burn to chaser spacecraft
 *
 * @param chaser Chaser spacecraft
 * @param solution CW guidance solution
 * @param sim Simulation state
 */
void apply_cw_guidance_burn(
    Spacecraft* chaser,
    CWGuidanceSolution* solution,
    SimulationState* sim
);

/**
 * Calculate relative velocity magnitude between chaser and target
 *
 * @param chaser Chaser spacecraft
 * @param target Target
 * @param parent Central body
 * @return Relative velocity magnitude (m/s)
 */
double calculate_relative_velocity_magnitude(
    Spacecraft* chaser,
    void* target,
    CelestialBody* parent
);

/**
 * Calculate distance between chaser and target
 *
 * @param chaser Chaser spacecraft
 * @param target Target
 * @return Distance (m)
 */
double calculate_rendezvous_distance(
    Spacecraft* chaser,
    void* target
);

#endif // RENDEZVOUS_H