import pytest
import math

from target_simulator.core.models import (
    Target,
    Waypoint,
    ManeuverType,
    TurnDirection,
    KNOTS_TO_FPS,
    NM_TO_FT,
    G_IN_FPS2,
)
from pytest import approx


@pytest.fixture
def initial_waypoint() -> Waypoint:
    return Waypoint(
        maneuver_type=ManeuverType.FLY_TO_POINT,
        duration_s=0.0,
        target_range_nm=0.0,
        target_azimuth_deg=0.0,
        target_altitude_ft=10000.0,
        target_velocity_fps=500.0,
        target_heading_deg=0.0,
    )


def test_generate_path_straight_fly_for_duration():
    start_velocity_fps = 500.0
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            duration_s=10.0,
            target_range_nm=20.0,
            target_azimuth_deg=90.0,
            target_altitude_ft=10000.0,
            target_velocity_fps=start_velocity_fps,
            target_heading_deg=90.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_FOR_DURATION,
            duration_s=10.0,
            target_velocity_fps=start_velocity_fps,
            target_heading_deg=90.0,
            target_altitude_ft=10000.0,
        ),
    ]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=False
    )
    assert path is not None
    assert len(path) == 2
    assert total_duration == approx(10.0)
    start_point = path[0]
    expected_start_x_ft = 20.0 * NM_TO_FT
    expected_start_y_ft = 0.0
    assert start_point[1] == approx(expected_start_x_ft)
    assert start_point[2] == approx(expected_start_y_ft, abs=1e-9)
    end_point = path[1]
    distance_traveled_ft = start_velocity_fps * 10.0
    expected_end_x_ft = expected_start_x_ft + distance_traveled_ft
    assert end_point[1] == approx(expected_end_x_ft)
    assert end_point[2] == approx(expected_start_y_ft, abs=1e-9)
    assert end_point[3] == approx(10000.0)


def test_generate_path_dynamic_turn(initial_waypoint):
    speed_fps = initial_waypoint.target_velocity_fps
    turn_g = 1.0
    duration_s = 10.0
    trajectory = [
        initial_waypoint,
        Waypoint(
            maneuver_type=ManeuverType.DYNAMIC_MANEUVER,
            duration_s=duration_s,
            maneuver_speed_fps=speed_fps,
            lateral_acceleration_g=turn_g,
            turn_direction=TurnDirection.RIGHT,
        ),
    ]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=False
    )
    assert total_duration == approx(duration_s, rel=0.01)
    assert len(path) == 101

    # --- Verification based on final heading and distance ---
    accel_fps2 = turn_g * G_IN_FPS2
    turn_rate_rad_s = accel_fps2 / speed_fps
    expected_final_heading_deg = (
        initial_waypoint.target_heading_deg + math.degrees(turn_rate_rad_s * duration_s)
    ) % 360

    final_point = path[-1]
    prev_point = path[-2]
    dx = final_point[1] - prev_point[1]
    dy = final_point[2] - prev_point[2]
    final_heading_deg = math.degrees(math.atan2(dx, dy)) % 360

    # Assert that final heading is close to what's expected
    assert final_heading_deg == approx(expected_final_heading_deg, abs=1.0)

    # Assert that the final position is on the circle radius
    radius_ft = speed_fps**2 / accel_fps2
    center_x = radius_ft  # Center for a right turn starting North
    final_dist_from_center = math.sqrt(
        (final_point[1] - center_x) ** 2 + (final_point[2] - 0) ** 2
    )
    assert final_dist_from_center == approx(radius_ft, rel=0.01)


def test_generate_path_dynamic_climb_accelerate(initial_waypoint):
    duration_s = 10.0
    trajectory = [
        initial_waypoint,
        Waypoint(
            maneuver_type=ManeuverType.DYNAMIC_MANEUVER,
            duration_s=duration_s,
            longitudinal_acceleration_g=0.5,
            vertical_acceleration_g=0.2,
        ),
    ]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=False
    )
    assert total_duration == approx(duration_s, rel=0.01)
    assert len(path) == 101
    start_speed = initial_waypoint.target_velocity_fps
    accel_lon_fps2 = 0.5 * G_IN_FPS2
    accel_ver_fps2 = 0.2 * G_IN_FPS2
    expected_y = (start_speed * duration_s) + (0.5 * accel_lon_fps2 * duration_s**2)
    expected_z_gain = 0.5 * accel_ver_fps2 * duration_s**2
    expected_z = initial_waypoint.target_altitude_ft + expected_z_gain
    final_point = path[-1]
    assert final_point[1] == approx(0, abs=1e-9)
    assert final_point[2] == approx(expected_y, rel=0.01)
    assert final_point[3] == approx(expected_z, rel=0.01)


def test_target_update_state_interpolation():
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=10.0,
            target_azimuth_deg=0.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            duration_s=10.0,
            target_range_nm=20.0,
            target_azimuth_deg=0.0,
        ),
    ]
    target = Target(target_id=1, trajectory=trajectory)
    target.update_state(delta_time_s=5.0)
    assert target.current_range_nm == approx(15.0)
    assert target.current_azimuth_deg == approx(0.0)
    expected_speed_fps = (10.0 * NM_TO_FT) / 10.0

    assert target.current_velocity_fps == approx(expected_speed_fps, rel=0.05)


# --- Test aggiuntivi per models.py ---
def test_waypoint_to_dict():
    wp = Waypoint(
        maneuver_type=ManeuverType.FLY_TO_POINT,
        duration_s=5.0,
        target_range_nm=2.0,
        target_azimuth_deg=45.0,
        target_altitude_ft=5000.0,
        target_velocity_fps=300.0,
        target_heading_deg=90.0,
        turn_direction=TurnDirection.LEFT,
    )
    d = wp.to_dict()
    assert d["maneuver_type"] == "Fly to Point"
    assert d["duration_s"] == 5.0
    assert d["target_range_nm"] == 2.0
    assert d["target_azimuth_deg"] == 45.0
    assert d["target_altitude_ft"] == 5000.0
    assert d["target_velocity_fps"] == 300.0
    assert d["target_heading_deg"] == 90.0
    assert d["turn_direction"] == "Left"


def test_target_id_validation():
    with pytest.raises(ValueError):
        Target(target_id=-1, trajectory=[])
    with pytest.raises(ValueError):
        Target(target_id=100, trajectory=[])
    # Valori validi
    t = Target(target_id=0, trajectory=[])
    assert t.target_id == 0
    t = Target(target_id=15, trajectory=[])
    assert t.target_id == 15


def test_target_reset_simulation_sets_state():
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
        )
    ]
    t = Target(target_id=1, trajectory=trajectory)
    t.current_range_nm = 999.0
    t.reset_simulation()
    assert t.current_range_nm == approx(1.0)
    assert t.active is True


def test_target_to_dict():
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
        )
    ]
    t = Target(target_id=2, trajectory=trajectory, traceable=False, use_spline=True)
    d = t.to_dict()
    assert d["target_id"] == 2
    assert d["traceable"] is False
    assert d["use_spline"] is True
    assert isinstance(d["trajectory"], list)


def test_scenario_add_remove_get_targets():
    from target_simulator.core.models import Scenario

    s = Scenario(name="TestScenario")
    t1 = Target(target_id=1, trajectory=[])
    t2 = Target(target_id=2, trajectory=[])
    s.add_target(t1)
    s.add_target(t2)
    assert s.get_target(1) is t1
    assert s.get_target(2) is t2
    s.remove_target(1)
    assert s.get_target(1) is None
    assert len(s.get_all_targets()) == 1


def test_scenario_reset_and_update_state():
    from target_simulator.core.models import Scenario

    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            duration_s=10.0,
            target_range_nm=2.0,
            target_azimuth_deg=0.0,
        ),
    ]
    t = Target(target_id=1, trajectory=trajectory)
    s = Scenario()
    s.add_target(t)
    t.update_state(5.0)
    assert t.current_range_nm != approx(1.0)
    s.reset_simulation()
    assert t.current_range_nm == approx(1.0)
    s.update_state(10.0)
    assert t.current_range_nm == approx(2.0)


def test_generate_path_fly_to_point_with_velocity_and_heading():
    # Copre le righe 193-194
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            duration_s=5.0,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
            target_altitude_ft=1000.0,
            target_velocity_fps=300.0,
            target_heading_deg=90.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            duration_s=5.0,
            target_range_nm=2.0,
            target_azimuth_deg=0.0,
            target_altitude_ft=1000.0,
            target_velocity_fps=350.0,
            target_heading_deg=180.0,
        ),
    ]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=False
    )
    assert total_duration == approx(5.0)
    assert len(path) > 0


def test_generate_path_with_spline():
    # Copre le righe 261-275
    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=2.0,
            target_azimuth_deg=45.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=3.0,
            target_azimuth_deg=90.0,
        ),
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=4.0,
            target_azimuth_deg=135.0,
        ),
    ]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=True
    )
    assert total_duration >= 0.0
    assert len(path) >= 4


def test_generate_path_empty_or_wrong_type():
    # Copre la riga 280
    path, total_duration = Target.generate_path_from_waypoints([], use_spline=False)
    assert path == []
    assert total_duration == 0.0
    # Waypoint con tipo diverso
    trajectory = [Waypoint(maneuver_type=ManeuverType.FLY_FOR_DURATION)]
    path, total_duration = Target.generate_path_from_waypoints(
        trajectory, use_spline=False
    )
    assert path == []
    assert total_duration == 0.0


def test_scenario_is_finished():
    # Copre la riga 370
    from target_simulator.core.models import Scenario

    t1 = Target(target_id=1, trajectory=[])
    t1.active = False
    t2 = Target(target_id=2, trajectory=[])
    t2.active = False
    s = Scenario()
    s.add_target(t1)
    s.add_target(t2)
    assert s.is_finished() is True


def test_scenario_from_dict_constant_turn():
    # Copre le righe 386-388
    from target_simulator.core.models import Scenario

    scenario_dict = {
        "name": "TestConstantTurn",
        "targets": [
            {
                "target_id": 1,
                "active": True,
                "traceable": True,
                "use_spline": False,
                "trajectory": [
                    {
                        "maneuver_type": "Constant Turn",
                        "duration_s": 5.0,
                        "target_range_nm": 1.0,
                        "target_azimuth_deg": 0.0,
                        "target_altitude_ft": 1000.0,
                        "target_velocity_fps": 300.0,
                        "target_heading_deg": 90.0,
                        "turn_direction": "Right",
                    }
                ],
            }
        ],
    }
    s = Scenario.from_dict(scenario_dict)
    assert s.get_target(1) is not None
    assert isinstance(s.get_target(1).trajectory[0], Waypoint)
    assert s.get_target(1).trajectory[0].maneuver_type == ManeuverType.DYNAMIC_MANEUVER


def test_scenario_from_dict_invalid_target_data(capfd):
    # Copre le righe 407-408
    from target_simulator.core.models import Scenario

    scenario_dict = {
        "name": "TestInvalid",
        "targets": [
            {"target_id": 1, "trajectory": ["not_a_dict"]},
        ],
    }
    s = Scenario.from_dict(scenario_dict)
    out, err = capfd.readouterr()
    assert "Skipping invalid target data" in out
    from target_simulator.core.models import Scenario

    trajectory = [
        Waypoint(
            maneuver_type=ManeuverType.FLY_TO_POINT,
            target_range_nm=1.0,
            target_azimuth_deg=0.0,
        ),
    ]
    t = Target(target_id=1, trajectory=trajectory)
    s = Scenario(name="Scenario1")
    s.add_target(t)
    d = s.to_dict()
    assert d["name"] == "Scenario1"
    assert isinstance(d["targets"], list)
    loaded = Scenario.from_dict(d)
    assert loaded.name == "Scenario1"
    assert loaded.get_target(1) is not None