import numpy as np
import pytest
from scipy.spatial.transform import Rotation

from defense_sim.entities.drone import Drone
from defense_sim.entities.projectile import Projectile
from defense_sim.physics.flight import NED_TO_ENU, FRD_TO_FLU, RotorPyDynamics, JSBSimDynamics


def test_attitude_frame_roundtrip():
    rotation = Rotation.from_euler('xyz', [.2, -.1, .7]).as_matrix()
    transformed = NED_TO_ENU @ rotation @ FRD_TO_FLU
    assert np.linalg.det(transformed) == pytest.approx(1.)
    assert np.allclose(NED_TO_ENU.T @ transformed @ FRD_TO_FLU, rotation)


def test_rotorpy_hover_and_thrust_affect_motion():
    pytest.importorskip('rotorpy')
    d = Drone('d', 'defense_drone', np.array([0., 0., -1.]),
              target_position_ned_m=np.array([0., 0., -1.]))
    dynamics = RotorPyDynamics(d)
    params = dynamics.params
    dynamics.state['rotor_speeds'][:] = np.sqrt(params['mass']*dynamics.vehicle.g/(4*params['k_eta']))
    for _ in range(50):
        dynamics.step(d, .01, np.zeros(3), True)
    assert np.allclose(d.position_ned_m, [0, 0, -1], atol=.001)
    assert np.linalg.norm(d.orientation_quaternion_wxyz) == pytest.approx(1.)
    d.target_position_ned_m = np.array([0., 0., -2.])
    for _ in range(50):
        dynamics.step(d, .01, np.zeros(3), True)
    assert d.position_ned_m[2] < -1.1
    assert np.all(dynamics.state['rotor_speeds'] <= params['rotor_speed_max'])


def test_jsbsim_headless_si_motion_and_model_validation():
    pytest.importorskip('jsbsim')
    p = Projectile('aircraft', np.array([0., 0., -300.]), np.array([50., 0., 0.]))
    dynamics = JSBSimDynamics(p, 'c172p')
    for _ in range(10):
        dynamics.step(p, .01, np.zeros(3))
    assert 4 < p.position_ned_m[0] < 6
    assert np.isfinite(p.velocity_ned_m_s).all()
    with pytest.raises(ValueError, match='controls'):
        JSBSimDynamics(p, 'c172p', {'throttle': 2.})