"""Tests for ``arautopilot.studio.simulator.rudder_dynamics``."""

from __future__ import annotations

import pytest

from arautopilot.studio.simulator.rudder_dynamics import (
    RudderDynamicsConfig,
    RudderSimulator,
    RunRecorder,
)


def test_zero_input_stays_at_rest() -> None:
    sim = RudderSimulator()
    sim.reset(angle_deg=0.0)
    for _ in range(1000):
        sim.step(dt=0.001, pwm_pct=0.0)
    assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9)
    assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-9)


def test_full_starboard_command_drives_to_stbd_endstop() -> None:
    """With marine-realistic gain (v_max ~5 dps) reaching 35 deg takes ~8 s."""
    sim = RudderSimulator()
    sim.reset(angle_deg=0.0)
    for _ in range(15000):
        sim.step(dt=0.001, pwm_pct=100.0)
    assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-6)
    assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6)


def test_full_port_command_drives_to_port_endstop() -> None:
    sim = RudderSimulator()
    sim.reset(angle_deg=0.0)
    for _ in range(15000):
        sim.step(dt=0.001, pwm_pct=-100.0)
    assert sim.state.angle_deg == pytest.approx(-sim.config.max_angle_deg, abs=1e-6)
    assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6)


def test_command_inside_deadband_produces_no_motion() -> None:
    sim = RudderSimulator(RudderDynamicsConfig(deadband_pct=10.0,
                                                min_useful_pwm_pct=15.0))
    sim.reset(angle_deg=0.0)
    for _ in range(2000):
        sim.step(dt=0.001, pwm_pct=5.0)  # under deadband
    assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9)


def test_command_just_above_deadband_snaps_to_min_useful() -> None:
    """A command of 11% with deadband=10/min_useful=15 should act like 15%."""
    cfg = RudderDynamicsConfig(deadband_pct=10.0, min_useful_pwm_pct=15.0)
    sim_a = RudderSimulator(cfg)
    sim_b = RudderSimulator(cfg)
    sim_a.reset()
    sim_b.reset()
    for _ in range(3000):
        sim_a.step(dt=0.001, pwm_pct=11.0)
        sim_b.step(dt=0.001, pwm_pct=15.0)
    assert sim_a.state.angle_deg == pytest.approx(sim_b.state.angle_deg, abs=1e-9)


def test_asymmetry_makes_starboard_faster() -> None:
    cfg = RudderDynamicsConfig(asymmetry_stbd_over_port=1.5,
                                deadband_pct=0.0, min_useful_pwm_pct=0.0)
    sim_s = RudderSimulator(cfg)
    sim_p = RudderSimulator(cfg)
    sim_s.reset()
    sim_p.reset()
    for _ in range(2000):
        sim_s.step(dt=0.001, pwm_pct=+50.0)
        sim_p.step(dt=0.001, pwm_pct=-50.0)
    # Starboard angle should be larger in magnitude than port at the same
    # effort, because the pump pushes harder to starboard.
    assert abs(sim_s.state.angle_deg) > abs(sim_p.state.angle_deg)


def test_recorder_captures_trajectory() -> None:
    """Sanity check the recorder: capture a few seconds of constant-command
    motion and verify the samples reflect the rudder moving in the commanded
    direction with strictly monotonic position (no oscillation under constant
    drive)."""
    sim = RudderSimulator()
    sim.reset()
    rec = RunRecorder()
    for k in range(2000):
        t = k * 0.001
        sim.step(dt=0.001, pwm_pct=80.0)
        rec.record(t=t, setpoint_deg=25.0, sim=sim, pwm_pct=80.0)
    assert len(rec.samples) == 2000
    # Monotonically increasing angle (constant +ve command + viscous damping
    # means no overshoot during the run).
    assert rec.samples[-1].angle_deg > rec.samples[0].angle_deg
    assert all(rec.samples[i + 1].angle_deg >= rec.samples[i].angle_deg
               for i in range(len(rec.samples) - 1))


def test_invalid_dt_rejected() -> None:
    sim = RudderSimulator()
    with pytest.raises(ValueError):
        sim.step(dt=0.0, pwm_pct=50.0)
    with pytest.raises(ValueError):
        sim.step(dt=-0.001, pwm_pct=50.0)


def test_endstop_zeros_outward_velocity_only() -> None:
    """Hitting the stbd end-stop should kill +velocity but allow reverse."""
    sim = RudderSimulator()
    sim.reset(angle_deg=sim.config.max_angle_deg, angular_vel_dps=20.0)
    sim.step(dt=0.001, pwm_pct=0.0)
    assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-9)
    assert sim.state.angular_vel_dps == 0.0
    # Now reverse: should move away from the stop.
    for _ in range(200):
        sim.step(dt=0.001, pwm_pct=-80.0)
    assert sim.state.angle_deg < sim.config.max_angle_deg