"""
<<<<<<< HEAD
remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32 USB CDC

{"kill": true}  -> writes 'E\n' to ESP32 BALANCE (ESTOP_REMOTE, immediate motor cutoff)
{"kill": false} -> writes 'Z\n' to ESP32 BALANCE (clear latch, robot can re-arm)
=======
remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 USB CDC

{"kill": true}  -> writes 'E\n' to ESP32-S3 (ESTOP_REMOTE, immediate motor cutoff)
{"kill": false} -> writes 'Z\n' to ESP32-S3 (clear latch, robot can re-arm)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT).

Publishes /saltybot/estop_active (std_msgs/Bool).
"""

import json
import time
import threading

import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool, String

import paho.mqtt.client as mqtt
import serial


class RemoteEstopNode(Node):
    def __init__(self):
        super().__init__('remote_estop_node')

        self.declare_parameter('serial_port', '/dev/esp32-bridge')
        self.declare_parameter('baud_rate', 921600)
        self.declare_parameter('mqtt_host', 'mqtt.example.com')
        self.declare_parameter('mqtt_port', 1883)
        self.declare_parameter('mqtt_user', 'saltybot')
        self.declare_parameter('mqtt_password', '')
        self.declare_parameter('mqtt_topic', 'saltybot/estop')
        self.declare_parameter('cellular_timeout_s', 5.0)
        self.declare_parameter('auto_estop_on_disconnect', True)
        self.declare_parameter('mqtt_keepalive', 10)

        self._serial_port = self.get_parameter('serial_port').value
        self._baud_rate = self.get_parameter('baud_rate').value
        self._mqtt_host = self.get_parameter('mqtt_host').value
        self._mqtt_port = self.get_parameter('mqtt_port').value
        self._mqtt_user = self.get_parameter('mqtt_user').value
        self._mqtt_password = self.get_parameter('mqtt_password').value
        self._mqtt_topic = self.get_parameter('mqtt_topic').value
        self._cellular_timeout_s = self.get_parameter('cellular_timeout_s').value
        self._auto_estop_on_disconnect = self.get_parameter('auto_estop_on_disconnect').value
        self._mqtt_keepalive = self.get_parameter('mqtt_keepalive').value

        self._ser = serial.Serial(self._serial_port, self._baud_rate, timeout=0.1)
        self._estop_pub = self.create_publisher(Bool, '/saltybot/estop_active', 10)
        self._mode_sub = self.create_subscription(
            String, '/saltybot/balance_state', self._balance_state_cb, 10)
        self._auto_mode = False
        self._estop_active = False
        self._last_mqtt_ok = time.monotonic()
        self._watchdog_fired = False
        self._lock = threading.Lock()

        self._mqtt = mqtt.Client(client_id='saltybot-estop')
        self._mqtt.username_pw_set(self._mqtt_user, self._mqtt_password)
        self._mqtt.on_connect = self._on_mqtt_connect
        self._mqtt.on_disconnect = self._on_mqtt_disconnect
        self._mqtt.on_message = self._on_mqtt_message
        self._mqtt.will_set(self._mqtt_topic + '/status', 'OFFLINE', qos=1, retain=True)
        try:
            self._mqtt.connect_async(self._mqtt_host, self._mqtt_port, self._mqtt_keepalive)
            self._mqtt.loop_start()
        except Exception as e:
            self.get_logger().error(f'MQTT connect failed: {e}')

        self.create_timer(1.0, self._watchdog_cb)
        self.get_logger().info(
            f'remote_estop_node started -- MQTT {self._mqtt_host}:{self._mqtt_port}')

    def _on_mqtt_connect(self, client, userdata, flags, rc):
        if rc == 0:
            client.subscribe(self._mqtt_topic, qos=1)
            with self._lock:
                self._last_mqtt_ok = time.monotonic()
                self._watchdog_fired = False
        else:
            self.get_logger().warning(f'MQTT connect refused rc={rc}')

    def _on_mqtt_disconnect(self, client, userdata, rc):
        self.get_logger().warning(f'MQTT disconnected rc={rc}')

    def _on_mqtt_message(self, client, userdata, msg):
        with self._lock:
            self._last_mqtt_ok = time.monotonic()
            self._watchdog_fired = False
        try:
            payload = json.loads(msg.payload.decode('utf-8'))
            kill = payload.get('kill', False)
        except (json.JSONDecodeError, UnicodeDecodeError) as e:
            self.get_logger().error(f'Bad MQTT payload: {e}')
            return
        if kill:
            self._send_estop('E')
            self.get_logger().warning('MQTT kill=true -> ESTOP_REMOTE')
        else:
            self._send_clear()
            self.get_logger().info('MQTT kill=false -> estop clear')

    def _balance_state_cb(self, msg: String):
        self._auto_mode = '"md":2' in msg.data

    def _watchdog_cb(self):
        if not self._auto_estop_on_disconnect:
            return
        with self._lock:
            elapsed = time.monotonic() - self._last_mqtt_ok
            fired = self._watchdog_fired
        if self._auto_mode and elapsed > self._cellular_timeout_s and not fired:
            with self._lock:
                self._watchdog_fired = True
            self.get_logger().error(
                f'Cellular link lost {elapsed:.1f}s in AUTO mode -> ESTOP_CELLULAR_TIMEOUT')
            self._send_estop('F')

    def _send_estop(self, cmd: str):
        try:
            self._ser.write(f'{cmd}\n'.encode())
            self._ser.flush()
        except serial.SerialException as e:
            self.get_logger().error(f'Serial write error: {e}')
        with self._lock:
            self._estop_active = True
        msg = Bool()
        msg.data = True
        self._estop_pub.publish(msg)

    def _send_clear(self):
        try:
            self._ser.write(b'Z\n')
            self._ser.flush()
        except serial.SerialException as e:
            self.get_logger().error(f'Serial write error: {e}')
        with self._lock:
            self._estop_active = False
        msg = Bool()
        msg.data = False
        self._estop_pub.publish(msg)

    def destroy_node(self):
        self._mqtt.loop_stop()
        self._mqtt.disconnect()
        if self._ser.is_open:
            self._ser.close()
        super().destroy_node()


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


if __name__ == '__main__':
    main()
