path: root/api_layers/arduino_layer.py

"""
api_layers/arduino_layer.py

Arduino serial communication layer.

═══════════════════════════════════════════════════════════════
PROTOCOL SPECIFICATION  (v1.1)
═══════════════════════════════════════════════════════════════

PC → Arduino  (commands, newline-terminated):
─────────────────────────────────────────────
  W:<pin>:<0|1>          Digital write
                           W:D13:1   → digitalWrite(13, HIGH)
                           W:D6:0    → digitalWrite(6,  LOW)

  P:<pin>:<0-255>        PWM (analogWrite)
                           P:D9:128  → analogWrite(9, 128)  ~50% duty

  V:<pin>:<voltage>      Analog voltage out (DAC, 0.0–5.0)
                           V:DAC0:2.5 → set DAC channel 0 to 2.5 V

  S:<id>:<angle>         Servo position (0–180 degrees)
                           S:SERVO0:90  → center servo 0

  C:<name>:<value>       Named setpoint / parameter
                           C:SETPOINT:75.0
                           C:KP:1.2
                           C:MODE:1

  DPIN:IN:<pins>         Configure digital input pins (CSV pin numbers, no 'D' prefix)
                           DPIN:IN:2,3,8  → set DI pins to 2,3,8; sets N_DIG_IN=3

  DPIN:OUT:<pins>        Configure digital output pins (CSV pin numbers, no 'D' prefix)
                           DPIN:OUT:5,6,13  → set DO pins to 5,6,13; sets N_DIG_OUT=3

  APIN:<indices>         Configure analog input pins (CSV indices 0–5 into A0–A5)
                           APIN:0,1,3  → read A0,A1,A3; sets N_ANALOG=3
                           Output labels use actual indices: A0:val,A1:val,A3:val

  Q:<name>               Request current reading by name (Arduino replies immediately)
                           Q:TEMP  → Arduino sends  TEMP:23.45\n

  R:ALL                  Request full data frame immediately (don't wait for interval)

  X:STOP                 Emergency stop — disable all outputs
  X:RESET                Reset all outputs to default state

Arduino → PC  (data, newline-terminated):
─────────────────────────────────────────
  <key>:<value>[,<key>:<value>...]\n    Continuous stream
    A0:3.142,A1:0.015,D2:1,TEMP:23.4\n

  ACK:<command>\n        Acknowledgement after command executed
    ACK:W:D13:1\n
    ACK:C:SETPOINT:75.0\n

  ERR:<message>\n        Error response
    ERR:Unknown command\n

═══════════════════════════════════════════════════════════════
ARDUINO FIRMWARE  (copy into Arduino IDE)
═══════════════════════════════════════════════════════════════

See ARDUINO_FIRMWARE string at the bottom of this file.
Upload it to your board, set baud to 115200.
"""

import math
import random
import json
import threading
import time
from typing import Any, Callable, Dict, List, Optional, Tuple


try:
    import serial                      # type: ignore
    import serial.tools.list_ports    # type: ignore
    _SERIAL_AVAILABLE = True
except ImportError:
    _SERIAL_AVAILABLE = False


class ArduinoLayer:

    DEFAULT_ANALOG_PINS  = ["A0", "A1", "A2", "A3", "A4", "A5"]
    DEFAULT_DIGITAL_PINS = ["D2", "D3", "D4", "D5", "D6", "D7"]
    # Firmware DIGITAL_IN[] and DIGITAL_OUT[] arrays — match arduino_layer.py firmware sketch
    DEFAULT_DI_PINS      = ["D2", "D3", "D4"]
    DEFAULT_DO_PINS      = ["D5", "D6", "D7", "D9", "D10", "D11"]

    def __init__(
        self,
        port:         str       = "COM3",
        baud:         int       = 115200,
        timeout:      float     = 1.0,
        analog_pins:  List[str] = None,
        digital_pins: List[str] = None,
        simulate:     bool      = True,
    ):
        self.port         = port
        self.baud         = baud
        self.timeout      = timeout
        self.analog_pins  = analog_pins  or self.DEFAULT_ANALOG_PINS
        self.digital_pins = digital_pins or []
        self.simulate     = simulate

        self._ser:        Optional[object]             = None
        self._cache:      Dict[str, float]             = {}
        self._ack_cache:  Dict[str, str]               = {}   # last ACK per command type
        self._lock        = threading.Lock()
        self._running     = False
        self._thread:     Optional[threading.Thread]   = None
        self._t0          = 0.0
        self._last_error  = ""

        # Callbacks: registered by higher layers to receive ACK/ERR/data
        self._ack_callbacks:  List[Callable] = []   # fn(command: str)
        self._err_callbacks:  List[Callable] = []   # fn(message: str)

        self._sim_params = {
            pin: {
                "freq":   0.1 + i * 0.13,
                "amp":    2.3,   # headroom below the [0,5] clamp so noise doesn't flat-clip every peak
                "offset": 2.5,
                "noise":  0.01,
                "phase":  i * 1.1,
            }
            for i, pin in enumerate(self.analog_pins)
        }
        # Simulated output state
        self._sim_outputs: Dict[str, Any] = {}

    # ── Lifecycle ─────────────────────────────────────────────────────────

    def connect(self) -> bool:
        self._t0 = time.time()
        self._last_error = ""

        if self.simulate:
            self._running = True
            self._thread  = threading.Thread(
                target=self._sim_loop, daemon=True, name="ArduinoSim"
            )
            self._thread.start()
            return True

        if not _SERIAL_AVAILABLE:
            self._last_error = "pyserial not installed — run: pip install pyserial"
            print(f"[ArduinoLayer] {self._last_error}")
            return False

        try:
            self._ser = serial.Serial()
            self._ser.port     = self.port
            self._ser.baudrate = self.baud
            self._ser.timeout  = self.timeout
            self._ser.open()
        except Exception as e:
            self._last_error = str(e)
            print(f"[ArduinoLayer] connect() failed on {self.port}: {e}")
            self._ser = None
            return False

        self._running = True
        self._thread  = threading.Thread(
            target=self._read_loop, daemon=True, name=f"Arduino-{self.port}"
        )
        self._thread.start()
        return True

    def disconnect(self) -> None:
        self._running = False
        if self._thread:
            self._thread.join(timeout=2.0)
            self._thread = None
        if self._ser:
            try:
                self._ser.close()
            except Exception:
                pass
            self._ser = None
        with self._lock:
            self._cache.clear()

    @property
    def is_connected(self) -> bool:
        if self.simulate:
            return self._running
        return self._ser is not None and self._ser.is_open

    @property
    def last_error(self) -> str:
        return self._last_error

    # ── READ ──────────────────────────────────────────────────────────────

    def read(self) -> Dict[str, float]:
        """Return latest cached values for all channels."""
        with self._lock:
            return dict(self._cache)

    # ── WRITE COMMANDS ────────────────────────────────────────────────────

    def digital_write(self, pin: str, value: int) -> bool:
        """
        Set a digital pin HIGH or LOW.
          pin:   "D13", "D6", etc.
          value: 1 = HIGH, 0 = LOW
        Sends:  W:D13:1\n
        """
        return self._send(f"W:{pin}:{int(bool(value))}")

    def pwm_write(self, pin: str, duty: int) -> bool:
        """
        Set PWM duty cycle on a pin.
          pin:   "D9", "D10", "D11" (PWM-capable pins)
          duty:  0–255  (0 = off, 255 = full on)
        Sends:  P:D9:128\n
        """
        duty = max(0, min(255, int(duty)))
        return self._send(f"P:{pin}:{duty}")

    def pwm_write_pct(self, pin: str, pct: float) -> bool:
        """Convenience: PWM by percentage 0.0–100.0."""
        return self.pwm_write(pin, int(pct / 100.0 * 255))

    def analog_voltage(self, pin: str, voltage: float) -> bool:
        """
        Set analog output voltage (requires DAC, e.g. Arduino Due/Zero/MKR).
          pin:     "DAC0", "DAC1"
          voltage: 0.0–5.0 V (or 0.0–3.3 V depending on board)
        Sends:  V:DAC0:2.500\n
        """
        return self._send(f"V:{pin}:{voltage:.3f}")

    def servo_write(self, servo_id: str, angle: int) -> bool:
        """
        Set servo position.
          servo_id: "SERVO0", "SERVO1", or "S0" etc.
          angle:    0–180 degrees
        Sends:  S:SERVO0:90\n
        """
        angle = max(0, min(180, int(angle)))
        return self._send(f"S:{servo_id}:{angle}")

    def set_parameter(self, name: str, value: Any) -> bool:
        """
        Send a named setpoint or configuration parameter.
          name:  any string your firmware recognises, e.g. "SETPOINT", "KP", "MODE"
          value: numeric or string
        Sends:  C:SETPOINT:75.000\n
        """
        if isinstance(value, float):
            return self._send(f"C:{name}:{value:.4f}")
        return self._send(f"C:{name}:{value}")

    def request_value(self, name: str) -> bool:
        """
        Ask the Arduino to send the current value of a named sensor immediately.
        Sends:  Q:TEMP\n
        Arduino replies:  TEMP:23.45\n  (parsed into cache automatically)
        """
        return self._send(f"Q:{name}")

    def request_frame(self) -> bool:
        """Ask the Arduino to send a full data frame immediately."""
        return self._send("R:ALL")

    def emergency_stop(self) -> bool:
        """Disable all outputs immediately.  Sends:  X:STOP\n"""
        return self._send("X:STOP")

    def reset_outputs(self) -> bool:
        """Reset all outputs to default state.  Sends:  X:RESET\n"""
        return self._send("X:RESET")

    def configure_pins(
        self,
        di_pins:     List[str] = None,
        do_pins:     List[str] = None,
        analog_pins: List[str] = None,
    ) -> bool:
        """
        Send pin-configuration commands so the firmware activates the requested channels.
        No-op in simulation mode.  Commands are deferred 1 s to allow Arduino boot time.

          di_pins:     ["D2","D3","D8"] — sends DPIN:IN:2,3,8
          do_pins:     ["D5","D6","D9"] — sends DPIN:OUT:5,6,9
          analog_pins: ["A0","A1","A3"] — sends APIN:0,1,3
        """
        if self.simulate:
            return True

        def _dnum(p: str) -> str:
            return p[1:] if p.upper().startswith("D") else p

        def _anum(p: str) -> str:
            return p[1:] if p.upper().startswith("A") else p

        cmds = []
        if di_pins:
            cmds.append(f"DPIN:IN:{','.join(_dnum(p) for p in di_pins)}")
        if do_pins:
            cmds.append(f"DPIN:OUT:{','.join(_dnum(p) for p in do_pins)}")
        if analog_pins:
            cmds.append(f"APIN:{','.join(_anum(p) for p in analog_pins)}")

        if not cmds:
            return True

        def _deferred():
            time.sleep(1.0)
            for cmd in cmds:
                self._send(cmd)

        threading.Thread(target=_deferred, daemon=True, name="PinInit").start()
        return True

    # ── Legacy compat ─────────────────────────────────────────────────────

    def write(self, pin: str, value: int) -> bool:
        """
        Backward-compatible write — calls digital_write().
        Pin names starting with 'D' → digital.
        Pin names starting with 'P' → PWM.
        """
        if pin.upper().startswith("P"):
            return self.pwm_write(pin, value)
        return self.digital_write(pin, value)

    # ── Callbacks ─────────────────────────────────────────────────────────

    def on_ack(self, callback: Callable[[str], None]):
        """Register callback called when Arduino sends ACK:<command>."""
        self._ack_callbacks.append(callback)

    def on_error(self, callback: Callable[[str], None]):
        """Register callback called when Arduino sends ERR:<message>."""
        self._err_callbacks.append(callback)

    # ── Internal send ──────────────────────────────────────────────────────

    def _send(self, command: str) -> bool:
        """Send a command string + newline to the Arduino."""
        if self.simulate:
            self._sim_handle_command(command)
            return True
        if not (self._ser and self._ser.is_open):
            return False
        try:
            self._ser.write(f"{command}\n".encode("utf-8"))
            return True
        except Exception as e:
            print(f"[ArduinoLayer] send '{command}' failed: {e}")
            return False

    # ── Background threads ────────────────────────────────────────────────

    def _read_loop(self):
        """Hardware read thread — waits for Arduino reset then reads continuously."""
        deadline = time.time() + 2.5
        while time.time() < deadline and self._running:
            time.sleep(0.05)
        if not self._running:
            return
        try:
            self._ser.reset_input_buffer()
        except Exception:
            pass

        consecutive_errors = 0
        while self._running:
            try:
                if not self._ser or not self._ser.is_open:
                    break
                raw = self._ser.readline()
                if not raw:
                    continue
                line = raw.decode("utf-8", errors="replace").strip()
                if line:
                    self._handle_line(line)
                consecutive_errors = 0
            except Exception as e:
                consecutive_errors += 1
                if consecutive_errors <= 3:
                    print(f"[ArduinoLayer] read error: {e}")
                if consecutive_errors > 20:
                    print(f"[ArduinoLayer] too many errors, stopping")
                    break
                time.sleep(0.1)

    def _sim_loop(self):
        """Simulation thread — generates waveforms for analog pins."""
        while self._running:
            t = time.time() - self._t0
            update = {}
            for pin, p in self._sim_params.items():
                val  = p["amp"] * math.sin(2 * math.pi * p["freq"] * t + p["phase"])
                val += p["offset"]
                val += random.gauss(0, p["noise"] * p["amp"])
                update[pin] = round(max(0.0, min(5.0, val)), 4)
            with self._lock:
                self._cache.update(update)
            time.sleep(0.05)

    def _sim_handle_command(self, command: str):
        """Simulate Arduino response to a command."""
        with self._lock:
            parts = command.split(":")
            if len(parts) >= 3:
                cmd_type = parts[0]
                target   = parts[1]
                value    = ":".join(parts[2:])
                self._sim_outputs[target] = value
                # Echo back as cached value for digital/pwm outputs
                try:
                    self._cache[target] = float(value)
                except ValueError:
                    pass
            elif parts[0] == "X":
                self._sim_outputs.clear()
        # Fire ACK callbacks
        for cb in self._ack_callbacks:
            try:
                cb(command)
            except Exception:
                pass

    # ── Line parser ────────────────────────────────────────────────────────

    def _handle_line(self, line: str):
        """Route a line from the Arduino to cache, ACK, or ERR handlers."""
        # ACK response
        if line.startswith("ACK:"):
            cmd = line[4:]
            with self._lock:
                self._ack_cache[cmd.split(":")[0]] = cmd
            for cb in self._ack_callbacks:
                try:
                    cb(cmd)
                except Exception:
                    pass
            return

        # ERR response
        if line.startswith("ERR:"):
            msg = line[4:]
            print(f"[Arduino ERR] {msg}")
            for cb in self._err_callbacks:
                try:
                    cb(msg)
                except Exception:
                    pass
            return

        # Data line — parse into cache
        parsed = self._parse_line(line)
        if parsed:
            with self._lock:
                self._cache.update(parsed)

    def _parse_line(self, line: str) -> Dict[str, float]:
        """
        Parse Arduino serial output into {key: float}.

        Handles all of these formats transparently:
          Standard protocol:   A0:3.142,A1:0.015
          With unit suffix:    A0:0.04 V,A1:1.23 mA
          With line prefix:    Analog:8, A0:0.04 V
          Plain CSV:           3.142,0.015
          JSON:                {"A0":3.142,"A1":0.015}

        Prefixes like "Analog:8" (where value is an integer channel index)
        are recognised and skipped so they don't pollute the channel cache.
        """
        line = line.strip()
        if not line:
            return {}

        # Store raw line for diagnostics (keep last 5)
        with self._lock:
            if not hasattr(self, '_raw_lines'):
                self._raw_lines = []
            self._raw_lines.append(line)
            if len(self._raw_lines) > 5:
                self._raw_lines.pop(0)

        # JSON
        if line.startswith("{"):
            try:
                return {k: float(v) for k, v in json.loads(line).items()}
            except Exception:
                return {}

        result: Dict[str, float] = {}
        positional_idx = 0

        for token in line.split(","):
            token = token.strip()
            if not token:
                continue

            if ":" in token:
                key_raw, _, val_raw = token.partition(":")
                key = key_raw.strip()
                val_raw = val_raw.strip()

                # Strip unit suffix — take only the first word (the number)
                # "0.04 V" → "0.04",  "1.23 mA" → "1.23",  "3.14" → "3.14"
                numeric_part = val_raw.split()[0] if val_raw else ""

                try:
                    val = float(numeric_part)
                except ValueError:
                    continue

                # Skip prefix tokens where the "value" is actually a channel
                # count or index, not a real measurement.
                # Heuristic: key is a generic word (Analog, Digital, Chan, Ch)
                # AND value is a small integer that looks like a count.
                _SKIP_KEYS = {"analog", "digital", "chan", "channel", "ch",
                              "sensor", "input", "output", "port", "pin"}
                if key.lower() in _SKIP_KEYS and val == int(val) and val < 32:
                    continue

                result[key] = val

            else:
                # Plain positional value — map to analog_pins in order
                try:
                    val = float(token.split()[0])   # strip any trailing unit
                    if positional_idx < len(self.analog_pins):
                        result[self.analog_pins[positional_idx]] = val
                    positional_idx += 1
                except ValueError:
                    pass

        return result

    @property
    def raw_lines(self) -> list:
        """Last few raw lines received — useful for diagnostics."""
        with self._lock:
            return list(getattr(self, "_raw_lines", []))

    # ── Utilities ──────────────────────────────────────────────────────────

    @staticmethod
    def list_ports() -> List[Tuple[str, str]]:
        if not _SERIAL_AVAILABLE:
            return []
        try:
            return [
                (p.device, p.description or "")
                for p in serial.tools.list_ports.comports()
            ]
        except Exception as e:
            print(f"[ArduinoLayer] list_ports() error: {e}")
            return []

    @staticmethod
    def is_pyserial_available() -> bool:
        return _SERIAL_AVAILABLE

    def __repr__(self):
        mode = "SIM" if self.simulate else f"HW:{self.port}@{self.baud}"
        return f"<ArduinoLayer {mode} pins={self.analog_pins}>"


# ═══════════════════════════════════════════════════════════════════════════
# Arduino firmware source: api_layers/firmware/LabUI_firmware.ino
# Upload to board via Arduino IDE (115200 baud).
# ═══════════════════════════════════════════════════════════════════════════