"""test_motor_daemon.py — Unit tests for saltybot_motor_daemon (Issue #523).

Tests cover:
  - Twist → W command conversion and clamping
  - Watchdog zeroing logic
  - FC status line parsing
  - RX line dispatch
"""

import math
import time
import unittest
from unittest.mock import MagicMock, patch


# ── Helpers (standalone, no ROS2 needed) ──────────────────────────────────────

def _clamp(v: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, v))


def twist_to_w(linear_x: float, angular_z: float,
               speed_scale: float = 1000.0,
               steer_scale: float = -500.0) -> tuple[int, int]:
    speed = int(_clamp(linear_x  * speed_scale, -1000.0, 1000.0))
    steer = int(_clamp(angular_z * steer_scale, -1000.0, 1000.0))
    return speed, steer


def parse_status_line(line: str) -> dict:
    result: dict = {"raw": line}
    for token in line.split():
        if "=" in token:
            k, _, v = token.partition("=")
            try:
                result[k] = int(v)
            except ValueError:
                result[k] = v
    return result


# ── Tests ──────────────────────────────────────────────────────────────────────

class TestTwistToW(unittest.TestCase):

    def test_forward(self):
        speed, steer = twist_to_w(1.0, 0.0)
        self.assertEqual(speed, 1000)
        self.assertEqual(steer, 0)

    def test_reverse(self):
        speed, steer = twist_to_w(-1.0, 0.0)
        self.assertEqual(speed, -1000)
        self.assertEqual(steer, 0)

    def test_turn_left(self):
        # ROS2 CCW+ (positive angular.z) → negative steer (flipped by negative steer_scale)
        speed, steer = twist_to_w(0.0, 1.0, steer_scale=-500.0)
        self.assertEqual(speed, 0)
        self.assertEqual(steer, -500)

    def test_turn_right(self):
        speed, steer = twist_to_w(0.0, -1.0, steer_scale=-500.0)
        self.assertEqual(speed, 0)
        self.assertEqual(steer, 500)

    def test_clamp_upper(self):
        speed, steer = twist_to_w(5.0, 0.0)
        self.assertEqual(speed, 1000)

    def test_clamp_lower(self):
        speed, steer = twist_to_w(-5.0, 0.0)
        self.assertEqual(speed, -1000)

    def test_steer_clamp(self):
        speed, steer = twist_to_w(0.0, 10.0, steer_scale=-500.0)
        self.assertEqual(steer, -1000)

    def test_zero(self):
        speed, steer = twist_to_w(0.0, 0.0)
        self.assertEqual(speed, 0)
        self.assertEqual(steer, 0)

    def test_combined(self):
        speed, steer = twist_to_w(0.5, 1.0, speed_scale=1000.0, steer_scale=-500.0)
        self.assertEqual(speed, 500)
        self.assertEqual(steer, -500)

    def test_fractional_truncates(self):
        # int() truncates toward zero
        speed, steer = twist_to_w(0.7777, 0.0, speed_scale=1000.0)
        self.assertEqual(speed, 777)


class TestWCommandFormat(unittest.TestCase):

    def test_format_positive(self):
        speed, steer = 500, -300
        cmd = f"W{speed},{steer}"
        self.assertEqual(cmd, "W500,-300")

    def test_format_zero(self):
        cmd = f"W{0},{0}"
        self.assertEqual(cmd, "W0,0")

    def test_format_negative(self):
        cmd = f"W{-1000},{-1000}"
        self.assertEqual(cmd, "W-1000,-1000")

    def test_max_positive(self):
        cmd = f"W{1000},{1000}"
        self.assertEqual(cmd, "W1000,1000")


class TestStatusLineParsing(unittest.TestCase):

    def test_typical_status(self):
        line = "ST cal=1 estop=0 pitch=? hb=12345"
        result = parse_status_line(line)
        self.assertEqual(result["cal"], 1)
        self.assertEqual(result["estop"], 0)
        self.assertEqual(result["pitch"], "?")
        self.assertEqual(result["hb"], 12345)

    def test_estop_active(self):
        line = "ST cal=1 estop=1 pitch=? hb=99999"
        result = parse_status_line(line)
        self.assertEqual(result["estop"], 1)

    def test_uncalibrated(self):
        line = "ST cal=0 estop=0 pitch=? hb=0"
        result = parse_status_line(line)
        self.assertEqual(result["cal"], 0)

    def test_raw_preserved(self):
        line = "ST cal=1 estop=0 pitch=? hb=1"
        result = parse_status_line(line)
        self.assertEqual(result["raw"], line)

    def test_empty_line(self):
        result = parse_status_line("")
        self.assertEqual(result["raw"], "")


class TestWatchdogLogic(unittest.TestCase):

    def test_watchdog_triggers_after_timeout(self):
        last_cmd_t = time.monotonic() - 0.3   # 300ms ago
        wd_timeout = 0.2
        age = time.monotonic() - last_cmd_t
        self.assertGreaterEqual(age, wd_timeout)

    def test_watchdog_does_not_trigger_before_timeout(self):
        last_cmd_t = time.monotonic() - 0.05   # 50ms ago
        wd_timeout = 0.2
        age = time.monotonic() - last_cmd_t
        self.assertLess(age, wd_timeout)

    def test_watchdog_speed_steer_zeroed(self):
        # When watchdog fires, output must be exactly zero
        wd_active = True
        speed = 0 if wd_active else 500
        steer = 0 if wd_active else 300
        self.assertEqual(speed, 0)
        self.assertEqual(steer, 0)


class TestAckParsing(unittest.TestCase):

    def test_valid_ack(self):
        line = "W:500,-300"
        self.assertTrue(line.startswith("W:"))
        parts = line[2:].split(",")
        self.assertEqual(int(parts[0]), 500)
        self.assertEqual(int(parts[1]), -300)

    def test_zero_ack(self):
        line = "W:0,0"
        self.assertTrue(line.startswith("W:"))

    def test_ack_not_confused_with_boot(self):
        self.assertFalse("SALTYLAB USART6 OK".startswith("W:"))


if __name__ == "__main__":
    unittest.main()
