"""VESC CAN protocol constants and frame parsers (FW 6.6).

VESC uses 29-bit extended CAN IDs:
  arbitration_id = (packet_type << 8) | vesc_can_id

Status frames are broadcast by the VESC at configured intervals.
GET_VALUES requests use CAN_PACKET_PROCESS_SHORT_BUFFER.
"""

import struct
import time
from dataclasses import dataclass

# ---------------------------------------------------------------------------
# Packet type IDs (upper bits of 29-bit extended arb ID)
# ---------------------------------------------------------------------------
CAN_PACKET_STATUS           = 9    # RPM, phase current, duty cycle
CAN_PACKET_STATUS_2         = 14   # Ah used / Ah charged
CAN_PACKET_STATUS_3         = 15   # Wh used / Wh charged
CAN_PACKET_STATUS_4         = 16   # FET temp, motor temp, input current
CAN_PACKET_STATUS_5         = 27   # tachometer, input voltage
CAN_PACKET_PROCESS_SHORT_BUFFER = 32  # send a command to the VESC (≤3-byte payload)

# VESC UART command IDs (embedded inside short-buffer requests)
COMM_GET_VALUES = 4

# ---------------------------------------------------------------------------
# Fault codes (FW 6.6)
# ---------------------------------------------------------------------------
FAULT_CODE_NONE                     = 0
FAULT_CODE_OVER_VOLTAGE             = 1
FAULT_CODE_UNDER_VOLTAGE            = 2
FAULT_CODE_DRV                      = 3
FAULT_CODE_ABS_OVER_CURRENT         = 4
FAULT_CODE_OVER_TEMP_FET            = 5
FAULT_CODE_OVER_TEMP_MOTOR          = 6
FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE = 7
FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE = 8
FAULT_CODE_MCU_UNDER_VOLTAGE        = 9
FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET = 10

FAULT_NAMES: dict[int, str] = {
    0:  "NONE",
    1:  "OVER_VOLTAGE",
    2:  "UNDER_VOLTAGE",
    3:  "DRV",
    4:  "ABS_OVER_CURRENT",
    5:  "OVER_TEMP_FET",
    6:  "OVER_TEMP_MOTOR",
    7:  "GATE_DRIVER_OVER_VOLTAGE",
    8:  "GATE_DRIVER_UNDER_VOLTAGE",
    9:  "MCU_UNDER_VOLTAGE",
    10: "BOOTING_FROM_WATCHDOG_RESET",
}


# ---------------------------------------------------------------------------
# State dataclass
# ---------------------------------------------------------------------------
@dataclass
class VescState:
    """Telemetry snapshot for one VESC, populated from status broadcast frames."""

    can_id: int = 0

    # From STATUS (9)
    rpm: int = 0
    current_a: float = 0.0      # phase current (A)
    duty_cycle: float = 0.0     # -1.0 .. +1.0

    # From STATUS_4 (16)
    temp_fet_c: float = 0.0
    temp_motor_c: float = 0.0
    current_in_a: float = 0.0   # battery/input current (A)

    # From STATUS_5 (27)
    voltage_v: float = 0.0

    # Fault (embedded in STATUS per FW 6.6 — lower byte of duty word)
    fault_code: int = 0

    # Timestamps (monotonic seconds)
    last_status_ts: float = 0.0
    last_status5_ts: float = 0.0

    ALIVE_TIMEOUT_S: float = 1.0  # mark offline if no STATUS frame for this long

    @property
    def fault_name(self) -> str:
        return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")

    @property
    def is_alive(self) -> bool:
        return (time.monotonic() - self.last_status_ts) < self.ALIVE_TIMEOUT_S

    @property
    def has_fault(self) -> bool:
        return self.fault_code != FAULT_CODE_NONE


# ---------------------------------------------------------------------------
# Frame parsers
# ---------------------------------------------------------------------------

def parse_status(data: bytes) -> dict | None:
    """Parse CAN_PACKET_STATUS (9): RPM, phase current, duty cycle.

    Wire format (8 bytes, all big-endian):
      bytes [0:4]  RPM          : int32
      bytes [4:6]  current×10   : int16  → ÷10 = A
      bytes [6:8]  duty×1000    : int16  → ÷1000 = fraction (-1..+1)
    """
    if len(data) < 8:
        return None
    rpm, current_x10, duty_x1000 = struct.unpack_from(">ihh", data, 0)
    return {
        "rpm":        rpm,
        "current_a":  current_x10 / 10.0,
        "duty_cycle": duty_x1000 / 1000.0,
    }


def parse_status_4(data: bytes) -> dict | None:
    """Parse CAN_PACKET_STATUS_4 (16): temperatures and input current.

    Wire format (8 bytes, all big-endian):
      bytes [0:2]  temp_fet×10    : int16  → ÷10 = °C
      bytes [2:4]  temp_motor×10  : int16  → ÷10 = °C
      bytes [4:6]  current_in×10  : int16  → ÷10 = A
      bytes [6:8]  pid_pos×50     : int16  (not used here)
    """
    if len(data) < 8:
        return None
    tf_x10, tm_x10, ci_x10, _pid = struct.unpack_from(">hhhh", data, 0)
    return {
        "temp_fet_c":   tf_x10 / 10.0,
        "temp_motor_c": tm_x10 / 10.0,
        "current_in_a": ci_x10 / 10.0,
    }


def parse_status_5(data: bytes) -> dict | None:
    """Parse CAN_PACKET_STATUS_5 (27): input voltage (and tachometer).

    Wire format (8 bytes, all big-endian):
      bytes [0:4]  tachometer  : int32  (unused here)
      bytes [4:6]  v_in×10     : int16  → ÷10 = V
      bytes [6:8]  reserved
    """
    if len(data) < 6:
        return None
    _tacho, v_in_x10 = struct.unpack_from(">ih", data, 0)
    return {
        "voltage_v": v_in_x10 / 10.0,
    }


# ---------------------------------------------------------------------------
# Request builder
# ---------------------------------------------------------------------------

def make_get_values_request(sender_id: int, target_id: int) -> tuple[int, bytes]:
    """Build a CAN_PACKET_PROCESS_SHORT_BUFFER GET_VALUES request frame.

    The VESC responds by refreshing its STATUS broadcast cadence.

    Args:
        sender_id: This node's CAN ID (0-127; use 127 for a host controller).
        target_id: CAN ID of the target VESC.

    Returns:
        (arbitration_id, data) — pass directly to can.Message().
    """
    arb_id = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | (target_id & 0xFF)
    payload = bytes([sender_id & 0xFF, 0x00, COMM_GET_VALUES])
    return arb_id, payload
