"""
api_layers/protocols/cml.py

CoolMuscle Language (CML) protocol layer.

For CoolMuscle CM-series servo motors over RS-232 (single axis)
or RS-485 (multi-drop, up to 31 axes).

Frame format
─────────────
  RS-232 (address=0):
    Command:   <CMD>[<data>]\\r
    Response:  <CMD>[<data>]\\r        (echo-back)

  RS-485 (address 1-31):
    Command:   #<addr><CMD>[<data>]\\r
    Response:  *<addr><CMD>[<data>]\\r

Read commands (query current state):
  TP  — Tell absolute position  (encoder counts, signed)
  TV  — Tell velocity           (counts/second, signed)
  TC  — Tell current            (% rated × 10, unsigned)
  TS  — Tell status word        (hex flags)

Write commands (control):
  ME  — Motor Enable
  MD  — Motor Disable
  MA<value>  — Move Absolute (encoder counts)
  MR<value>  — Move Relative (encoder counts)
  VS<value>  — Velocity Setpoint

Channel IDs follow pattern:  <motor_id>_<CMD>
  e.g. "M1_TP", "M1_TV", "M2_TC"
"""

import math
import re
from dataclasses import dataclass, field
from typing import Dict, List, Optional

from api_layers.protocols.base_protocol import BaseProtocol


_READ_CMDS  = ["TP", "TV", "TC", "TS"]
_RESP_RE    = re.compile(r"[*@]?\d*([A-Z]{2})([\s\S]*)")
_NUM_RE     = re.compile(r"[+-]?\d+")


@dataclass
class CMLMotor:
    motor_id:  str
    address:   int        = 1          # 0 = RS-232 (no address prefix)
    read_cmds: List[str]  = field(default_factory=lambda: ["TP", "TV", "TC"])


class CMLLayer(BaseProtocol):

    def __init__(
        self,
        port:          str,
        baud:          int            = 38400,
        motors:        List[CMLMotor] = None,
        poll_interval: float          = 0.1,
        simulate:      bool           = True,
    ):
        super().__init__(port, baud, poll_interval, simulate)
        self.motors = motors or [CMLMotor("M1", address=1)]

    # ── Protocol ──────────────────────────────────────────────────────────

    def _send_query(self, motor: CMLMotor, cmd: str) -> Optional[float]:
        if motor.address == 0:
            frame = f"{cmd}\r".encode()
        else:
            frame = f"#{motor.address}{cmd}\r".encode()
        self._ser.write(frame)
        self._ser.flush()
        resp = self._ser.readline().decode(errors="replace").strip()
        return _parse_cml_response(resp)

    def _poll(self) -> Dict[str, float]:
        result: Dict[str, float] = {}
        for motor in self.motors:
            for cmd in motor.read_cmds:
                try:
                    val = self._send_query(motor, cmd)
                    if val is not None:
                        result[f"{motor.motor_id}_{cmd}"] = val
                except Exception:
                    pass
        return result

    def _simulate(self, t: float) -> Dict[str, float]:
        result: Dict[str, float] = {}
        for i, motor in enumerate(self.motors):
            phase = i * 1.0
            for cmd in motor.read_cmds:
                key = f"{motor.motor_id}_{cmd}"
                if cmd == "TP":
                    val = math.sin(t * 0.2 + phase) * 10000.0
                elif cmd == "TV":
                    val = math.cos(t * 0.2 + phase) * 2000.0
                elif cmd == "TC":
                    val = abs(math.sin(t * 0.4 + phase)) * 500.0
                elif cmd == "TS":
                    val = 0.0
                else:
                    val = math.sin(t + i) * 100.0
                result[key] = val
        return result

    def write(self, channel_id: str, value) -> bool:
        # channel_id: "<motor_id>_<CMD>[<data>]"
        # e.g. "M1_ME", "M1_MA" (value carries position)
        parts = channel_id.split("_", 1)
        if len(parts) != 2:
            return False
        motor_id, cmd = parts
        motor = next((m for m in self.motors if m.motor_id == motor_id), None)
        if motor is None:
            return False
        try:
            data = "" if cmd in ("ME", "MD") else str(int(value))
            if motor.address == 0:
                frame = f"{cmd}{data}\r".encode()
            else:
                frame = f"#{motor.address}{cmd}{data}\r".encode()
            self._ser.write(frame)
            return True
        except Exception:
            return False


def _parse_cml_response(resp: str) -> Optional[float]:
    """Extract numeric value from CML response like '*1TP+001234'."""
    m = _NUM_RE.search(resp[3:] if resp and resp[0] in "*@#" else resp)
    if m:
        return float(m.group())
    return None