import pytest

from lexia.robot.driver import NullMotionDriver, antenna_array, head_matrix
from lexia.robot.expression import ExpressionLoop
from lexia.robot.motion import Pose
from lexia.robot.state_machine import ReachyStateMachine, RobotState


class BibliotecaSinGrabaciones:
    """Aísla el repertorio manual aunque el SDK esté instalado en este PC."""

    def resolve(self, _name):
        return None


@pytest.fixture()
def bucle():
    estados = ReachyStateMachine()
    estados.transition(RobotState.IDLE)
    return ExpressionLoop(
        estados, NullMotionDriver(), seed=7, library=BibliotecaSinGrabaciones()
    )


def test_cada_fotograma_llega_al_driver(bucle):
    bucle.tick(0.0, 0.02)
    bucle.tick(0.02, 0.02)
    assert bucle.driver.last is not None


def test_el_bucle_sigue_al_estado(bucle):
    bucle.states.transition(RobotState.LISTENING)
    for paso in range(200):
        bucle.tick(paso * 0.02, 0.02)
    assert bucle.driver.last.head_pitch > 2.0, "en escucha la barbilla sube"


def test_un_gesto_se_reproduce_una_sola_vez(bucle):
    bucle.gesture("asentir")
    bucle.tick(0.0, 0.02)
    assert len(bucle.driver.gestures) == 3
    bucle.tick(0.02, 0.02)
    assert len(bucle.driver.gestures) == 3


def test_al_cambiar_de_estado_se_lanza_su_gesto(bucle):
    bucle.states.transition(RobotState.LISTENING)
    bucle.tick(0.0, 0.02)
    cantidad = len(bucle.driver.gestures)
    assert cantidad, "escuchar debe tener una entrada visible"
    bucle.tick(0.02, 0.02)
    assert len(bucle.driver.gestures) == cantidad


def test_un_gesto_inexistente_falla_pronto(bucle):
    with pytest.raises(KeyError):
        bucle.gesture("bailar")


def test_la_matriz_de_cabeza_es_una_pose_valida():
    matriz = head_matrix(Pose(head_yaw=30.0, head_z_mm=10.0))
    assert matriz.shape == (4, 4)
    assert matriz[3, 3] == pytest.approx(1.0)
    assert matriz[2, 3] == pytest.approx(0.01)
    rotacion = matriz[:3, :3]
    assert abs(float(rotacion.T.dot(rotacion).trace()) - 3.0) < 1e-6


def test_las_antenas_van_en_radianes():
    valores = antenna_array(Pose(antenna_left=45.0, antenna_right=-45.0))
    assert valores[0] == pytest.approx(0.7853981, abs=1e-5)
    assert valores[1] == pytest.approx(-0.7853981, abs=1e-5)