from machine import Pin, PWM
from micropython import const
from time import sleep_ms
import _thread
import struct

# Constantes
VEL_RANGE = (-20000, 20000)  # mm/s
TEMP_RANGE = (0, 3000)       # centésimas de °C
FREQUENCY_PWM = const(10000)
LED_PIN = const(25)
BLINK_ON_MS = const(100)

# PWM setup (D0 = velocidad, D1 = temperatura)
velocity_pwm = PWM(Pin(0))
velocity_pwm.freq(FREQUENCY_PWM)
temperature_pwm = PWM(Pin(1))
temperature_pwm.freq(FREQUENCY_PWM)

def map_to_16bits(value: float, range_: tuple) -> int:
    mapped = int(((value - range_[0]) / (range_[1] - range_[0])) * 65535 + 0.5)
    return max(0, min(mapped, 65535))

def blink(gpio: int, time_ms: int) -> None:
    Pin(gpio, Pin.OUT, value=1)
    sleep_ms(time_ms)
    Pin(gpio, Pin.OUT, value=0)

def find_ensemble(buffer: bytearray) -> int:
    for i in range(len(buffer) - 1):
        if buffer[i] == 0x7F and buffer[i+1] == 0x7F:
            return i
    return -1

def parse_pd0_ensemble(data: bytes) -> tuple | None:
    try:
        n_types = data[5]
        offsets = []
        for i in range(n_types):
            offset = struct.unpack_from('<H', data, 6 + 2*i)[0]
            offsets.append(offset)

        var_leader_offset = None
        velocity_offset = None

        for off in offsets:
            block_id = struct.unpack_from('<H', data, off)[0]
            if block_id == 0x8000:
                var_leader_offset = off
            elif block_id == 0x0001:
                velocity_offset = off

        if var_leader_offset is None or velocity_offset is None:
            return None

        temp_raw = struct.unpack_from('<H', data, var_leader_offset + 26)[0]
        temperature = temp_raw / 100.0

        vel_raw = struct.unpack_from('<h', data, velocity_offset + 2)[0]
        velocity = abs(vel_raw)

        return temperature, velocity

    except Exception as e:
        print("Error parsing ensemble:", e)
        return None

def generate_fake_pd0():
    data = bytearray(600)
    data[0:2] = b'\x7F\x7F'
    data[5] = 2  # Dos tipos de datos

    # Offsets
    var_leader_offset = 50
    velocity_offset = 100
    struct.pack_into('<H', data, 6, var_leader_offset)
    struct.pack_into('<H', data, 8, velocity_offset)

    # Bloques
    struct.pack_into('<H', data, var_leader_offset, 0x8000)
    struct.pack_into('<H', data, velocity_offset, 0x0001)

    # Temperatura (15.00°C = 1500 centésimas)
    struct.pack_into('<H', data, var_leader_offset + 26, 1500)

    # Velocidad (1000 mm/s)
    struct.pack_into('<h', data, velocity_offset + 2, 1000)

    return data

# Loop principal
while True:
    fake_data = generate_fake_pd0()
    result = parse_pd0_ensemble(fake_data)

    if result:
        temperature, velocity = result

        velocity_u16 = map_to_16bits(velocity, VEL_RANGE)
        temperature_u16 = map_to_16bits(temperature * 100, TEMP_RANGE)

        velocity_pwm.duty_u16(velocity_u16)
        temperature_pwm.duty_u16(temperature_u16)

        duty_velocity = (velocity_u16 / 65535) * 100
        duty_temperature = (temperature_u16 / 65535) * 100

        print(f"""
Velocidad:
  mm/s: {velocity:.2f}
  PWM D0: {duty_velocity:.2f} %

Temperatura:
  °C: {temperature:.2f}
  PWM D1: {duty_temperature:.2f} %
------------------------------""")

        _thread.start_new_thread(blink, (LED_PIN, BLINK_ON_MS))

    sleep_ms(1000)