#!/usr/bin/env python3
"""
Pre-compute Hohmann transfer rendezvous parameters for test validation.
Replicates the exact rendezvous module phasing logic from src/rendezvous.cpp.
Hardcoded from tests/test_rendezvous.toml — no TOML parser needed.

Usage: python3 tests/compute_rendezvous_params.py
"""

import math
import sys

G = 6.67430e-11

# Central body
EARTH_MASS = 5.972e24

# Spacecraft orbits (from test_rendezvous.toml)
TARGET_R = 6.771e6       # 400 km altitude
TARGET_NU = 0.0

CHASER_R = 6.671e6       # 300 km altitude
CHASER_NU = 4.71238898038469  # 270 degrees

MU = G * EARTH_MASS


def calc_mean_motion(radius, mass):
    """n = sqrt(mu / a^3)"""
    return math.sqrt(MU / (radius ** 3))


def hohmann_transfer_time(r1, r2, mass):
    """Half orbit of transfer ellipse."""
    a_transfer = (r1 + r2) / 2.0
    T_transfer = 2.0 * math.pi * math.sqrt(a_transfer ** 3 / MU)
    return T_transfer / 2.0


def required_separation(r1, r2, mass):
    """
    Required angular separation at first burn.
    chaser_pos - target_pos = target_angle - pi
    """
    transfer_time = hohmann_transfer_time(r1, r2, mass)
    n2 = calc_mean_motion(r2, mass)
    target_angle = n2 * transfer_time
    return target_angle - math.pi


def normalize_angle_2pi(angle):
    """Normalize to [0, 2*pi)."""
    while angle < 0.0:
        angle += 2.0 * math.pi
    while angle >= 2.0 * math.pi:
        angle -= 2.0 * math.pi
    return angle


def normalize_angle_pi(angle):
    """Normalize to [-pi, pi]."""
    angle = normalize_angle_2pi(angle)
    while angle > math.pi:
        angle -= 2.0 * math.pi
    while angle < -math.pi:
        angle += 2.0 * math.pi
    return angle


def calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass):
    """
    Wait time before Hohmann transfer.
    Positive = wait, negative = transfer already late.
    """
    required_sep = required_separation(r1, r2, mass)
    n1 = calc_mean_motion(r1, mass)
    n2 = calc_mean_motion(r2, mass)
    rel_angular_vel = n1 - n2

    current_sep = normalize_angle_pi(angular_separation)
    required_sep = normalize_angle_pi(required_sep)

    angle_to_close = required_sep - current_sep

    return angle_to_close / rel_angular_vel


def calculate_relative_orbit_period(r1, r2, mass):
    """Time between consecutive phasing opportunities."""
    n1 = calc_mean_motion(r1, mass)
    n2 = calc_mean_motion(r2, mass)
    rel_angular_vel = abs(n1 - n2)
    return 2.0 * math.pi / rel_angular_vel


def calculate_next_hohmann_wait_time(r1, r2, angular_separation, mass, min_wait_time):
    """
    Like calculate_wait_time_for_hohmann, but advances to next phasing
    opportunity if wait_time < min_wait_time. Always returns non-negative.
    """
    wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass)
    rel_period = calculate_relative_orbit_period(r1, r2, mass)

    while wait_time < min_wait_time:
        wait_time += rel_period

    return wait_time


def main():
    print(f"Central body: Earth, mass = {EARTH_MASS:.6e} kg")
    print(f"mu = {MU:.6e} m^3/s^2")

    print(f"\n=== INITIAL ORBITAL ELEMENTS ===")
    print(f"Chaser_Lower: r = {CHASER_R:.6e} m, nu = {CHASER_NU:.6f} rad ({math.degrees(CHASER_NU):.2f} deg)")
    print(f"Target:       r = {TARGET_R:.6e} m, nu = {TARGET_NU:.6f} rad ({math.degrees(TARGET_NU):.2f} deg)")

    # Angular separation: chaser - target
    angular_sep = CHASER_NU - TARGET_NU
    angular_sep = normalize_angle_pi(angular_sep)
    print(f"\nAngular separation (chaser - target): {angular_sep:.6f} rad ({math.degrees(angular_sep):.2f} deg)")

    # Mean motions
    n1 = calc_mean_motion(CHASER_R, EARTH_MASS)
    n2 = calc_mean_motion(TARGET_R, EARTH_MASS)
    print(f"\nMean motions:")
    print(f"  n1 (chaser):  {n1:.10f} rad/s")
    print(f"  n2 (target):  {n2:.10f} rad/s")
    print(f"  n1 - n2:      {n1 - n2:.10f} rad/s")

    # Orbital periods
    p_chaser = 2.0 * math.pi / n1
    p_target = 2.0 * math.pi / n2
    print(f"\nOrbital periods:")
    print(f"  Chaser:  {p_chaser:.2f} s ({p_chaser/3600:.2f} h)")
    print(f"  Target:  {p_target:.2f} s ({p_target/3600:.2f} h)")

    # Hohmann transfer
    tt = hohmann_transfer_time(CHASER_R, TARGET_R, EARTH_MASS)
    a_t = (CHASER_R + TARGET_R) / 2.0
    print(f"\n=== HOHMANN TRANSFER ===")
    print(f"  Transfer semi-major axis: {a_t:.6e} m")
    print(f"  Transfer time:            {tt:.6f} s ({tt/60:.2f} min)")

    # Required separation
    req_sep = required_separation(CHASER_R, TARGET_R, EARTH_MASS)
    req_sep_norm = normalize_angle_pi(req_sep)
    print(f"\n=== REQUIRED SEPARATION ===")
    print(f"  Raw:  {req_sep:.6f} rad ({math.degrees(req_sep):.2f} deg)")
    print(f"  Norm: {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)")

    # Relative orbit period
    rel_period = calculate_relative_orbit_period(CHASER_R, TARGET_R, EARTH_MASS)
    print(f"\nRelative orbit period: {rel_period:.6f} s ({rel_period/3600:.2f} h)")

    # Detailed phasing calculation
    print(f"\n=== PHASING CALCULATION ===")
    current_sep = normalize_angle_pi(angular_sep)
    print(f"  Current separation (normalized): {current_sep:.6f} rad ({math.degrees(current_sep):.2f} deg)")
    print(f"  Required separation (normalized): {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)")

    angle_to_close = req_sep_norm - current_sep
    print(f"  Angle to close:                {angle_to_close:.6f} rad ({math.degrees(angle_to_close):.2f} deg)")

    wait_time = calculate_wait_time_for_hohmann(CHASER_R, TARGET_R, angular_sep, EARTH_MASS)
    print(f"  Raw wait_time:                 {wait_time:.6f} s ({wait_time/3600:.2f} h)")

    # Wait times for various DT values
    dt_values = [0.1, 0.5, 1.0, 2.0, 5.0, 10.0]
    print(f"\n=== WAIT TIME vs DT (via calculate_next_hohmann_wait_time) ===")
    for dt in dt_values:
        wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt)
        arrival = wt + tt
        steps = int(arrival / dt) + 1
        print(f"  DT={dt:6.1f} s:  wait={wt:12.2f} s  arrival={arrival:12.2f} s  steps~{steps}")

    # Recommended values for TIME_STEP = 0.1
    dt = 0.1
    wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt)
    arrival = wt + tt
    max_steps = int(arrival / dt) + 1000

    print(f"\n=== RECOMMENDED FOR TEST (DT=0.1) ===")
    print(f"  wait_time:           {wt:.2f} s")
    print(f"  arrival_time:        {arrival:.2f} s")
    print(f"  expected_steps:      {int(arrival / dt)}")
    print(f"  max_steps (with margin): {max_steps}")
    print(f"  safety_limit (1 yr): {3600.0 * 24.0 * 365.0:.2f} s")
    print(f"\n  Milestone step indices:")
    print(f"    just_before_departure: {int(wt / dt)}")
    print(f"    after_departure:       {int(wt / dt) + 1}")
    print(f"    just_before_arrival:   {int(arrival / dt)}")

    # Verify against C++ test output
    print(f"\n=== COMPARISON WITH C++ TEST OUTPUT ===")
    print(f"  Python wait_time:   {wt:.2f} s")
    print(f"  C++ test wait_time: 60062.7 s")
    print(f"  Python arrival:     {arrival:.2f} s")
    print(f"  C++ test arrival:   62804.5 s")
    print(f"  Match: {abs(wt - 60062.7) < 0.1 and abs(arrival - 62804.5) < 0.1}")


if __name__ == '__main__':
    main()