"""Stand-in for the ESP32 firmware — same topics, same watchdog behavior as
firmware.ino. Lets mqtt_bridge.py be verified end-to-end before real hardware
arrives; swap this process out for the real ESP32 later, no bridge changes.
"""
import json
import threading
import time

import paho.mqtt.client as mqtt

from mqtt_bridge import BROKER, PORT, TOPIC_DRIVE, TOPIC_STOP, TOPIC_HEARTBEAT, TOPIC_SENSORS

HEARTBEAT_TIMEOUT_S = 0.5
SENSOR_PUBLISH_INTERVAL_S = 1.0

_client = mqtt.Client()
_last_heartbeat = time.monotonic()
_motor_state = "stopped"


def _stop_motors(reason=""):
    global _motor_state
    if _motor_state != "stopped":
        print(f"MOTORS: stop {reason}".strip())
    _motor_state = "stopped"


def _drive_motors(direction, speed):
    global _motor_state
    _motor_state = f"{direction}@{speed}"
    print(f"MOTORS: {_motor_state}")


def _on_message(_client, _userdata, msg):
    global _last_heartbeat
    if msg.topic == TOPIC_HEARTBEAT:
        _last_heartbeat = time.monotonic()
        return
    if msg.topic == TOPIC_STOP:
        _stop_motors()
        return
    if msg.topic == TOPIC_DRIVE:
        payload = json.loads(msg.payload)
        _drive_motors(payload.get("direction", "stop"), payload.get("speed", 0))


def _watchdog_loop():
    while True:
        if time.monotonic() - _last_heartbeat > HEARTBEAT_TIMEOUT_S:
            _stop_motors("(watchdog: no heartbeat)")
        time.sleep(0.1)


def _sensor_loop():
    battery = 8.4
    while True:
        battery = max(6.0, battery - 0.01)
        _client.publish(TOPIC_SENSORS, json.dumps({
            "battery_v": round(battery, 2),
            "uptime_ms": int(time.monotonic() * 1000),
        }))
        time.sleep(SENSOR_PUBLISH_INTERVAL_S)


def main():
    _client.on_message = _on_message
    _client.connect(BROKER, PORT)
    _client.subscribe(TOPIC_DRIVE)
    _client.subscribe(TOPIC_STOP)
    _client.subscribe(TOPIC_HEARTBEAT)
    _client.loop_start()

    threading.Thread(target=_watchdog_loop, daemon=True).start()
    threading.Thread(target=_sensor_loop, daemon=True).start()

    print("ESP32 simulator running (Ctrl+C to stop)")
    while True:
        time.sleep(1)


if __name__ == "__main__":
    main()