"""Binary protocol encoder/decoder matching STM32 debug_protocol.h"""
import struct
from dataclasses import dataclass, field
from typing import Optional

SYNC_BYTE = 0xAA
HEADER_SIZE = 3

CMD_TELEMETRY = 0x01
CMD_PARAM_WRITE = 0x02
CMD_PARAM_WRITE_ACK = 0x03
CMD_PARAM_READ_ALL = 0x04
CMD_PARAM_VALUE = 0x05
CMD_PING = 0x10
CMD_PONG = 0x11
CMD_SHUTDOWN = 0x12
CMD_RESET = 0x13
CMD_TEST_50 = 0x14
CMD_RELAY_ON = 0x15
CMD_RELAY_OFF = 0x16
CMD_HOLD_CONVERTER = 0x17
CMD_TOGGLE_PRECHARGE = 0x18
CMD_ERROR_MSG = 0xE0

PTYPE_FLOAT = 0
PTYPE_UINT16 = 1
PTYPE_UINT8 = 2
PTYPE_INT32 = 3
PTYPE_INT16 = 4  # wire format = sign-extended int32, stored firmware-side as int16_t

# CRC-16/CCITT-FALSE: poly 0x1021, init 0xFFFF, no reflection, no XOR-out.
# Matches STM32 hardware CRC configured in main.c MX_CRC_Init.
def crc16(data: bytes) -> int:
    crc = 0xFFFF
    for b in data:
        crc ^= b << 8
        for _ in range(8):
            if crc & 0x8000:
                crc = ((crc << 1) ^ 0x1021) & 0xFFFF
            else:
                crc = (crc << 1) & 0xFFFF
    return crc


@dataclass
class TelemetryData:
    vin: float = 0.0
    vout: float = 0.0
    iin: float = 0.0
    iout: float = 0.0
    vfly: float = 0.0
    etemp: float = 0.0
    btemp: float = 0.0
    last_tmp: int = 0
    VREF: int = 0
    vfly_correction: int = 0
    cmp_outer: int = 0       # HRTIM Timer F CMP1xR (outer pair, T1/T4)
    vfly_integral: float = 0.0
    vfly_avg_debug: float = 0.0
    cc_output_f: float = 0.0
    mppt_iref: float = 0.0
    mppt_last_vin: float = 0.0
    mppt_last_iin: float = 0.0
    p_in: float = 0.0
    p_out: float = 0.0
    iout_slow: float = 0.0
    seq: int = 0
    cmp_inner: int = 0       # HRTIM Timer E CMP1xR (inner pair, T2/T3)
    vfly_ofs_applied: int = 0  # master-phase offset last written, signed ticks

TELEMETRY_FMT = "<7f hHhH 6f 3f BxH h"  # 78 bytes
TELEMETRY_SIZE = struct.calcsize(TELEMETRY_FMT)

def decode_telemetry(payload: bytes) -> Optional[TelemetryData]:
    if len(payload) < TELEMETRY_SIZE:
        return None
    vals = struct.unpack(TELEMETRY_FMT, payload[:TELEMETRY_SIZE])
    return TelemetryData(
        vin=vals[0], vout=vals[1], iin=vals[2], iout=vals[3],
        vfly=vals[4], etemp=vals[5], btemp=vals[6],
        last_tmp=vals[7], VREF=vals[8], vfly_correction=vals[9],
        cmp_outer=vals[10],
        vfly_integral=vals[11], vfly_avg_debug=vals[12],
        cc_output_f=vals[13], mppt_iref=vals[14],
        mppt_last_vin=vals[15], mppt_last_iin=vals[16],
        p_in=vals[17], p_out=vals[18],
        iout_slow=vals[19],
        seq=vals[20],
        cmp_inner=vals[21],
        vfly_ofs_applied=vals[22],
    )


def build_frame(cmd: int, payload: bytes = b"") -> bytes:
    header = bytes([SYNC_BYTE, cmd, len(payload)])
    frame_no_crc = header + payload
    crc = crc16(frame_no_crc)
    return frame_no_crc + bytes([(crc >> 8) & 0xFF, crc & 0xFF])  # big-endian


def build_param_write(param_id: int, param_type: int, value) -> bytes:
    if param_type == PTYPE_FLOAT:
        val_bytes = struct.pack("<f", float(value))
    elif param_type == PTYPE_UINT16:
        val_bytes = struct.pack("<HH", int(value), 0)  # pad to 4 bytes
    elif param_type == PTYPE_UINT8:
        val_bytes = struct.pack("<Bxxx", int(value))
    elif param_type == PTYPE_INT32:
        val_bytes = struct.pack("<i", int(value))
    elif param_type == PTYPE_INT16:
        val_bytes = struct.pack("<i", int(value))  # sign-extended 32-bit wire
    else:
        val_bytes = struct.pack("<I", int(value))
    payload = struct.pack("<BBxx", param_id, param_type) + val_bytes
    return build_frame(CMD_PARAM_WRITE, payload)


def build_ping() -> bytes:
    return build_frame(CMD_PING)


def build_shutdown() -> bytes:
    return build_frame(CMD_SHUTDOWN)


def build_reset() -> bytes:
    return build_frame(CMD_RESET)


def build_test_50() -> bytes:
    return build_frame(CMD_TEST_50)


def build_relay_on() -> bytes:
    return build_frame(CMD_RELAY_ON)


def build_relay_off() -> bytes:
    return build_frame(CMD_RELAY_OFF)


def build_hold_converter() -> bytes:
    return build_frame(CMD_HOLD_CONVERTER)


def build_toggle_precharge() -> bytes:
    return build_frame(CMD_TOGGLE_PRECHARGE)


def build_param_read_all() -> bytes:
    return build_frame(CMD_PARAM_READ_ALL)


def decode_param_value(payload: bytes) -> Optional[tuple[int, float]]:
    """Decode a PARAM_VALUE payload. Returns (param_id, value) or None."""
    if len(payload) < 8:
        return None
    param_id = payload[0]
    param_type = payload[1]
    value_bytes = payload[4:8]
    if param_type == PTYPE_FLOAT:
        value = struct.unpack("<f", value_bytes)[0]
    elif param_type == PTYPE_UINT16:
        value = float(struct.unpack("<H", value_bytes[:2])[0])
    elif param_type == PTYPE_UINT8:
        value = float(value_bytes[0])
    elif param_type == PTYPE_INT32:
        value = float(struct.unpack("<i", value_bytes)[0])
    elif param_type == PTYPE_INT16:
        value = float(struct.unpack("<i", value_bytes)[0])  # sign-extended 32-bit wire
    else:
        value = float(struct.unpack("<I", value_bytes)[0])
    return (param_id, value)


class FrameParser:
    """State machine parser for incoming frames."""

    WAIT_SYNC = 0
    WAIT_CMD = 1
    WAIT_LEN = 2
    WAIT_PAYLOAD = 3
    WAIT_CRC_HI = 4
    WAIT_CRC_LO = 5

    def __init__(self):
        self.state = self.WAIT_SYNC
        self.cmd = 0
        self.length = 0
        self.buf = bytearray()
        self.payload = bytearray()
        self.idx = 0
        self.crc_hi = 0

    def feed(self, data: bytes):
        """Feed bytes, yield (cmd, payload) tuples for complete frames."""
        for b in data:
            if self.state == self.WAIT_SYNC:
                if b == SYNC_BYTE:
                    self.buf = bytearray([b])
                    self.state = self.WAIT_CMD
            elif self.state == self.WAIT_CMD:
                self.cmd = b
                self.buf.append(b)
                self.state = self.WAIT_LEN
            elif self.state == self.WAIT_LEN:
                self.length = b
                self.buf.append(b)
                self.payload = bytearray()
                self.idx = 0
                if b == 0:
                    self.state = self.WAIT_CRC_HI
                elif b > 128:
                    self.state = self.WAIT_SYNC
                else:
                    self.state = self.WAIT_PAYLOAD
            elif self.state == self.WAIT_PAYLOAD:
                self.payload.append(b)
                self.buf.append(b)
                self.idx += 1
                if self.idx >= self.length:
                    self.state = self.WAIT_CRC_HI
            elif self.state == self.WAIT_CRC_HI:
                self.crc_hi = b
                self.state = self.WAIT_CRC_LO
            elif self.state == self.WAIT_CRC_LO:
                received = (self.crc_hi << 8) | b
                expected = crc16(bytes(self.buf))
                self.state = self.WAIT_SYNC
                if received == expected:
                    yield (self.cmd, bytes(self.payload))


# Parameter registry
@dataclass
class ParamDef:
    id: int
    name: str
    ptype: int
    group: str
    min_val: float = -1e9
    max_val: float = 1e9
    fmt: str = ".4f"

PARAMS = [
    # Compensator
    ParamDef(0x25, "VREF",       PTYPE_UINT16, "Compensator", 2340, 3500, ".0f"),
    # Vfly
    ParamDef(0x20, "vfly_kp",    PTYPE_FLOAT,  "Vfly", -10, 10, ".4f"),
    ParamDef(0x21, "vfly_ki",    PTYPE_FLOAT,  "Vfly", -10, 10, ".6f"),
    ParamDef(0x62, "vfly_kp_phase", PTYPE_FLOAT, "Vfly", -10, 10, ".4f"),  # mode 2: P gain, error -> phase
    ParamDef(0x63, "vfly_phase_clamp", PTYPE_UINT16, "Vfly", 0, 10000, ".0f"),  # mode 2: phase offset clamp
    ParamDef(0x22, "vfly_clamp", PTYPE_UINT16, "Vfly", 0, 10000, ".0f"),
    ParamDef(0x23, "vfly_loop_trig", PTYPE_UINT16, "Vfly", 1, 10000, ".0f"),
    ParamDef(0x24, "vfly_active",    PTYPE_UINT8,  "Vfly", 0, 3, ".0f"),  # 0=off 1=auto duty-asymmetry 2=auto phase offset 3=manual both
    ParamDef(0x26, "test_corr",      PTYPE_INT16,  "Vfly", -3000, 3000, ".0f"),  # mode-3 manual duty asymmetry
    ParamDef(0x27, "phase_ofs",      PTYPE_INT16,  "Vfly", -3000, 3000, ".0f"),  # master-phase: mode-3 manual, mode-2 P-driven (readback)
    # CC
    ParamDef(0x30, "cc_target",      PTYPE_FLOAT,  "CC", 0, 60000, ".0f"),
    ParamDef(0x31, "cc_gain",        PTYPE_FLOAT,  "CC", -1, 1, ".4f"),
    ParamDef(0x32, "cc_min_step",    PTYPE_FLOAT,  "CC", -1000, 0, ".1f"),
    ParamDef(0x33, "cc_max_step",    PTYPE_FLOAT,  "CC", 0, 1000, ".1f"),
    ParamDef(0x34, "cc_loop_trig",   PTYPE_UINT16, "CC", 1, 10000, ".0f"),
    ParamDef(0x35, "cc_active",      PTYPE_INT32,  "CC", 0, 1, ".0f"),
    # MPPT
    ParamDef(0x40, "mppt_step",      PTYPE_FLOAT,  "MPPT", 1, 200, ".0f"),
    ParamDef(0x41, "mppt_duty_min",  PTYPE_FLOAT,  "MPPT", 0, 6800, ".0f"),
    ParamDef(0x42, "mppt_duty_max",  PTYPE_FLOAT,  "MPPT", 0, 6800, ".0f"),
    ParamDef(0x44, "mppt_loop_trig", PTYPE_UINT16, "MPPT", 1, 50000, ".0f"),
    ParamDef(0x45, "mppt_active",    PTYPE_INT32,  "MPPT", 0, 1, ".0f"),
    ParamDef(0x46, "cv_threshold",   PTYPE_FLOAT,  "MPPT", 20000, 30000, ".0f"),
    ParamDef(0x47, "cv_hysteresis",  PTYPE_FLOAT,  "MPPT", 0, 5000, ".0f"),
    ParamDef(0x48, "cc_threshold",   PTYPE_FLOAT,  "MPPT", 0, 55000, ".0f"),
    ParamDef(0x50, "cc_hysteresis",  PTYPE_FLOAT,  "MPPT", 0, 10000, ".0f"),
    # Deadtime (single static value, dt register units)
    ParamDef(0x60, "dt_normal",     PTYPE_UINT16, "Deadtime", 14, 200, ".0f"),
    # Manual fixed-duty mode (base duty in CMP ticks; D = override_duty/7158, 716..6442 = 10..90%)
    ParamDef(0x64, "override_duty",  PTYPE_UINT16, "Manual", 716, 6442, ".0f"),  # enable manual_duty_en first (seeds at current duty), then sweep
    ParamDef(0x65, "manual_duty_en", PTYPE_UINT8,  "Manual", 0, 1, ".0f"),       # 1 = fixed duty, supervisor parked; FMAC balancer still runs per vfly_active
    # Duty dither: avoid the V_fly de-stack band by delta-sigma modulating the commanded
    # duty between two out-of-band anchors in the FMAC ISR (all CMP ticks). Defaults bracket
    # the Vin=75 collapse band 3686..3801 (D 0.515..0.531) with anchors 3643/3844 (D 0.509/0.537).
    # Closed-loop precharge: PI controller drives the precharge FET PWM (TIM3_CH1) to Vin/2.
    ParamDef(0x76, "precharge_kp",     PTYPE_FLOAT, "Precharge", 0, 100, ".3f"),  # P: CCR ticks per mV of (Vin/2 - Vfly)
    ParamDef(0x78, "precharge_ki",     PTYPE_FLOAT, "Precharge", 0, 10, ".4f"),   # I: CCR ticks per mV per ~1 kHz tick
    ParamDef(0x77, "precharge_reg_en", PTYPE_UINT8, "Precharge", 0, 1, ".0f"),    # 1 = closed-loop precharge on
    ParamDef(0x70, "dither_en",      PTYPE_UINT8,  "Dither", 0, 1, ".0f"),
    ParamDef(0x71, "dither_band_lo", PTYPE_UINT16, "Dither", 716, 6442, ".0f"),
    ParamDef(0x72, "dither_band_hi", PTYPE_UINT16, "Dither", 716, 6442, ".0f"),
    ParamDef(0x73, "dither_anear",   PTYPE_UINT16, "Dither", 716, 6442, ".0f"),
    ParamDef(0x74, "dither_afar",    PTYPE_UINT16, "Dither", 716, 6442, ".0f"),
    ParamDef(0x75, "dither_dzero",   PTYPE_UINT16, "Dither", 716, 6442, ".0f"),  # |e| fold center (D=0.5); covers both lobes
]

PARAM_BY_ID = {p.id: p for p in PARAMS}
PARAM_BY_NAME = {p.name: p for p in PARAMS}