"""
test_blocks.py — Tests individuals del mòdul blocks.py.
Executa des del directori Rasp/:  python tests/test_blocks.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
from blocks import (
    blocks_setup,
    servo_move_to, OPEN_POSITION, EJECT_POSITION, MIN_SERVO_US, MAX_SERVO_US,
    read_block_color, read_color_raw,
    _COLORS,
    BK, RD, GN, BU, YE, OG, VT,
)

_COLOR_NAMES = {
    BK: "Negre (BK)",
    RD: "Vermell (RD)",
    GN: "Verd (GN)",
    BU: "Blau (BU)",
    YE: "Groc (YE)",
    OG: "Taronja (OG)",
    VT: "Violeta (VT)",
}

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

def _teardown(pi):
    pi.stop()


# ==================
# TEST 1 — Servo
# ==================

def test_servo():
    """
    Mou el servo a les posicions principals: oberta, expulsió i torna a oberta.
    Hauries de veure/sentir el servo moure's suaument entre posicions.
    """
    print("=== TEST SERVO ===")
    pi = _setup()

    print(f"Movent a OPEN_POSITION ({OPEN_POSITION} µs)...")
    servo_move_to(OPEN_POSITION)
    time.sleep(1.0)
    print("Open: OK")

    print(f"Movent a EJECT_POSITION ({EJECT_POSITION} µs)...")
    servo_move_to(EJECT_POSITION)
    time.sleep(1.0)
    print("Eject: OK")

    print(f"Tornant a OPEN_POSITION...")
    servo_move_to(OPEN_POSITION)
    time.sleep(1.0)
    print("Torna a open: OK")

    print(f"Provant posició mínima ({MIN_SERVO_US} µs)...")
    servo_move_to(MIN_SERVO_US)
    time.sleep(1.0)

    print(f"Tornant a OPEN_POSITION...")
    servo_move_to(OPEN_POSITION)
    time.sleep(1.0)

    _teardown(pi)
    print("Test servo completat.\n")


# ==================
# TEST 2 — Sensor de color TCS34725
# ==================

def test_color_sensor():
    """
    Llegeix el color 15 vegades cada segon.
    Posa davant del sensor els blocs de colors per verificar que els reconeix.
    """
    print("=== TEST SENSOR DE COLOR TCS34725 ===")
    pi = _setup()

    print("Llegint color durant 15 segons (posa els blocs davant del sensor)...")
    for i in range(15):
        color_id = read_block_color()
        name = _COLOR_NAMES.get(color_id, "Desconegut")
        print(f"  Lectura {i+1:2d}: {name}")
        time.sleep(1.0)

    _teardown(pi)
    print("Test sensor de color completat.\n")


# ==================
# TEST 3 — Calibració del sensor de color (valors RGB crus)
# ==================

def test_color_raw():
    """
    Mostra valors RGB crus i la classificació actual durant 30 segons.
    Útil per calibrar la taula _COLORS a blocks.py.
    Format: R=xxx  G=xxx  B=xxx  → classificat com 'XX' (diff=xx)
    Si el diff és gran (>10), els valors de referència necessiten ajust.
    """
    print("=== TEST COLOR RAW (calibració) ===")
    pi = _setup()

    print("Llegint valors RGB crus durant 30 segons (posa cada bloc davant del sensor)...")
    print(f"  {'R':>5}  {'G':>5}  {'B':>5}    classificat    diff")
    for i in range(30):
        r, g, b = read_color_raw()
        best_name = "??"
        best_diff = 9999
        for ref in _COLORS:
            diff = abs(r - ref["r"]) + abs(g - ref["g"]) + abs(b - ref["b"])
            if diff < best_diff:
                best_diff = diff
                best_name = ref["name"]
        print(f"  R={r:3d}  G={g:3d}  B={b:3d}    → {best_name}  (diff={best_diff})")
        time.sleep(1.0)

    _teardown(pi)
    print("Test color raw completat.\n")


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

if __name__ == "__main__":
    # Descomenta el test que vols executar:
    test_servo()
    # test_color_sensor()
    # test_color_raw()    # Per calibrar la taula de colors