#!/usr/bin/env python3
"""
Generic orbital mechanics simulation engine.
Replicates the exact physics from src/orbital_mechanics.cpp and src/simulation.cpp.

Usage:
    from sim_engine import Simulator
    sim = Simulator("path/to/config.toml", dt=60.0)
    sim.run(steps=1000)
    for event in sim.events:
        print(event)
"""

import math
import tomllib
from dataclasses import dataclass, field, replace
from typing import Dict, Tuple, Any


# Constants

G = 6.67430e-11
PARABOLIC_TOLERANCE = 1e-3
KEPLER_TOLERANCE = 1e-10
KEPLER_MAX_ITER = 50
VEL_DRIFT_THRESHOLD = 1e-6  # m/s


# Vector operations

def vadd(a, b):
    return (a[0]+b[0], a[1]+b[1], a[2]+b[2])

def vsub(a, b):
    return (a[0]-b[0], a[1]-b[1], a[2]-b[2])

def vscale(v, s):
    return (v[0]*s, v[1]*s, v[2]*s)

def vmag(v):
    return math.sqrt(v[0]**2 + v[1]**2 + v[2]**2)

def vdot(a, b):
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]

def vcross(a, b):
    return (
        a[1]*b[2] - a[2]*b[1],
        a[2]*b[0] - a[0]*b[2],
        a[0]*b[1] - a[1]*b[0]
    )

def vnorm(v):
    m = vmag(v)
    if m < 1e-15:
        return (0.0, 0.0, 0.0)
    return (v[0]/m, v[1]/m, v[2]/m)

def normalize_angle(angle):
    while angle < 0.0:
        angle += 2.0 * math.pi
    while angle >= 2.0 * math.pi:
        angle -= 2.0 * math.pi
    return angle


def angular_distance(a, b):
    """Shortest angular distance on unit circle (matches C++)."""
    diff = abs(normalize_angle(a) - normalize_angle(b))
    return (2.0 * math.pi - diff) if diff > math.pi else diff


def true_anomaly_to_eccentric_anomaly(true_anomaly, eccentricity):
    """Convert true anomaly to eccentric anomaly (matches C++).
    Near-parabolic case uses cos/sin formulation to avoid instability.
    TODO: parabolic (e≈1) and hyperbolic (e>1) branches.
    """
    if abs(1.0 - eccentricity) < 0.01:
        # Near-parabolic: use cos/sin formulation
        nu = true_anomaly
        e = eccentricity
        cos_nu = math.cos(nu)
        sin_nu = math.sin(nu)
        denominator = 1.0 + e * cos_nu
        cos_E = (cos_nu + e) / denominator
        sin_E = sin_nu * math.sqrt(max(0.0, 1.0 - e * e)) / denominator
        cos_E = max(-1.0, min(1.0, cos_E))
        sin_E = max(-1.0, min(1.0, sin_E))
        return math.atan2(sin_E, cos_E)

    tan_half_nu = math.tan(true_anomaly / 2.0)
    tan_half_E = math.sqrt((1.0 - eccentricity) / (1.0 + eccentricity)) * tan_half_nu
    return 2.0 * math.atan(tan_half_E)


# Data structures

@dataclass
class OrbitalElements:
    a: float = 0.0            # semi-major axis (elliptical) / semi-latus rectum (parabolic)
    e: float = 0.0            # eccentricity
    nu: float = 0.0           # true anomaly
    inc: float = 0.0          # inclination
    Omega: float = 0.0        # longitude of ascending node
    omega: float = 0.0        # argument of periapsis
    p: float = 0.0            # semi-latus rectum (parabolic only)


@dataclass
class Body:
    name: str = ""
    mass: float = 0.0
    radius: float = 0.0
    parent_index: int = -1
    orbit: OrbitalElements = field(default_factory=OrbitalElements)
    local_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    local_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    global_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    global_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0)


@dataclass
class Spacecraft:
    name: str = ""
    mass: float = 0.0
    parent_index: int = -1
    orbit: OrbitalElements = field(default_factory=OrbitalElements)
    local_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    local_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    global_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    global_vel: Tuple[float, float, float] = (0.0, 0.0, 0.0)


class BurnDirection:
    PROGRADE = 0
    RETROGRADE = 1
    NORMAL = 2
    ANTINORMAL = 3
    RADIAL_IN = 4
    RADIAL_OUT = 5
    CUSTOM = 6


BURN_NAMES = ["PROGRADE", "RETROGRADE", "NORMAL", "ANTINORMAL", "RADIAL_IN", "RADIAL_OUT", "CUSTOM"]


class TriggerType:
    TIME = 0
    TRUE_ANOMALY = 1


TRIGGER_NAMES = ["TIME", "TRUE_ANOMALY"]


@dataclass
class BurnResult:
    """State vectors captured at the exact moment a burn fires (matches C++ BurnResult)."""
    valid: bool = False
    position: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    velocity: Tuple[float, float, float] = (0.0, 0.0, 0.0)
    true_anomaly: float = 0.0


@dataclass
class Maneuver:
    """Impulsive burn with trigger conditions (matches C++ Maneuver struct)."""
    name: str = ""
    craft_index: int = -1
    direction: int = 0          # BurnDirection
    delta_v: float = 0.0
    trigger_type: int = 0       # TriggerType
    trigger_value: float = 0.0
    scheduled_dt: float = 0.0
    executed: bool = False
    executed_time: float = 0.0
    burn_result: BurnResult = field(default_factory=BurnResult)


@dataclass
class Event:
    """Recorded simulation event."""
    kind: str = "state"
    time: float = 0.0
    data: Dict[str, Any] = field(default_factory=dict)


# Burn direction vectors (local frame)

def get_burn_direction(direction, local_pos, local_vel):
    """Calculate burn direction vector in local frame."""
    if direction == BurnDirection.PROGRADE:
        return vnorm(local_vel)
    elif direction == BurnDirection.RETROGRADE:
        return vscale(vnorm(local_vel), -1.0)
    elif direction == BurnDirection.NORMAL:
        h = vcross(local_pos, local_vel)
        return vnorm(h)
    elif direction == BurnDirection.ANTINORMAL:
        h = vcross(local_pos, local_vel)
        return vscale(vnorm(h), -1.0)
    elif direction == BurnDirection.RADIAL_IN:
        return vscale(vnorm(local_pos), -1.0)
    elif direction == BurnDirection.RADIAL_OUT:
        return vnorm(local_pos)
    elif direction == BurnDirection.CUSTOM:
        raise ValueError("CUSTOM requires explicit delta_v vector")
    return (0.0, 0.0, 0.0)


def apply_impulsive_burn(craft, direction, delta_v, parent_mass):
    """Apply an impulsive burn to a spacecraft. Updates orbit elements."""
    burn_dir = get_burn_direction(direction, craft.local_pos, craft.local_vel)
    dv_vec = vscale(burn_dir, delta_v)
    craft.local_vel = vadd(craft.local_vel, dv_vec)
    craft.global_vel = vadd(craft.global_vel, dv_vec)
    # Reconstruct orbital elements from new state
    if craft.parent_index >= 0:
        craft.orbit = cartesian_to_orbital_elements(craft.local_pos, craft.local_vel, parent_mass)


def check_maneuver_trigger(maneuver, craft, sim_time, sim_dt, bodies):
    """Check if a maneuver trigger fires this timestep (matches C++ check_maneuver_trigger).
    Sets maneuver.scheduled_dt and returns True if trigger fires.
    TODO: parabolic (Barker's equation) and hyperbolic branches for TRIGGER_TRUE_ANOMALY.
    """
    if maneuver.trigger_type == TriggerType.TIME:
        if sim_time > maneuver.trigger_value:
            maneuver.scheduled_dt = 0.0
            return True
        if sim_time + sim_dt <= maneuver.trigger_value:
            return False
        dt_to_burn = maneuver.trigger_value - sim_time
        maneuver.scheduled_dt = max(0.0, min(dt_to_burn, sim_dt))
        return True

    elif maneuver.trigger_type == TriggerType.TRUE_ANOMALY:
        if craft.parent_index < 0 or craft.parent_index >= len(bodies):
            return False

        parent = bodies[craft.parent_index]
        current_nu = normalize_angle(craft.orbit.nu)
        target_nu = normalize_angle(maneuver.trigger_value)

        # Near: fire immediately
        if angular_distance(current_nu, target_nu) < 0.01:
            maneuver.scheduled_dt = 0.0
            return True

        a = craft.orbit.a
        e = craft.orbit.e
        mu = G * parent.mass
        n = math.sqrt(mu / (a ** 3.0))

        E_current = true_anomaly_to_eccentric_anomaly(current_nu, e)
        E_target = true_anomaly_to_eccentric_anomaly(target_nu, e)

        M_current = E_current - e * math.sin(E_current)
        M_target = E_target - e * math.sin(E_target)

        M_delta = M_target - M_current
        dt_needed = M_delta / n

        # Wrap to next periapsis if negative
        if dt_needed < 0:
            M_period = 2.0 * math.pi
            dt_needed += M_period / n

        if dt_needed <= 0.0 or dt_needed > sim_dt:
            return False

        maneuver.scheduled_dt = dt_needed
        return True

    return False


def apply_custom_burn(craft, delta_v_vec):
    """Apply a custom delta-v vector directly to spacecraft velocity."""
    craft.local_vel = vadd(craft.local_vel, delta_v_vec)
    craft.global_vel = vadd(craft.global_vel, delta_v_vec)


# Kepler equation solvers (exact C++ logic)

def get_initial_trial_value(mean_anomaly, eccentricity):
    """Initial guess for Kepler solver (C++ get_initial_trial_value)."""
    return (mean_anomaly + eccentricity * math.sin(mean_anomaly)
            + ((eccentricity ** 2 / 2.0) * math.sin(2.0 * mean_anomaly)))


def solve_kepler_elliptical(mean_anomaly, eccentricity):
    E = get_initial_trial_value(mean_anomaly, eccentricity)
    E_prev = E + 2.0 * KEPLER_TOLERANCE
    for _ in range(KEPLER_MAX_ITER):
        if abs(E - E_prev) < KEPLER_TOLERANCE:
            break
        E_prev = E
        sin_E = math.sin(E)
        E = E - (E - eccentricity * sin_E - mean_anomaly) / (1.0 - eccentricity * math.cos(E))
    return E


# Coordinate transforms

def orbital_to_cartesian(elements, parent_mass):
    """Convert orbital elements to local position/velocity vectors."""
    mu = G * parent_mass
    a = elements.a
    e = elements.e
    nu = elements.nu

    if abs(e - 1.0) < PARABOLIC_TOLERANCE:
        p = elements.p
    else:
        p = a * (1.0 - e * e)

    r = p / (1.0 + e * math.cos(nu))

    x_orb = r * math.cos(nu)
    y_orb = r * math.sin(nu)

    vx_orb = -math.sqrt(mu / p) * math.sin(nu)
    vy_orb = math.sqrt(mu / p) * (e + math.cos(nu))

    # z-x-z rotation: Rz(Omega) * Rx(inc) * Rz(omega)
    cos_w, sin_w = math.cos(elements.omega), math.sin(elements.omega)
    x1 = x_orb * cos_w - y_orb * sin_w
    y1 = x_orb * sin_w + y_orb * cos_w

    cos_i, sin_i = math.cos(elements.inc), math.sin(elements.inc)
    x2 = x1
    y2 = y1 * cos_i
    z2 = y1 * sin_i

    cos_O, sin_O = math.cos(elements.Omega), math.sin(elements.Omega)
    pos = (x2 * cos_O - y2 * sin_O,
           x2 * sin_O + y2 * cos_O,
           z2)

    vx1 = vx_orb * cos_w - vy_orb * sin_w
    vy1 = vx_orb * sin_w + vy_orb * cos_w
    vx2 = vx1
    vy2 = vy1 * cos_i
    vz2 = vy1 * sin_i

    vel = (vx2 * cos_O - vy2 * sin_O,
           vx2 * sin_O + vy2 * cos_O,
           vz2)

    return pos, vel


def cartesian_to_orbital_elements(pos, vel, parent_mass):
    """Convert local position/velocity to orbital elements."""
    mu = G * parent_mass
    r = vmag(pos)
    v = vmag(vel)
    v_sq = v * v

    specific_energy = -mu / r + v_sq / 2.0
    h_vec = vcross(pos, vel)
    h = vmag(h_vec)

    # Eccentricity vector: e_vec = (v² - μ/r)r - (r·v)v  all divided by μ
    r_dot_v = vdot(pos, vel)
    e_vec = ((v_sq - mu / r) * pos[0] - r_dot_v * vel[0]) / mu, \
            ((v_sq - mu / r) * pos[1] - r_dot_v * vel[1]) / mu, \
            ((v_sq - mu / r) * pos[2] - r_dot_v * vel[2]) / mu
    e = vmag(e_vec)

    # Semi-major axis
    if abs(specific_energy) < 1e-10:
        a = 1e10
    else:
        a = -mu / (2.0 * specific_energy)

    # True anomaly
    if e < 1e-10:
        # Nearly circular: use argument of latitude
        n_vec = vcross((0.0, 0.0, 1.0), h_vec)
        n_mag = vmag(n_vec)
        sin_i = (n_mag / h) if h > 1e-10 else 1.0
        if sin_i > 1e-6 and n_mag > 1e-10:
            # Well-defined ascending node: compute argument of latitude
            x_AN = n_vec[0] / n_mag
            y_AN = n_vec[1] / n_mag
            hcn = vcross(h_vec, n_vec)
            hcn_mag = vmag(hcn)
            if hcn_mag > 1e-10:
                hcn = vscale(hcn, 1.0 / hcn_mag)
            r_xAN = pos[0] * x_AN + pos[1] * y_AN
            r_yAN = pos[0] * hcn[0] + pos[1] * hcn[1] + pos[2] * hcn[2]
            nu = math.atan2(r_yAN, r_xAN)
        else:
            # Nearly coplanar: use atan2(y, x) as argument of latitude
            nu = math.atan2(pos[1], pos[0])
        nu = normalize_angle(nu)
    else:
        cos_nu = vdot(pos, e_vec) / (r * e)
        cos_nu = max(-1.0, min(1.0, cos_nu))
        sin_nu = None

        if abs(cos_nu) > 1.0 - 1e-10:
            h_cross_e = vcross(h_vec, e_vec)
            denom = r * e * h
            sin_nu = vdot(pos, h_cross_e) / denom if denom > 1e-10 else 0.0
        else:
            r_cross_h = vcross(pos, h_vec)
            denom = r * e * h
            sin_nu = vdot(r_cross_h, e_vec) / denom if denom > 1e-10 else 0.0

        nu = math.atan2(sin_nu, cos_nu)
        if nu == -math.pi:
            nu = math.pi
        nu = normalize_angle(nu)

    # Inclination
    i = math.acos(h_vec[2] / h) if h > 1e-10 else 0.0

    # RAAN
    n_vec = vcross((0.0, 0.0, 1.0), h_vec)
    n_mag = vmag(n_vec)
    if n_mag > 1e-10:
        Omega = math.acos(n_vec[0] / n_mag)
        if n_vec[1] < 0.0:
            Omega = 2.0 * math.pi - Omega
    else:
        Omega = 0.0

    # Argument of periapsis
    inclination_threshold = 0.01
    if e > 1e-10 and n_mag > 1e-10 and i > inclination_threshold:
        cos_omega = vdot(e_vec, n_vec) / (e * n_mag)
        n_cross_e = vcross(n_vec, e_vec)
        sin_omega = vdot(n_cross_e, h_vec) / (e * n_mag * h)
        omega = math.atan2(sin_omega, cos_omega)
        if omega < 0.0:
            omega += 2.0 * math.pi
    elif e > 1e-10:
        omega = math.atan2(e_vec[1], e_vec[0])
        if omega < 0.0:
            omega += 2.0 * math.pi
    else:
        omega = 0.0

    elements = OrbitalElements()
    if abs(e - 1.0) < 1e-3:
        elements.p = (h * h) / mu
    else:
        elements.a = a
    elements.e = e
    elements.nu = nu
    elements.inc = i
    elements.Omega = Omega
    elements.omega = omega

    return elements


# Propagation

def propagate(elements, dt, parent_mass):
    """Propagate orbital elements forward by dt. Returns new elements."""
    mu = G * parent_mass
    a = elements.a
    e = elements.e
    nu = elements.nu

    if abs(e - 1.0) < PARABOLIC_TOLERANCE:
        # Parabolic (Barker's equation)
        p = elements.p
        D = math.tan(nu / 2.0)
        M = D + (D * D * D) / 3.0
        n = math.sqrt(mu / (p ** 3.0))
        M = M + n * dt
        # Solve Barker's: D + D^3/3 = M
        c = 1.5 * M
        disc = c * c + 1.0
        sqrt_disc = math.sqrt(disc)
        D_new = math.cbrt(c + sqrt_disc) + math.cbrt(c - sqrt_disc)
        return replace(elements, nu=2.0 * math.atan(D_new))

    elif e < 1.0:
        # Elliptical
        n = math.sqrt(mu / (a ** 3.0))
        E = 2.0 * math.atan(math.sqrt((1.0 - e) / (1.0 + e)) * math.tan(nu / 2.0))
        M = E - e * math.sin(E)
        M = M + n * dt
        E_new = get_initial_trial_value(M, e)
        E_prev = E_new + 2.0 * KEPLER_TOLERANCE
        for _ in range(KEPLER_MAX_ITER):
            if abs(E_new - E_prev) < KEPLER_TOLERANCE:
                break
            E_prev = E_new
            sin_E = math.sin(E_new)
            E_new = E_new - (E_new - e * sin_E - M) / (1.0 - e * math.cos(E_new))
        nu_new = 2.0 * math.atan(math.sqrt((1.0 + e) / (1.0 - e)) * math.tan(E_new / 2.0))
        return replace(elements, nu=nu_new)

    else:
        # Hyperbolic
        raise NotImplementedError("hyperbolic propagation not yet implemented")


# Global coordinate computation

def compute_global_coordinates(bodies):
    """
    Compute global position/velocity for all bodies.
    Matches C++ compute_global_coordinates() exactly.
    """
    for body in bodies:
        if body.parent_index == -1:
            body.global_pos = body.local_pos
            body.global_vel = body.local_vel
        elif 0 <= body.parent_index < len(bodies):
            parent = bodies[body.parent_index]
            body.global_pos = vadd(body.local_pos, parent.global_pos)
            body.global_vel = vadd(body.local_vel, parent.global_vel)


# Velocity drift check

def check_velocity_drift(body, parent, parent_mass):
    """
    Check if local velocity has drifted from expected Keplerian velocity.
    If so, reconstruct orbital elements from current state.
    Matches C++ update_bodies_physics() drift check.
    """
    if parent is None:
        return

    _, expected_vel = orbital_to_cartesian(body.orbit, parent_mass)
    vel_diff = vmag(vsub(body.local_vel, expected_vel))
    if vel_diff > VEL_DRIFT_THRESHOLD:
        body.orbit = cartesian_to_orbital_elements(body.local_pos, body.local_vel, parent_mass)


# Body physics update

def update_body(bodies, body_index, dt):
    """
    Update a single body: drift check, propagation.
    Matches C++ update_bodies_physics() per-body logic (without SOI).
    """
    body = bodies[body_index]

    if body.parent_index == -1:
        return  # Root body doesn't propagate

    if 0 <= body.parent_index < len(bodies):
        parent = bodies[body.parent_index]
        check_velocity_drift(body, parent, parent.mass)
        body.orbit = propagate(body.orbit, dt, parent.mass)
        body.local_pos, body.local_vel = orbital_to_cartesian(body.orbit, parent.mass)


# Spacecraft physics update

def update_spacecraft(spacecraft_list, bodies, maneuvers, dt, sim_time):
    """
    Update spacecraft: drift check, maneuver triggers, propagation.
    Matches C++ update_spacecraft_physics() with maneuver trigger system.
    """
    for i, craft in enumerate(spacecraft_list):
        if craft.parent_index < 0 or craft.parent_index >= len(bodies):
            continue
        parent = bodies[craft.parent_index]

        # Velocity drift check
        _, expected_vel = orbital_to_cartesian(craft.orbit, parent.mass)
        vel_diff = vmag(vsub(craft.local_vel, expected_vel))
        if vel_diff > VEL_DRIFT_THRESHOLD:
            craft.orbit = cartesian_to_orbital_elements(craft.local_pos, craft.local_vel, parent.mass)

        # Check all pending maneuvers for this craft
        maneuver_fired = False
        burn_dt = 0.0
        fired_maneuver = None
        for j, maneuver in enumerate(maneuvers):
            if maneuver.executed:
                continue
            if maneuver.craft_index != i:
                continue
            if check_maneuver_trigger(maneuver, craft, sim_time, dt, bodies):
                burn_dt = maneuver.scheduled_dt
                fired_maneuver = maneuver
                maneuver_fired = True
                break

        if maneuver_fired:
            # Propagate to burn time
            craft.orbit = propagate(craft.orbit, burn_dt, parent.mass)
            craft.local_pos, craft.local_vel = orbital_to_cartesian(craft.orbit, parent.mass)

            # Capture exact pre-burn state (matches C++ BurnResult)
            fired_maneuver.burn_result = BurnResult(
                valid=True,
                position=tuple(craft.local_pos),
                velocity=tuple(craft.local_vel),
                true_anomaly=craft.orbit.nu,
            )

            # Execute burn
            apply_impulsive_burn(craft, fired_maneuver.direction, fired_maneuver.delta_v, parent.mass)
            fired_maneuver.executed = True
            fired_maneuver.executed_time = sim_time + burn_dt

            # Propagate remaining time
            remaining_dt = dt - burn_dt
            craft.orbit = propagate(craft.orbit, remaining_dt, parent.mass)
            craft.local_pos, craft.local_vel = orbital_to_cartesian(craft.orbit, parent.mass)
        else:
            # No maneuver: propagate full timestep
            craft.orbit = propagate(craft.orbit, dt, parent.mass)
            craft.local_pos, craft.local_vel = orbital_to_cartesian(craft.orbit, parent.mass)


def compute_global_coordinates_spacecraft(spacecraft_list, bodies):
    """
    Compute global position/velocity for all spacecraft.
    """
    for craft in spacecraft_list:
        if craft.parent_index >= 0 and craft.parent_index < len(bodies):
            parent = bodies[craft.parent_index]
            craft.global_pos = vadd(craft.local_pos, parent.global_pos)
            craft.global_vel = vadd(craft.local_vel, parent.global_vel)


# TOML config loader

def load_config(config_path):
    """Load a TOML 1.0 config file and return parsed data."""
    with open(config_path, "rb") as f:
        return tomllib.load(f)


def bodies_from_config(config):
    """
    Create Body objects from TOML config.
    Parent references are resolved by name, then by index.
    """
    bodies = []
    name_to_idx = {}

    # First pass: create bodies without positions
    for body_cfg in config.get("bodies", []):
        orbit_cfg = body_cfg.get("orbit", {})
        elements = OrbitalElements(
            a=orbit_cfg.get("semi_major_axis", 0.0),
            e=orbit_cfg.get("eccentricity", 0.0),
            nu=orbit_cfg.get("true_anomaly", 0.0),
            inc=orbit_cfg.get("inclination", 0.0),
            Omega=orbit_cfg.get("longitude_of_ascending_node", 0.0),
            omega=orbit_cfg.get("argument_of_periapsis", 0.0),
            p=orbit_cfg.get("semi_latus_rectum", 0.0),
        )

        parent_ref = body_cfg.get("parent_index", -1)
        if isinstance(parent_ref, str):
            # Resolve by name
            if parent_ref in name_to_idx:
                parent_index = name_to_idx[parent_ref]
            elif parent_ref == "Sun" or parent_ref == "root" or parent_ref == "-1":
                parent_index = -1
            else:
                raise ValueError(f"Unknown parent name: {parent_ref}")
        else:
            parent_index = int(parent_ref)

        body = Body(
            name=body_cfg.get("name", f"Body_{len(bodies)}"),
            mass=body_cfg.get("mass", 0.0),
            radius=body_cfg.get("radius", 0.0),
            parent_index=parent_index,
            orbit=elements,
        )
        bodies.append(body)
        name_to_idx[body.name] = len(bodies) - 1

    return bodies


def spacecraft_from_config(config, bodies):
    """
    Create Spacecraft objects from TOML config.
    Parent references resolved by body name.
    """
    spacecraft_list = []
    name_to_body = {b.name: i for i, b in enumerate(bodies)}

    for craft_cfg in config.get("spacecraft", []):
        orbit_cfg = craft_cfg.get("orbit", {})
        elements = OrbitalElements(
            a=orbit_cfg.get("semi_major_axis", 0.0),
            e=orbit_cfg.get("eccentricity", 0.0),
            nu=orbit_cfg.get("true_anomaly", 0.0),
            inc=orbit_cfg.get("inclination", 0.0),
            Omega=orbit_cfg.get("longitude_of_ascending_node", 0.0),
            omega=orbit_cfg.get("argument_of_periapsis", 0.0),
            p=orbit_cfg.get("semi_latus_rectum", 0.0),
        )

        parent_ref = craft_cfg.get("parent_index", -1)
        if isinstance(parent_ref, str):
            parent_index = name_to_body.get(parent_ref, -1)
        else:
            parent_index = int(parent_ref)

        craft = Spacecraft(
            name=craft_cfg.get("name", f"Craft_{len(spacecraft_list)}"),
            mass=craft_cfg.get("mass", 0.0),
            parent_index=parent_index,
            orbit=elements,
        )
        spacecraft_list.append(craft)

    return spacecraft_list


def initialize_spacecraft(spacecraft_list, bodies):
    """
    Initialize spacecraft from orbital elements.
    Compute local pos/vel and global pos/vel.
    """
    for craft in spacecraft_list:
        if craft.parent_index >= 0 and craft.parent_index < len(bodies):
            parent = bodies[craft.parent_index]
            local_pos, local_vel = orbital_to_cartesian(craft.orbit, parent.mass)
            craft.local_pos = local_pos
            craft.local_vel = local_vel
            craft.global_pos = vadd(parent.global_pos, local_pos)
            craft.global_vel = vadd(parent.global_vel, local_vel)
        else:
            craft.local_pos = (0.0, 0.0, 0.0)
            craft.local_vel = (0.0, 0.0, 0.0)
            craft.global_pos = (0.0, 0.0, 0.0)
            craft.global_vel = (0.0, 0.0, 0.0)


def maneuvers_from_config(config, spacecraft_list):
    """
    Create Maneuver objects from TOML config.
    Resolves spacecraft_name to craft_index.
    """
    maneuver_list = []
    name_to_craft = {c.name: i for i, c in enumerate(spacecraft_list)}

    direction_map = {
        "prograde": BurnDirection.PROGRADE,
        "retrograde": BurnDirection.RETROGRADE,
        "normal": BurnDirection.NORMAL,
        "antinormal": BurnDirection.ANTINORMAL,
        "radial_in": BurnDirection.RADIAL_IN,
        "radial_out": BurnDirection.RADIAL_OUT,
        "custom": BurnDirection.CUSTOM,
    }

    trigger_map = {
        "time": TriggerType.TIME,
        "true_anomaly": TriggerType.TRUE_ANOMALY,
    }

    for man_cfg in config.get("maneuvers", []):
        craft_name = man_cfg.get("spacecraft_name", "")
        craft_index = name_to_craft.get(craft_name, -1)

        direction = direction_map.get(man_cfg.get("direction", "prograde").lower(), BurnDirection.PROGRADE)
        trigger_type = trigger_map.get(man_cfg.get("trigger_type", "time").lower(), TriggerType.TIME)

        maneuver = Maneuver(
            name=man_cfg.get("name", f"Maneuver_{len(maneuver_list)}"),
            craft_index=craft_index,
            direction=direction,
            delta_v=float(man_cfg.get("delta_v", 0.0)),
            trigger_type=trigger_type,
            trigger_value=float(man_cfg.get("trigger_value", 0.0)),
        )
        maneuver_list.append(maneuver)

    return maneuver_list


# Initialization

def initialize_bodies(bodies):
    """
    Initialize orbital objects from orbital elements.
    Matches C++ initialize_orbital_objects() exactly (without SOI).
    """
    for i, body in enumerate(bodies):
        if body.parent_index >= 0 and body.parent_index < len(bodies):
            parent = bodies[body.parent_index]
            local_pos, local_vel = orbital_to_cartesian(body.orbit, parent.mass)
            body.local_pos = local_pos
            body.local_vel = local_vel
            body.global_pos = vadd(parent.global_pos, local_pos)
            body.global_vel = vadd(parent.global_vel, local_vel)
        else:
            body.local_pos = (0.0, 0.0, 0.0)
            body.local_vel = (0.0, 0.0, 0.0)
            body.global_pos = (0.0, 0.0, 0.0)
            body.global_vel = (0.0, 0.0, 0.0)


# Simulator — public API

class Simulator:
    """
    Generic orbital mechanics simulator.

    Usage:
        sim = Simulator("config.toml", dt=60.0)
        sim.run(steps=1000)

        # Access results
        for event in sim.events:
            print(event)

        # Access final state
        for body in sim.bodies:
            print(f"{body.name}: r={vmag(body.global_pos):.0f} m")
    """

    def __init__(self, config_path, dt=60.0):
        self.dt = dt
        self.time = 0.0
        self.events = []
        self._body_count = 0

        config = load_config(config_path)
        self.bodies = bodies_from_config(config)
        initialize_bodies(self.bodies)
        self._body_count = len(self.bodies)
        self.spacecraft = spacecraft_from_config(config, self.bodies)
        initialize_spacecraft(self.spacecraft, self.bodies)
        self.maneuvers = maneuvers_from_config(config, self.spacecraft)

    def run(self, steps):
        """Run simulation for the given number of timesteps."""
        for _ in range(steps):
            self._step()

    def _step(self):
        """Single simulation step. Matches C++ update_simulation() order."""
        sim_time = self.time
        # 1. Update body physics (drift, propagation)
        for i in range(self._body_count):
            update_body(self.bodies, i, self.dt)

        # 2. Compute global coordinates for bodies
        compute_global_coordinates(self.bodies)

        # 3. Update spacecraft physics (drift, propagation, maneuver triggers)
        update_spacecraft(self.spacecraft, self.bodies, self.maneuvers, self.dt, sim_time)

        # 4. Compute global coordinates for spacecraft
        compute_global_coordinates_spacecraft(self.spacecraft, self.bodies)

        self.time += self.dt

    def record_state(self, label=""):
        """Record current simulation state as an event."""
        state = {}
        for body in self.bodies:
            r = vmag(body.global_pos)
            state[body.name] = {
                "r": r,
                "nu": body.orbit.nu,
                "a": body.orbit.a,
                "e": body.orbit.e,
                "parent": body.parent_index,
                "parent_name": self.bodies[body.parent_index].name if body.parent_index >= 0 else "root",
            }
        self.events.append(Event(kind="state", time=self.time, data={"label": label, "state": state}))

    def get_body(self, name_or_index):
        """Get a body by name or index."""
        if isinstance(name_or_index, int):
            return self.bodies[name_or_index]
        for body in self.bodies:
            if body.name == name_or_index:
                return body
        raise KeyError(f"Body not found: {name_or_index}")

    def get_craft(self, name_or_index):
        """Get a spacecraft by name or index."""
        if isinstance(name_or_index, int):
            return self.spacecraft[name_or_index]
        for craft in self.spacecraft:
            if craft.name == name_or_index:
                return craft
        raise KeyError(f"Spacecraft not found: {name_or_index}")

    def record_craft_state(self, label=""):
        """Record current spacecraft state as an event."""
        state = {}
        for craft in self.spacecraft:
            r = vmag(craft.global_pos)
            state[craft.name] = {
                "r": r,
                "nu": craft.orbit.nu,
                "a": craft.orbit.a,
                "e": craft.orbit.e,
                "parent": craft.parent_index,
                "parent_name": self.bodies[craft.parent_index].name if craft.parent_index >= 0 else "root",
            }
        self.events.append(Event(kind="craft_state", time=self.time, data={"label": label, "state": state}))

    def print_summary(self):
        """Print a summary of all recorded state events."""
        for event in self.events:
            label = event.data.get("label", "")
            if label:
                print(f"\n*** {label} (t={event.time:.1f}s) ***")
            for name, info in event.data.get("state", {}).items():
                print(f"  {name}: r={info['r']:.0f} m, "
                      f"nu={math.degrees(info['nu']):.1f}°, "
                      f"a={info['a']:.0f}, e={info['e']:.6f}, "
                      f"parent={info['parent_name']}")