"""
blocks.py — Lectura del sensor de color TCS34725 i servo d'expulsió de blocs.
Equivalent a blocks.cpp del codi Arduino/ESP32.

Requereix /boot/config.txt:
    dtoverlay=i2c-gpio,bus=4,i2c_gpio_sda=22,i2c_gpio_scl=27
"""

import time
import pigpio
import adafruit_extended_bus
import adafruit_tcs34725

from pins import SERVO_PWM

# ==================
# IDs de color
# ==================

BK = 0   # Negre (no reconegut)
RD = 1   # Vermell  → avançar
GN = 2   # Verd     → girar dreta
BU = 3   # Blau     → girar esquerra
YE = 4   # Groc     → xuclar líquid
OG = 5   # Taronja  → buidar líquid
VT = 6   # Violeta  → sorpresa

NUM_COLORS = 7

# Taula de colors de referència (valors RGB 0–255, calibrats amb el sensor)
_COLORS = [
    {"name": "BK", "r":  80, "g":  80, "b":  80},
    {"name": "RD", "r": 202, "g":  32, "b":  34},
    {"name": "GN", "r": 107, "g":  90, "b":  57},
    {"name": "BU", "r": 104, "g":  83, "b":  66},
    {"name": "YE", "r": 150, "g":  69, "b":  33},
    {"name": "OG", "r": 185, "g":  44, "b":  32},
    {"name": "VT", "r": 129, "g":  70, "b":  55},
]

# ==================
# Paràmetres del servo
# ==================

# Valors en µs de pulse width (pigpio set_servo_pulsewidth).
# Conversió des de l'ESP32 (16 bits, 50Hz): valor/65535 * 20000µs
# MIN  = 3277/65535 * 20000 ≈ 1000µs
# MAX  = 8000/65535 * 20000 ≈ 2440µs
# EJECT= 6450/65535 * 20000 ≈ 1968µs
MIN_SERVO_US   = 1000   # µs → ~0°
MAX_SERVO_US   = 2440   # µs → ~180°
OPEN_POSITION  = 2440   # µs (equivalent a 8000 ESP32)
EJECT_POSITION = 1968   # µs (equivalent a 6450 ESP32)
_INCREMENT_US  = 3      # µs per iteració de 1ms (equivalent a increment de 10 ESP32)

# ==================
# Instàncies globals
# ==================

_pi: pigpio.pi           = None
_color_sensor            = None
_current_servo_pos: int  = OPEN_POSITION


# ==================
# Setup
# ==================

def blocks_setup(pi: pigpio.pi):
    """
    Inicialitza el servo i el sensor de color TCS34725.
    El servo arranca en OPEN_POSITION.
    """
    global _pi, _color_sensor, _current_servo_pos

    _pi = pi

    # Servo
    pi.set_mode(SERVO_PWM, pigpio.OUTPUT)
    _current_servo_pos = OPEN_POSITION
    pi.set_servo_pulsewidth(SERVO_PWM, _current_servo_pos)

    # Sensor de color TCS34725 via bus I2C 4 (bit-bang GPIO22=SDA, GPIO27=SCL)
    i2c = adafruit_extended_bus.ExtendedI2C(4)
    _color_sensor = adafruit_tcs34725.TCS34725(i2c)
    _color_sensor.integration_time = 50    # ms
    _color_sensor.gain = 4                 # 4x (equivalent a TCS34725::Gain::X04)


# ==================
# Servo
# ==================

def servo_move_to(target_us: int):
    """
    Mou el servo fins a target_us de forma suau.
    Incrementa/decrementa _INCREMENT_US cada mil·lisegon.
    """
    global _current_servo_pos

    if not (MIN_SERVO_US <= target_us <= MAX_SERVO_US):
        return

    while True:
        if _current_servo_pos < target_us - _INCREMENT_US:
            _current_servo_pos += _INCREMENT_US
        elif _current_servo_pos > target_us + _INCREMENT_US:
            _current_servo_pos -= _INCREMENT_US
        else:
            _current_servo_pos = target_us

        _pi.set_servo_pulsewidth(SERVO_PWM, _current_servo_pos)
        time.sleep(0.001)

        if _current_servo_pos == target_us:
            return


# ==================
# Sensor de color
# ==================

def _calc_colors_difference(measured: tuple, reference: dict) -> int:
    """Distància Manhattan entre el color mesurat i un color de referència."""
    return (abs(measured[0] - reference["r"]) +
            abs(measured[1] - reference["g"]) +
            abs(measured[2] - reference["b"]))


def read_color_raw() -> tuple:
    """Retorna (r, g, b) en bytes 0-255 sense classificació. Útil per calibrar."""
    try:
        return _color_sensor.color_rgb_bytes
    except Exception:
        return (0, 0, 0)


def read_block_color() -> int:
    """
    Llegeix el color del bloc des del sensor TCS34725.
    Compara contra la taula de referència per distància Manhattan.
    Retorna l'ID del color més proper, o BK si cap supera el llindar.
    """
    MAX_DIFFERENCE = 15

    try:
        r, g, b = _color_sensor.color_rgb_bytes
    except Exception:
        return BK

    min_difference   = MAX_DIFFERENCE
    min_diff_color   = BK

    for color_id, ref in enumerate(_COLORS):
        diff = _calc_colors_difference((r, g, b), ref)
        if diff < min_difference:
            min_difference = diff
            min_diff_color = color_id

    return min_diff_color