<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
=======
"""esp32_cmd_node.py — Full bidirectional binary-framed ESP32-S3↔Jetson bridge.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py

Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).

<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
This node is NOT the primary drive path (that is CAN via can_bridge_node).
It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
=======
TX commands (Jetson → ESP32-S3):
  SPEED_STEER  — 50 Hz from /cmd_vel subscription
  HEARTBEAT    — 200 ms timer (ESP32-S3 watchdog fires at 500 ms)
  ARM          — via /saltybot/arm service
  SET_MODE     — via /saltybot/set_mode service
  PID_UPDATE   — via /saltybot/pid_update topic
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py

Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]  @ 460800 baud

<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
RX from ESP32 IO:
  RC_CHANNELS (0x01) → /saltybot/rc_channels  (std_msgs/String JSON)
  SENSORS     (0x02) → /saltybot/sensors       (std_msgs/String JSON)
=======
RX telemetry (ESP32-S3 → Jetson):
  IMU          → /saltybot/imu           (sensor_msgs/Imu)
  BATTERY      → /saltybot/telemetry/battery (std_msgs/String JSON)
  MOTOR_RPM    → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
  ARM_STATE    → /saltybot/arm_state     (std_msgs/String JSON)
  ERROR        → /saltybot/error         (std_msgs/String JSON)
  All frames   → /diagnostics            (diagnostic_msgs/DiagnosticArray)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py

TX to ESP32 IO:
  LED_CMD     (0x10) ← /saltybot/leds          (std_msgs/String JSON)
  OUTPUT_CMD  (0x11) ← /saltybot/outputs       (std_msgs/String JSON)
  HEARTBEAT   (0x20)  — sent every heartbeat_period (keep IO watchdog alive)

<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
Parameters (config/stm32_cmd_params.yaml):
  serial_port       /dev/esp32-io
  baud_rate         460800
  reconnect_delay   2.0
  heartbeat_period  0.2   (ESP32 IO watchdog fires at ~500 ms)
=======
Parameters (config/esp32_cmd_params.yaml):
  serial_port       /dev/ttyACM0
  baud_rate         921600
  reconnect_delay   2.0   (seconds)
  heartbeat_period  0.2   (seconds)
  watchdog_timeout  0.5   (seconds — no /cmd_vel → send zero-speed)
  speed_scale       1000.0 (linear.x m/s → ESC units)
  steer_scale      -500.0  (angular.z rad/s → ESC units, neg to flip convention)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
"""

from __future__ import annotations

import json
import threading
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy

import serial

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from std_msgs.msg import String

<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
from .stm32_protocol import (
    BAUD_RATE,
=======
from .esp32_protocol import (
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
    FrameParser,
    RcChannels,
    SensorData,
    encode_heartbeat,
    encode_led_cmd,
    encode_output_cmd,
)


class Stm32CmdNode(Node):
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
    """Orin ↔ ESP32-S3 IO auxiliary bridge node."""
=======
    """Binary-framed Jetson↔ESP32-S3 bridge node."""
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py

    def __init__(self) -> None:
        super().__init__("esp32_cmd_node")

        # ── Parameters ────────────────────────────────────────────────────
        self.declare_parameter("serial_port",      "/dev/esp32-io")
        self.declare_parameter("baud_rate",        BAUD_RATE)
        self.declare_parameter("reconnect_delay",  2.0)
        self.declare_parameter("heartbeat_period", 0.2)

        self._port_name       = self.get_parameter("serial_port").value
        self._baud            = self.get_parameter("baud_rate").value
        self._reconnect_delay = self.get_parameter("reconnect_delay").value
        self._hb_period       = self.get_parameter("heartbeat_period").value

        # ── QoS ───────────────────────────────────────────────────────────
        rel_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )

        # ── Publishers ────────────────────────────────────────────────────
        self._rc_pub   = self.create_publisher(String,          "/saltybot/rc_channels", rel_qos)
        self._sens_pub = self.create_publisher(String,          "/saltybot/sensors",     rel_qos)
        self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics",          rel_qos)

        # ── Subscriptions ─────────────────────────────────────────────────
        self.create_subscription(String, "/saltybot/leds",    self._on_leds,    rel_qos)
        self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)

        # ── Serial state ──────────────────────────────────────────────────
        self._ser: serial.Serial | None = None
        self._ser_lock = threading.Lock()
        self._parser   = FrameParser()
        self._rx_count = 0

        # ── Open serial and start timers ──────────────────────────────────
        self._open_serial()

        self._read_timer = self.create_timer(0.005,           self._read_cb)
        self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
        self._diag_timer = self.create_timer(1.0,             self._publish_diagnostics)

        self.get_logger().info(
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
            f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
=======
            f"esp32_cmd_node started — {port} @ {baud} baud | "
            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
        )

    # ── Serial management ─────────────────────────────────────────────────

    def _open_serial(self) -> bool:
        with self._ser_lock:
            try:
                self._ser = serial.Serial(
                    port=self._port_name,
                    baudrate=self._baud,
                    timeout=0.005,
                    write_timeout=0.1,
                )
                self._ser.reset_input_buffer()
                self._parser.reset()
                self.get_logger().info(f"Serial open: {self._port_name}")
                return True
            except serial.SerialException as exc:
                self.get_logger().error(
                    f"Cannot open {self._port_name}: {exc}",
                    throttle_duration_sec=self._reconnect_delay,
                )
                self._ser = None
                return False

    def _write(self, data: bytes) -> bool:
        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
                return False
            try:
                self._ser.write(data)
                return True
            except serial.SerialException as exc:
                self.get_logger().error(f"Serial write error: {exc}")
                self._ser = None
                return False

    # ── RX ────────────────────────────────────────────────────────────────

    def _read_cb(self) -> None:
        raw: bytes | None = None
        reconnect = False

        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
                reconnect = True
            else:
                try:
                    n = self._ser.in_waiting
                    if n:
                        raw = self._ser.read(n)
                except serial.SerialException as exc:
                    self.get_logger().error(f"Serial read error: {exc}")
                    self._ser = None
                    reconnect = True

        if reconnect:
            self.get_logger().warn(
                "Serial disconnected — will retry",
                throttle_duration_sec=self._reconnect_delay,
            )
            time.sleep(self._reconnect_delay)
            self._open_serial()
            return

        if not raw:
            return

        for byte in raw:
            msg = self._parser.feed(byte)
            if msg is not None:
                self._rx_count += 1
                self._dispatch(msg)

    def _dispatch(self, msg) -> None:
        now = self.get_clock().now().to_msg()
        ts  = f"{now.sec}.{now.nanosec:09d}"

        if isinstance(msg, RcChannels):
            out = String()
            out.data = json.dumps({
                "channels": msg.channels,
                "source":   msg.source,
                "ts":       ts,
            })
            self._rc_pub.publish(out)

        elif isinstance(msg, SensorData):
            out = String()
            out.data = json.dumps({
                "pressure_pa":   msg.pressure_pa,
                "temperature_c": msg.temperature_c,
                "tof_mm":        msg.tof_mm,
                "ts":            ts,
            })
            self._sens_pub.publish(out)

        elif isinstance(msg, tuple):
            type_code, _ = msg
            self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")

<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
    # ── TX ────────────────────────────────────────────────────────────────
=======
        elif isinstance(frame, ArmStateFrame):
            self._publish_arm_state(frame, now)

        elif isinstance(frame, ErrorFrame):
            self._publish_error(frame, now)

        elif isinstance(frame, tuple):
            type_code, payload = frame
            self.get_logger().debug(
                f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
            )

    # ── Telemetry publishers ──────────────────────────────────────────────────

    def _publish_imu(self, frame: ImuFrame, stamp) -> None:
        msg = Imu()
        msg.header.stamp    = stamp
        msg.header.frame_id = IMU_FRAME_ID

        # orientation: unknown — signal with -1 in first covariance
        msg.orientation_covariance[0] = -1.0

        msg.angular_velocity.x = math.radians(frame.pitch_deg)
        msg.angular_velocity.y = math.radians(frame.roll_deg)
        msg.angular_velocity.z = math.radians(frame.yaw_deg)
        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from ESP32-S3 BMI088
        msg.angular_velocity_covariance[0] = cov
        msg.angular_velocity_covariance[4] = cov
        msg.angular_velocity_covariance[8] = cov

        msg.linear_acceleration.x = frame.accel_x
        msg.linear_acceleration.y = frame.accel_y
        msg.linear_acceleration.z = frame.accel_z
        acov = 0.05 ** 2    # ±0.05 m/s² noise
        msg.linear_acceleration_covariance[0] = acov
        msg.linear_acceleration_covariance[4] = acov
        msg.linear_acceleration_covariance[8] = acov

        self._imu_pub.publish(msg)

    def _publish_battery(self, frame: BatteryFrame, stamp) -> None:
        payload = {
            "voltage_v":  round(frame.voltage_mv / 1000.0, 3),
            "voltage_mv": frame.voltage_mv,
            "current_ma": frame.current_ma,
            "soc_pct":    frame.soc_pct,
            "charging":   frame.current_ma < -100,
            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        self._last_battery_mv = frame.voltage_mv
        msg = String()
        msg.data = json.dumps(payload)
        self._battery_pub.publish(msg)

    def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None:
        payload = {
            "left_rpm":  frame.left_rpm,
            "right_rpm": frame.right_rpm,
            "ts":        f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._rpm_pub.publish(msg)

    def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None:
        label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})")
        if frame.state != self._last_arm_state:
            self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})")
            self._last_arm_state = frame.state

        payload = {
            "state":       frame.state,
            "state_label": label,
            "error_flags": frame.error_flags,
            "ts":          f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._arm_pub.publish(msg)

    def _publish_error(self, frame: ErrorFrame, stamp) -> None:
        self.get_logger().error(
            f"ESP32-S3 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
        )
        payload = {
            "error_code": frame.error_code,
            "subcode":    frame.subcode,
            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._error_pub.publish(msg)

    # ── TX — command send ─────────────────────────────────────────────────────

    def _on_cmd_vel(self, msg: Twist) -> None:
        """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz."""
        speed = int(_clamp(msg.linear.x  * self._speed_scale, -1000.0, 1000.0))
        steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0))
        self._last_speed    = speed
        self._last_steer    = steer
        self._last_cmd_t    = time.monotonic()
        self._watchdog_sent = False

        frame = encode_speed_steer(speed, steer)
        if not self._write(frame):
            self.get_logger().warn(
                "SPEED_STEER dropped — serial not open",
                throttle_duration_sec=2.0,
            )
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py

    def _heartbeat_cb(self) -> None:
        self._write(encode_heartbeat())

    def _on_leds(self, msg: String) -> None:
        """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
        try:
            d = json.loads(msg.data)
            frame = encode_led_cmd(
                int(d.get("pattern", 0)),
                int(d.get("r", 0)),
                int(d.get("g", 0)),
                int(d.get("b", 0)),
            )
        except (ValueError, KeyError, json.JSONDecodeError) as exc:
            self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
            return
        self._write(frame)

    def _on_outputs(self, msg: String) -> None:
        """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
        try:
            d = json.loads(msg.data)
            frame = encode_output_cmd(
                bool(d.get("horn",     False)),
                bool(d.get("buzzer",   False)),
                int(d.get("headlight", 0)),
                int(d.get("fan",       0)),
            )
        except (ValueError, KeyError, json.JSONDecodeError) as exc:
            self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
            return
        self._write(frame)

    # ── Diagnostics ───────────────────────────────────────────────────────

    def _publish_diagnostics(self) -> None:
        diag = DiagnosticArray()
        diag.header.stamp = self.get_clock().now().to_msg()
        status = DiagnosticStatus()
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
        status.name        = "saltybot/esp32_io_bridge"
        status.hardware_id = "esp32-s3-io"
=======
        status.name        = "saltybot/esp32_cmd_node"
        status.hardware_id = "esp32s322"

>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
        port_ok = self._ser is not None and self._ser.is_open
        status.level   = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
        status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
        status.values  = [
            KeyValue(key="serial_port", value=self._port_name),
            KeyValue(key="baud_rate",   value=str(self._baud)),
            KeyValue(key="port_open",   value=str(port_ok)),
            KeyValue(key="rx_frames",   value=str(self._rx_count)),
            KeyValue(key="rx_errors",   value=str(self._parser.frames_error)),
        ]
        diag.status.append(status)
        self._diag_pub.publish(diag)

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

    def destroy_node(self) -> None:
        self._write(encode_heartbeat(state=0))
        with self._ser_lock:
            if self._ser and self._ser.is_open:
                try:
                    self._ser.close()
                except Exception:
                    pass
            self._ser = None
        super().destroy_node()


def main(args=None) -> None:
    rclpy.init(args=args)
    node = Stm32CmdNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
