"""
test_motion.py — Tests individuals del mòdul motion.py.
Executa des del directori Rasp/:  python tests/test_motion.py

Descomenta la funció que vols provar al final del fitxer.
Assegura't que el venv està activat i pigpiod en marxa (sudo pigpiod -s 1).
"""

import sys
import os
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))

import time
import pigpio
import motion
from motion import (
    motion_setup, motion_setup_steppers, motion_setup_sensors, motion_cleanup,
    enable_wheels, enable_arms, enable_syringe,
    arms_home, syringe_home,
    distance_to_object,
    ON, OFF, CW, CCW,
)

def _pi_connect():
    pi = pigpio.pi()
    if not pi.connected:
        print("ERROR: pigpiod no està en marxa. Executa: sudo pigpiod -s 1")
        sys.exit(1)
    return pi

def _setup_motors():
    """Setup mínim per a tests de motors (sense sensors I2C)."""
    pi = _pi_connect()
    motion_setup_steppers(pi)
    return pi

def _setup_sensors():
    """Setup mínim per a tests de sensors I2C (sense steppers)."""
    pi = _pi_connect()
    motion_setup_sensors(pi)
    return pi

def _teardown_motors(pi):
    motion_cleanup()
    pi.stop()

def _teardown_sensors(pi):
    pi.stop()


# ==================
# TEST 1 — Motors pas a pas (rodes)
# ==================

def test_motor_unic():
    """
    Prova únicament la roda DRETA: 200 passos endavant i enrere.
    Primer test a fer — verifica que un sol motor funciona correctament.
    Hauries de veure la roda girar ~1/5 de volta (reducció 1:5).
    """
    print("=== TEST MOTOR ÚNIC (RODA DRETA) ===")
    pi = _setup_motors()

    PASSOS = 200

    print(f"Movent roda DRETA {PASSOS} passos endavant...")
    enable_wheels(ON)
    motion.wheel_R.move_to(PASSOS)
    while motion.wheel_R.is_running():
        time.sleep(0.05)
    print("Roda dreta endavant: OK")

    time.sleep(0.5)

    print(f"Movent roda DRETA {PASSOS} passos enrere...")
    motion.wheel_R.move_to(0)
    while motion.wheel_R.is_running():
        time.sleep(0.05)
    print("Roda dreta enrere: OK")

    enable_wheels(OFF)
    _teardown_motors(pi)
    print("Test motor únic completat.\n")


def test_motors():
    """
    Prova les dues rodes: avança 200 passos i torna enrere.
    Executa test_motor_unic() primer per verificar que un motor funciona.
    Hauries de veure cada roda girar ~1/5 de volta (reducció 1:5).
    """
    print("=== TEST MOTORS (RODES) ===")
    pi = _setup_motors()

    PASSOS = 200

    print(f"Movent roda DRETA {PASSOS} passos endavant...")
    enable_wheels(ON)
    motion.wheel_R.move_to(PASSOS)
    while motion.wheel_R.is_running():
        time.sleep(0.05)
    print("Roda dreta: OK")

    time.sleep(0.5)

    print(f"Movent roda DRETA {PASSOS} passos enrere...")
    motion.wheel_R.move_to(0)
    while motion.wheel_R.is_running():
        time.sleep(0.05)
    print("Roda dreta enrere: OK")

    time.sleep(0.5)

    print(f"Movent roda ESQUERRA {PASSOS} passos endavant...")
    motion.wheel_L.move_to(PASSOS)
    while motion.wheel_L.is_running():
        time.sleep(0.05)
    print("Roda esquerra: OK")

    time.sleep(0.5)

    print(f"Movent roda ESQUERRA {PASSOS} passos enrere...")
    motion.wheel_L.move_to(0)
    while motion.wheel_L.is_running():
        time.sleep(0.05)
    print("Roda esquerra enrere: OK")

    enable_wheels(OFF)
    _teardown_motors(pi)
    print("Test motors completat.\n")


# ==================
# TEST 2 — Homing (finals de carrera)
# ==================

def test_homing_brac():
    """
    Executa el homing únicament dels BRAÇOS.
    Primer test de homing — verifica que un sol conjunt de finals de carrera funciona.
    ATENCIÓ: assegura't que els braços tinguin espai per moure's.
    """
    print("=== TEST HOMING BRAÇOS ===")
    pi = _setup_motors()

    print("Iniciant homing dels BRAÇOS...")
    arms_home()
    print("Homing braços: OK")

    _teardown_motors(pi)
    print("Test homing braços completat.\n")


def test_homing():
    """
    Executa el homing complet: braços i xeringa.
    Executa test_homing_brac() primer per verificar els finals de carrera dels braços.
    ATENCIÓ: assegura't que els braços i la xeringa tinguin espai per moure's.
    """
    print("=== TEST HOMING COMPLET ===")
    pi = _setup_motors()

    print("Iniciant homing dels BRAÇOS...")
    arms_home()
    print("Homing braços: OK")

    time.sleep(1.0)

    print("Iniciant homing de la XERINGA...")
    syringe_home()
    print("Homing xeringa: OK")

    _teardown_motors(pi)
    print("Test homing completat.\n")


# ==================
# TEST 3 — Sensor de distància VL53L0X
# ==================

def test_distance_sensor():
    """
    Llegeix la distància 10 vegades cada 500ms.
    Posa la mà davant del sensor per verificar que canvia el valor.
    """
    print("=== TEST SENSOR DISTÀNCIA VL53L0X ===")
    pi = _setup_sensors()

    print("Llegint distància durant 5 segons (posa la mà davant del sensor)...")
    for i in range(10):
        dist = distance_to_object()
        if dist == 65535:
            print(f"  Lectura {i+1:2d}: fora de rang")
        else:
            print(f"  Lectura {i+1:2d}: {dist} mm")
        time.sleep(0.5)

    _teardown_sensors(pi)
    print("Test sensor distància completat.\n")


# ==================
# TEST 4 — Sensors de línia ADS1115
# ==================

def test_line_sensors():
    """
    Llegeix els dos sensors de línia 10 vegades cada 500ms.
    Posa el sensor sobre superfícies de diferent color per veure la variació.
    Valor alt (~9700+) = negre. Valor baix = blanc/clar.
    """
    print("=== TEST SENSORS DE LÍNIA ADS1115 ===")
    pi = _setup_sensors()

    print("Llegint sensors de línia durant 5 segons...")
    print("  (posa els sensors sobre blanc i negre per veure la diferència)")
    for i in range(10):
        r = motion._chan_r.value
        l = motion._chan_l.value
        print(f"  Lectura {i+1:2d}: DRETA={r:5d}  ESQUERRA={l:5d}  error={r-l:+6d}")
        time.sleep(0.5)

    _teardown_sensors(pi)
    print("Test sensors de línia completat.\n")


# ==================
# Execució
# ==================

if __name__ == "__main__":
    # Descomenta el test que vols executar:
    test_motor_unic()       # Primer: prova un sol motor
    # test_motors()         # Segon: prova les dues rodes
    # test_homing_brac()    # Primer homing: només els braços
    # test_homing()         # Homing complet: braços i xeringa
    # test_distance_sensor()
    # test_line_sensors()