import ast
import math
from pathlib import Path
import threading
from types import SimpleNamespace
import unittest


class Twist:
    def __init__(self):
        self.linear = SimpleNamespace(x=0.0)
        self.angular = SimpleNamespace(z=0.0)


class TeleopTest(unittest.TestCase):
    def setUp(self):
        self.now = 10.0
        self.messages = []
        source = ast.parse(Path(__file__).with_name('bridge.py').read_text())
        node = next(item for item in source.body if isinstance(item, ast.ClassDef) and item.name == 'SlamNode')
        methods = [item for item in node.body if isinstance(item, ast.FunctionDef)
                   and item.name in ('publish_cmd_vel', '_check_teleop_timeout')]
        scope = {'math': math, 'Twist': Twist, 'time': SimpleNamespace(monotonic=lambda: self.now)}
        exec(compile(ast.Module(body=methods, type_ignores=[]), 'bridge-teleop', 'exec'), scope)
        cls = type('Teleop', (), {name: scope[name] for name in ('publish_cmd_vel', '_check_teleop_timeout')})
        self.node = cls()
        self.node.read_only = False
        self.node._teleop_lock = threading.Lock()
        self.node._teleop_deadline = None
        self.node._cmd_vel_pub = SimpleNamespace(publish=lambda msg: self.messages.append((msg.linear.x, msg.angular.z)))

    def test_low_speed_and_expiry(self):
        self.node.publish_cmd_vel(5.0, -5.0, True)
        self.assertEqual(self.messages[-1], (0.1, -0.5))
        self.now += 0.6
        self.node._check_teleop_timeout()
        self.assertEqual(self.messages[-1], (0.0, 0.0))
        count = len(self.messages)
        self.node._check_teleop_timeout()
        self.assertEqual(len(self.messages), count)

    def test_heartbeat_and_release(self):
        self.node.publish_cmd_vel(0.1, 0, True)
        self.now += 0.4
        self.node.publish_cmd_vel(0.1, 0, True)
        self.now += 0.2
        self.node._check_teleop_timeout()
        self.assertEqual(len(self.messages), 2)
        self.node.publish_cmd_vel(0, 0, True)
        self.assertIsNone(self.node._teleop_deadline)

    def test_simulation_and_idle_navigation_unchanged(self):
        self.node._check_teleop_timeout()
        self.assertEqual(self.messages, [])
        self.node.publish_cmd_vel(0.22, 2.84)
        self.now += 2
        self.node._check_teleop_timeout()
        self.assertEqual(self.messages, [(0.22, 2.84)])

    def test_invalid_and_readonly_rejected(self):
        with self.assertRaises(RuntimeError):
            self.node.publish_cmd_vel(float('nan'), 0, True)
        self.node.read_only = True
        with self.assertRaises(RuntimeError):
            self.node.publish_cmd_vel(0.1, 0, True)
        self.assertEqual(self.messages, [])


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