import sys
import unittest
from pathlib import Path
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linker_hand_ros2_sdk'))
from linker_hand_ros2_sdk.trial_guard import TrialGuard

class GuardTests(unittest.TestCase):
    def test_no_input_no_command(self):
        self.assertIsNone(TrialGuard([100]*20).step(100))

    def test_anchor_no_jump(self):
        g = TrialGuard([100]*20)
        g.accept([255]*20, 0)
        self.assertEqual(g.step(0.05), [100]*20)

    def test_slew_and_envelope(self):
        g = TrialGuard([100]*20)
        g.accept([100]*20, 0)
        for i in range(1, 101):
            now = i * 0.05
            g.accept([255]*20, now)
            out = g.step(now)
            self.assertLessEqual(max(out), min(110, 100 + i*0.5 + 0.5))
        self.assertEqual(out, [110]*20)

    def test_timeout_latches_even_if_input_resumes(self):
        g = TrialGuard([100]*20)
        g.accept([100]*20, 0)
        self.assertIsNone(g.step(0.31))
        g.accept([120]*20, 0.32)
        self.assertIsNone(g.step(0.33))
        self.assertTrue(g.locked)

    def test_callback_gap_latches(self):
        g = TrialGuard([100]*20)
        g.accept([100]*20, 0)
        g.accept([120]*20, 0.31)
        self.assertTrue(g.locked)

    def test_half_mapping(self):
        g = TrialGuard([100]*20, 0.5)
        g.accept([200]*20, 0)
        self.assertEqual(g.target, [150]*20)
        self.assertEqual(g.step(0), [100]*20)
        self.assertEqual(g.step(0.1), [100]*20)  # 0.5-unit capped step

    def test_full_mapping_reaches_target_slowly(self):
        g = TrialGuard([100]*20, 1.0)
        g.accept([200]*20, 0)
        self.assertEqual(g.target, [200]*20)
        self.assertEqual(g.step(0), [100]*20)
        for i in range(1, 241):
            now = i * 0.05
            g.accept([200]*20, now)
            out = g.step(now)
            self.assertLessEqual(max(out), min(200, 100 + i*0.5 + 0.5))
        self.assertEqual(out, [200]*20)

    def test_scaled_modes_bounds_and_timeout(self):
        for scale in (0.5, 1.0):
            for initial, target in ((0,255),(255,0)):
                g = TrialGuard([initial]*20, scale)
                g.accept([target]*20, 0)
                self.assertTrue(all(0 <= x <= 255 for x in g.target))
                self.assertIsNone(g.step(0.31))
                self.assertTrue(g.locked)

    def test_follow_slew_and_stall_cap(self):
        g = TrialGuard([100]*20, 1.0, 60.0)
        g.accept([200]*20, 0)
        self.assertEqual(g.step(0.05), [103]*20)
        g.accept([200]*20, 0.2)
        self.assertEqual(g.step(0.2), [106]*20)
        self.assertIsNone(g.step(0.51))
        self.assertTrue(g.locked)

    def test_bad_slew(self):
        for rate in (0, -1, 61, float('inf'), float('nan')):
            with self.assertRaises(ValueError):
                TrialGuard([100]*20, 1.0, rate)

    def test_bad_scale(self):
        for scale in (-1, 0.75, 2, float('nan')):
            with self.assertRaises(ValueError):
                TrialGuard([100]*20, scale)

    def test_invalid_input_latches(self):
        for values in ([1]*19, [float('nan')]*20, [256]*20, [-1]*20):
            g = TrialGuard([100]*20)
            g.accept(values, 0)
            self.assertTrue(g.locked)
            self.assertIsNone(g.step(0))

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