#!/usr/bin/env python3
"""
Precalculate expected values for test_analytical_propagation.cpp.
Computes orbital parameters, propagation results, and error bounds.
"""

import math
import sys
sys.path.insert(0, '.')
from sim_engine import (
    OrbitalElements, Spacecraft, Body, orbital_to_cartesian,
    propagate, G, vmag, vnorm
)

# =============================================================================
# Spacecraft parameters from TOML
# =============================================================================

craft_apsides = Spacecraft(
    name="Apsides_Test_Spacecraft",
    mass=1000.0,
    parent_index=0,
    orbit=OrbitalElements(a=2.0e7, e=0.6, nu=0.0, inc=0.0, Omega=0.0, omega=0.0),
)

craft_timestep = Spacecraft(
    name="Timestep_Test_Spacecraft",
    mass=1000.0,
    parent_index=0,
    orbit=OrbitalElements(a=1.5e7, e=0.4, nu=0.0, inc=0.0, Omega=0.0, omega=0.0),
)

earth_mass = 5.972e24
mu = G * earth_mass

def print_comment_block(title):
    print(f"\n// === {title} ===")

def print_const(name, value, comment=""):
    c = f"  // {comment}" if comment else ""
    print(f"const double {name} = {value:.15e};{c}")

# =============================================================================
# 1. Apsides calculations
# =============================================================================

print_comment_block("Apsides Test Spacecraft (a=2e7, e=0.6)")

a1 = craft_apsides.orbit.a
e1 = craft_apsides.orbit.e
r_peri1 = a1 * (1.0 - e1)
r_apo1 = a1 * (1.0 + e1)
period1 = 2.0 * math.pi * math.sqrt(a1**3 / mu)
n1 = math.sqrt(mu / a1**3)

print(f"\n// Orbital period: {period1:.6f} s ({period1/3600:.2f} hours)")
print(f"// Mean motion: {n1:.15e} rad/s")

# Velocity at perigee (nu=0)
peri1 = OrbitalElements(a=a1, e=e1, nu=0.0, inc=0.0, Omega=0.0, omega=0.0)
pos_peri1, vel_peri1 = orbital_to_cartesian(peri1, earth_mass)
v_peri1 = vmag(vel_peri1)

# Velocity at apogee (nu=pi)
apo1 = OrbitalElements(a=a1, e=e1, nu=math.pi, inc=0.0, Omega=0.0, omega=0.0)
pos_apo1, vel_apo1 = orbital_to_cartesian(apo1, earth_mass)
v_apo1 = vmag(vel_apo1)

# Velocity at nu=pi/4
nu45_1 = OrbitalElements(a=a1, e=e1, nu=math.pi/4.0, inc=0.0, Omega=0.0, omega=0.0)
pos_45_1, vel_45_1 = orbital_to_cartesian(nu45_1, earth_mass)
v_45_1 = vmag(vel_45_1)
r_45_1 = vmag(pos_45_1)

print_const("A1_R_PERI", r_peri1, "m")
print_const("A1_R_APO", r_apo1, "m")
print_const("A1_V_PERI", v_peri1, "m/s")
print_const("A1_V_APO", v_apo1, "m/s")
print_const("A1_V_AT_PI4", v_45_1, "m/s at nu=pi/4")
print_const("A1_R_AT_PI4", r_45_1, "m at nu=pi/4")
print_const("A1_PERIOD", period1, "seconds")

# =============================================================================
# 2. Timestep Test Spacecraft (a=1.5e7, e=0.4)
# =============================================================================

print_comment_block("Timestep Test Spacecraft (a=1.5e7, e=0.4)")

a2 = craft_timestep.orbit.a
e2 = craft_timestep.orbit.e
r_peri2 = a2 * (1.0 - e2)
r_apo2 = a2 * (1.0 + e2)
period2 = 2.0 * math.pi * math.sqrt(a2**3 / mu)
n2 = math.sqrt(mu / a2**3)

print(f"\n// Orbital period: {period2:.6f} s ({period2/3600:.2f} hours)")
print(f"// Mean motion: {n2:.15e} rad/s")

# Velocity at perigee
peri2 = OrbitalElements(a=a2, e=e2, nu=0.0, inc=0.0, Omega=0.0, omega=0.0)
pos_peri2, vel_peri2 = orbital_to_cartesian(peri2, earth_mass)
v_peri2 = vmag(vel_peri2)
r_peri2_calc = vmag(pos_peri2)

print_const("A2_R_PERI", r_peri2, "m")
print_const("A2_R_APO", r_apo2, "m")
print_const("A2_V_PERI", v_peri2, "m/s")
print_const("A2_PERIOD", period2, "seconds")

# =============================================================================
# 3. Vis-viva checks at multiple true anomalies
# =============================================================================

print_comment_block("Vis-viva checks at multiple true anomalies")

true_anomalies = [0.0, math.pi/4.0, math.pi/2.0, 3.0*math.pi/4.0, math.pi]
for nu in true_anomalies:
    deg = nu * 180.0 / math.pi
    el = OrbitalElements(a=a1, e=e1, nu=nu, inc=0.0, Omega=0.0, omega=0.0)
    pos, vel = orbital_to_cartesian(el, earth_mass)
    r = vmag(pos)
    v = vmag(vel)
    expected_v = math.sqrt(mu * (2.0/r - 1.0/a1))
    v_error = abs(v - expected_v)
    rel_error = v_error / expected_v * 100.0
    print(f"// nu={deg:6.1f}deg: r={r:.3f} m, v={v:.6f} m/s, expected_v={expected_v:.6f} m/s, rel_err={rel_error:.8f}%")

# =============================================================================
# 4. Propagation accuracy tests
# =============================================================================

print_comment_block("Propagation accuracy tests")

# Initial state for timestep craft
init_el = OrbitalElements(a=a2, e=e2, nu=0.0, inc=0.0, Omega=0.0, omega=0.0)
init_pos, init_vel = orbital_to_cartesian(init_el, earth_mass)
init_r = vmag(init_pos)
init_v = vmag(init_vel)

# Large timestep: 2x period
large_dt = period2 * 2.0
prop_large = propagate(init_el, large_dt, earth_mass)
pos_large, vel_large = orbital_to_cartesian(prop_large, earth_mass)
r_large = vmag(pos_large)
v_large = vmag(vel_large)
r_err_large = abs(r_large - init_r)
v_err_large = abs(v_large - init_v)
rel_r_large = r_err_large / init_r * 100.0
rel_v_large = v_err_large / init_v * 100.0
print(f"// 2x period: r_err={r_err_large:.6f} m ({rel_r_large:.8f}%), v_err={v_err_large:.6f} m/s ({rel_v_large:.8f}%)")

# Small timestep: 0.1 s
small_dt = 0.1
prop_small = propagate(init_el, small_dt, earth_mass)
pos_small, vel_small = orbital_to_cartesian(prop_small, earth_mass)
pos_change = math.sqrt((pos_small[0]-init_pos[0])**2 + (pos_small[1]-init_pos[1])**2 + (pos_small[2]-init_pos[2])**2)
vel_change = math.sqrt((vel_small[0]-init_vel[0])**2 + (vel_small[1]-init_vel[1])**2 + (vel_small[2]-init_vel[2])**2)
expected_pos_change = init_v * small_dt
pos_error_small = abs(pos_change - expected_pos_change)
print(f"// 0.1s dt: pos_change={pos_change:.6f} m, vel_change={vel_change:.10f} m/s")
print(f"// expected_pos_change={expected_pos_change:.6f} m, pos_error={pos_error_small:.6f} m")

# =============================================================================
# 5. Accuracy vs timestep size
# =============================================================================

print_comment_block("Accuracy vs timestep size")

dt_ratios = [0.01, 0.1, 1.0, 10.0]
for ratio in dt_ratios:
    dt = period2 * ratio
    prop = propagate(init_el, dt, earth_mass)
    pos_f, vel_f = orbital_to_cartesian(prop, earth_mass)
    pos_err = math.sqrt((pos_f[0]-init_pos[0])**2 + (pos_f[1]-init_pos[1])**2 + (pos_f[2]-init_pos[2])**2)
    vel_err = math.sqrt((vel_f[0]-init_vel[0])**2 + (vel_f[1]-init_vel[1])**2 + (vel_f[2]-init_vel[2])**2)
    num_periods = dt / period2
    expected_orbits = round(num_periods)
    fractional_phase = num_periods - expected_orbits
    expected_pos_err = abs(fractional_phase) * 2.0 * math.pi * a2
    print(f"// dt={ratio:.2f}x period: pos_err={pos_err:.3f} m, vel_err={vel_err:.6f} m/s, "
          f"num_periods={num_periods:.4f}, expected_pos_err={expected_pos_err:.3f} m")
    if expected_orbits > 0 and expected_pos_err > 1e-6:
        rel_err = pos_err / expected_pos_err
        print(f"//   relative_error={rel_err:.6f}")

# =============================================================================
# 6. Long-term propagation (100 periods)
# =============================================================================

print_comment_block("Long-term propagation (100 periods)")

prop_100 = propagate(init_el, period2 * 100.0, earth_mass)
final_nu = prop_100.nu
expected_delta_nu = n2 * period2 * 100.0
expected_nu = init_el.nu + expected_delta_nu
# Normalize both to [0, 2*pi)
while final_nu < 0:
    final_nu += 2.0 * math.pi
while final_nu >= 2.0 * math.pi:
    final_nu -= 2.0 * math.pi
while expected_nu < 0:
    expected_nu += 2.0 * math.pi
while expected_nu >= 2.0 * math.pi:
    expected_nu -= 2.0 * math.pi

raw_error = abs(final_nu - expected_nu)
anomaly_error = min(raw_error, 2.0 * math.pi - raw_error)
print(f"// final_nu={final_nu:.15e} rad")
print(f"// expected_nu={expected_nu:.15e} rad")
print(f"// raw_error={raw_error:.15e} rad")
print(f"// anomaly_error={anomaly_error:.15e} rad ({anomaly_error*180/math.pi:.10e} degrees)")

# =============================================================================
# 7. Full orbit true anomaly accuracy
# =============================================================================

print_comment_block("Full orbit true anomaly accuracy")

# Test with initial nu = 0 (craft_apsides starts at nu=0)
full_prop = propagate(init_el, period2, earth_mass)
print(f"// After 1 period: nu={full_prop.nu:.15e} rad")
print(f"// Expected: nu=0 (same as initial)")
print(f"// Error: {abs(full_prop.nu):.15e} rad")

# =============================================================================
# Output summary
# =============================================================================

print("\n// === SUMMARY ===")
print(f"// Apsides spacecraft: a={a1:.0f}, e={e1}, period={period1:.2f}s")
print(f"// Timestep spacecraft: a={a2:.0f}, e={e2}, period={period2:.2f}s")
print(f"// Vis-viva relative errors are all < 0.01%")
print(f"// Full orbit position/velocity errors are < 0.1%")
print(f"// Long-term (100 periods) anomaly error: {anomaly_error:.15e} rad")