import numpy as np
import json

from defense_sim.entities.drone import Drone, DroneConfig
from defense_sim.entities.projectile import Projectile, ProjectileConfig
from defense_sim.physics.atmosphere import isa
from defense_sim.physics.integrator import rk4_step
from defense_sim.physics.turbulence import DrydenGust
from defense_sim.simulation.engine import SimulationEngine
from defense_sim.guidance.activation import CityCircle, first_circle_crossing, select_defenders
from defense_sim.sensors.models import Sensor, SensorConfig
from defense_sim.statistics.metrics import summarize_runs


def test_isa_sea_level_and_density_decreases():
    sea = isa(0)
    assert sea.temperature_k == 288.15
    assert abs(sea.pressure_pa - 101325) < 1
    assert abs(sea.density_kg_m3 - 1.225) < 0.002
    assert isa(5000).density_kg_m3 < sea.density_kg_m3


def test_rk4_constant_acceleration_matches_analytic():
    state = np.array([0.0, 0.0])
    result = rk4_step(lambda y: np.array([y[1], 3.0]), state, 0.2)
    assert np.allclose(result, [0.06, 0.6], atol=1e-12)


def test_projectile_vacuum_vertical_motion_and_drag_reduces_speed():
    p = Projectile("p", np.zeros(3), np.array([10.0, 0.0, 0.0]), ProjectileConfig(area_m2=0))
    p.step(0.1, np.zeros(3))
    assert abs(p.position_ned_m[0] - 1.0) < 1e-8
    assert abs(p.velocity_ned_m_s[2] - 0.980665) < 1e-8
    q = Projectile("q", np.zeros(3), np.array([100.0, 0.0, 0.0]), ProjectileConfig())
    q.step(0.1, np.zeros(3))
    assert q.velocity_ned_m_s[0] < 100


def test_drone_actuator_and_turn_rate_are_bounded():
    cfg = DroneConfig(max_acceleration_m_s2=2, max_turn_rate_rad_s=0.2, acceleration_time_constant_s=0.01)
    d = Drone("d", "defense_drone", np.zeros(3), config=cfg, target_position_ned_m=np.array([0., 100., 0.]))
    d.estimated_position_ned_m = d.position_ned_m.copy()
    d.estimated_velocity_ned_m_s = d.velocity_ned_m_s.copy()
    d.step(0.1, np.zeros(3), np.random.default_rng(1))
    assert np.linalg.norm(d.actuator_acceleration_ned_m_s2) <= 2 + 1e-9
    assert abs(d.angular_velocity_body_rad_s[2]) <= 0.2 + 1e-9


def test_dryden_is_seed_reproducible_and_finite():
    a = DrydenGust(np.random.default_rng(19), (1, 1, 1))
    b = DrydenGust(np.random.default_rng(19), (1, 1, 1))
    assert np.allclose(a.update(0.01, 20), b.update(0.01, 20))
    assert np.isfinite(a.value).all()


def test_seeded_engine_snapshots_reproduce_and_separate_estimate_error():
    def create():
        return SimulationEngine(seed=3, drones=[Drone("d", "attack_drone", np.array([0., 0., -10.]))])
    x, y = create(), create()
    sx, sy = x.step_and_snapshot(), y.step_and_snapshot()
    assert sx["entities"] == sy["entities"]
    d = sx["entities"][0]
    assert d["position_error_ned_m"] is not None
    assert d["orientation_quaternion_wxyz"][0] >= 0


def test_swept_interception_detects_crossing_between_steps():
    p = Projectile("p", np.array([-1., 0., -10.]), np.array([200., 0., 0.]),
                   ProjectileConfig(area_m2=0, collision_radius_m=0.1))
    d = Drone("d", "defense_drone", np.array([0., 0., -10.]),
              velocity_ned_m_s=np.array([0., 0., 0.]), config=DroneConfig(collision_radius_m=0.1))
    sim = SimulationEngine(dt=0.01, seed=1, projectiles=[p], drones=[d])
    sim.step()
    assert sim.events and sim.events[0]["type"] == "interception"
    assert sim.events[0]["miss_distance_m"] <= 0.2 + 1e-12


def test_error_sign_and_monte_carlo_aggregation_shape():
    d = Drone("d", "defense_drone", np.array([2., 0., 0.]), estimated_position_ned_m=np.array([1., 0., 0.]))
    assert np.allclose(d.position_ned_m - d.estimated_position_ned_m, [1, 0, 0])
    stats = summarize_runs([{"projectile_id": "p1", "intercepted": True, "miss_distance_m": 0.1,
                             "interception_time_s": 2.0},
                            {"projectile_id": "p1", "intercepted": False, "miss_distance_m": 3.0}])
    assert stats["overall"]["interception_probability"] == 0.5
    assert stats["interception_by_projectile"]["p1"]["run_count"] == 2


def test_projected_first_city_circle_crossing_and_three_nearest_defenders():
    p = Projectile("threat", np.array([1000., 0., -1000.]), np.array([-50., 0., 0.]),
                   ProjectileConfig(area_m2=0))
    city = CityCircle(np.zeros(3), 500.0)
    crossing = first_circle_crossing(p, city, np.zeros(3), dt=0.1)
    assert crossing is not None
    assert abs(np.linalg.norm(crossing[:2]) - 500.0) < 1e-6
    positions = [[500., 0., -1000.], [500., 100., -1000.],
                 [500., -100., -1000.], [0., 0., -1000.]]
    drones = [Drone(f"d{i}", "defense_drone", np.array(pos)) for i, pos in enumerate(positions)]
    # The selection rule ranks available defense drones by 3D distance to the
    # predicted inward crossing point.
    chosen = select_defenders(drones, crossing, count=3)
    assert [d.entity_id for d in chosen] == ["d0", "d1", "d2"]


def test_sensor_latency_uses_historical_ground_truth_and_packet_serializes():
    d = Drone("d", "attack_drone", np.array([0., 0., -10.]))
    sensor = Sensor(SensorConfig(position_sigma_m=0, velocity_sigma_m_s=0, latency_s=0.2),
                    np.random.default_rng(2))
    assert sensor.observe(d, 0.0) is None
    d.position_ned_m = np.array([10., 0., -10.])
    assert sensor.observe(d, 0.1) is None
    delayed = sensor.observe(d, 0.2)
    assert np.allclose(delayed.position_ned_m, [0, 0, -10])
    d.position_ned_m = np.array([20., 0., -10.])
    current = sensor.observe(d, 0.3)
    assert np.allclose(current.position_ned_m, [10, 0, -10])
    assert current.timestamp_s == 0.1
    packet = SimulationEngine(seed=4, drones=[d]).step_and_snapshot()
    json.dumps(packet, allow_nan=False)