"""Run from the software folder:  python3 -m unittest discover -s tests -v"""
import math
import os
import sys
import tempfile
import time
import unittest

os.environ['SANDTABLE_QUIET'] = '1'      # never pop up desktop notifications from the tests

sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
sys.path.insert(0, os.path.dirname(__file__))

from sandtable import config, patterns, thr
from sandtable.grbl import Controller, SwitchTripped
from sandtable.motion import Planner, phys_len
from fakegrbl import FakeGrbl

CFG = config.load('/nonexistent.toml')      # defaults only


class TestThr(unittest.TestCase):
    def test_parse(self):
        pts = thr.loads('# comment\n// also\n0 0\n1.5, 0.5\nbad line\n6.3 1.2\n')
        self.assertEqual(pts, [(0.0, 0.0), (1.5, 0.5), (6.3, 1.0)])   # rho clamped to 1

    def test_roundtrip(self):
        pts = patterns.spiral(loops=2)
        with tempfile.TemporaryDirectory() as d:
            p = os.path.join(d, 'a.thr')
            thr.save(p, pts, header='test')
            back = thr.load(p)
        self.assertEqual(len(back), len(pts))
        self.assertAlmostEqual(back[-1][0], pts[-1][0], places=4)


class TestPatterns(unittest.TestCase):
    def test_all_generators(self):
        for name in patterns.GENERATORS:
            pts = patterns.generate(name)
            self.assertGreater(len(pts), 10, name)
            rhos = [r for _, r in pts]
            self.assertTrue(all(0 <= r <= 1 for r in rhos), name)
            for end in (rhos[0], rhos[-1]):          # start and finish at the centre or the edge
                self.assertTrue(end < 0.02 or end > 0.98, f'{name} ends at rho {end}')
            for (t1, r1), (t2, r2) in zip(pts, pts[1:]):   # no jumps across the sand
                gap = math.hypot(r1 * math.cos(t1) - r2 * math.cos(t2), r1 * math.sin(t1) - r2 * math.sin(t2))
                self.assertLess(gap, 0.06, name)

    def test_lissajous_copies_join(self):
        pts = patterns.generate('lissajous', copies='3', twist_deg='20')
        for (t1, r1), (t2, r2) in zip(pts, pts[1:]):
            gap = math.hypot(r1 * math.cos(t1) - r2 * math.cos(t2), r1 * math.sin(t1) - r2 * math.sin(t2))
            self.assertLess(gap, 0.06)
        self.assertGreater(abs(pts[-1][1]), 0.98)

    def test_params(self):
        self.assertGreater(len(patterns.generate('rose', petals='7')), 100)
        with self.assertRaises(KeyError):
            patterns.generate('rose', nope=1)


class TestMotion(unittest.TestCase):
    def test_feed_keeps_ball_speed(self):
        pl = Planner(CFG)
        # 1 degree at rho 100 mm is 1.745 mm on the sand: at 25 mm/s that is 1 deg per 0.07 s
        f = pl.feed(0, 100, 1, 100, 25)
        self.assertAlmostEqual(f, 60 * 25 / (100 * math.pi / 180), delta=0.5)
        # pure rho move: machine units are mm, so F = speed * 60
        self.assertAlmostEqual(pl.feed(0, 10, 0, 20, 25), 1500, delta=0.1)

    def test_densify_limits(self):
        pl = Planner(CFG)
        pts = pl.densify([(0, 0), (720, 133)])
        for (t1, r1), (t2, r2) in zip(pts, pts[1:]):
            self.assertLessEqual(phys_len(t1, r1, t2, r2), CFG['motion']['max_segment_mm'] + 1e-6)
            self.assertLessEqual(abs(t2 - t1), CFG['motion']['max_segment_deg'] + 1e-6)

    def test_erase_single_leg(self):
        pl = Planner(CFG, 0.0, 2.0)
        path, shift = pl.erase_path(37.0, 133.0, 7.0)
        self.assertEqual(len(path), 2)
        end_t, end_r = path[-1]
        self.assertAlmostEqual(end_r, 133.0)
        self.assertAlmostEqual((end_t - 37.0) % 360, 0, places=6)
        self.assertAlmostEqual(abs(end_t) / 360, (133 - 2) / 7, delta=0.6)    # about pitch-spaced

    def test_erase_unwinds(self):
        pl = Planner(CFG, 50000.0, 133.0)          # arm has turned a lot one way
        path, _ = pl.erase_path(10.0, 133.0, 7.0)
        rmin = CFG['machine']['rho_min_mm']
        self.assertEqual([round(r, 3) for _, r in path], [133, rmin, 133])   # in to the floor and back out
        self.assertLess(path[-1][0], 50000.0)

    def test_never_inside_floor(self):
        # patterns through the centre stop at rho_min, so they never press the home switch
        rmin = CFG['machine']['rho_min_mm']
        pl = Planner(CFG, 0.0, rmin)       # (just after homing it starts at 2 mm and only moves out)
        for gen in ('rose', 'test_f', 'polygons'):
            lines, _, _ = pl.program(patterns.generate(gen))
            ys = [float(l.split('Y')[1].split('F')[0]) for l in lines if l.startswith('G1')]
            self.assertGreaterEqual(min(ys), rmin - 1e-9, gen)
            self.assertLess(min(ys), rmin + 0.01, gen)          # and still reach it

    def test_program_continuity(self):
        pl = Planner(CFG, 0.0, 2.0)
        lines, segs, est = pl.program(patterns.rose(petals=3))
        self.assertTrue(lines[0].startswith('(---'))
        self.assertGreater(est, 60)
        # the pattern part starts where the erase ended
        self.assertEqual(segs[0][1][-1], segs[1][1][0])
        # and the planner now knows where the ball is
        self.assertEqual((pl.theta, pl.rho), segs[-1][1][-1])


class TestController(unittest.TestCase):
    def make(self, **kw):
        fake = FakeGrbl(**kw)
        c = Controller(fake, 'grbl', log=lambda *a: None)
        c.wake(wait=0.3)
        return fake, c

    def test_stream_never_overflows(self):
        fake, c = self.make(speedup=400)
        pl = Planner(CFG, 0.0, 0.0)
        lines, _, _ = pl.program(patterns.spiral(loops=3), erase=False)
        lines = [l for l in lines if not l.startswith('(')]
        c.stream(lines)
        self.assertLessEqual(fake.max_rx, 128)
        got = [l for l in fake.lines if l.startswith('G1')]
        self.assertEqual(got, [l.upper() for l in lines])
        st = c.query()
        self.assertAlmostEqual(st.wpos[0], pl.theta, places=2)
        self.assertAlmostEqual(st.wpos[1], pl.rho, places=2)
        fake.close()

    def test_homing(self):
        fake, c = self.make(speedup=8, carriage_mm=40.0, switch_at=2.0)
        st = c.home_rho(CFG, log=lambda *a: None)
        self.assertAlmostEqual(st.wpos[1], CFG['homing']['switch_rho_mm'], places=2)
        real = fake.mpos[1] + fake.offset_true          # where the carriage really is
        self.assertLess(abs(real - fake.switch_at), 0.3)
        fake.close()

    def test_homing_from_pressed_switch(self):
        fake, c = self.make(speedup=8, carriage_mm=1.0, switch_at=2.0)
        st = c.home_rho(CFG, log=lambda *a: None)
        self.assertAlmostEqual(st.wpos[1], CFG['homing']['switch_rho_mm'], places=2)
        fake.close()

    def test_guard_quiet_on_a_normal_run(self):
        # homing leaves the switch pressed; moving off it and drawing must not trip the guard
        fake, c = self.make(speedup=60, carriage_mm=30.0, switch_at=2.0)
        c.home_rho(CFG, log=lambda *a: None)
        c.arm_guard(CFG)
        st = c.query()
        pl = Planner(CFG, st.wpos[0], st.wpos[1])
        for d in ('out', 'in'):
            lines, _, _ = pl.program(patterns.spiral(loops=2, direction=d), erase=False)
            c.stream([l for l in lines if not l.startswith('(')])
        self.assertEqual(c.guard, 'armed')
        self.assertFalse(fake.alarm)
        fake.close()

    def test_guard_stops_a_lost_carriage(self):
        fake, c = self.make(speedup=3, carriage_mm=30.0, switch_at=2.0)
        c.home_rho(CFG, log=lambda *a: None)
        c.arm_guard(CFG)
        c.stream(['G1X0Y60F6000'])
        with fake.lock:
            fake.offset_true -= 50.0       # belt skipped: the carriage is 50 mm further in than GRBL thinks
        with self.assertRaises(SwitchTripped) as cm:
            c.stream(['G1X0Y5F600'])
        self.assertIn('lost its place', str(cm.exception))
        y = float(str(cm.exception).split('rho ')[1].split(' mm')[0])
        self.assertTrue(40 < y < 53, y)                   # caught soon after the switch closed (rho 52)
        time.sleep(0.1)
        self.assertTrue(fake.alarm)                       # stopped by a reset while moving
        self.assertEqual(fake.planner, [])
        fake.close()

    def test_guard_spots_a_stuck_switch(self):
        fake, c = self.make(speedup=10, carriage_mm=30.0, switch_at=2.0)
        c.home_rho(CFG, log=lambda *a: None)
        with fake.lock:
            fake.switch_at = 1000.0         # switch now reads pressed everywhere
        c.arm_guard(CFG)
        with self.assertRaises(SwitchTripped) as cm:
            c.stream(['G1X0Y60F1200'])
        self.assertIn('stuck', str(cm.exception))
        fake.close()

    def test_stop_keeps_position(self):
        fake, c = self.make(speedup=5)
        c.command('G92 X0 Y10')                      # as homing leaves it; a reset clears this
        c.command('G1X0Y100F6000')
        time.sleep(0.2)
        st = c.stop()
        y = st.wpos[1]
        self.assertGreater(y, 0)
        self.assertEqual(c.query().state, 'Idle')
        self.assertAlmostEqual(c.query().wpos[1], y, places=3)
        fake.close()


class TestCommands(unittest.TestCase):
    """The real commands, end to end, against the simulator instead of a serial port."""

    def setUp(self):
        from sandtable import grbl
        self.fakes = []
        orig = grbl.Controller.open

        def fake_open(device, baud=115200, kind='grbl', log=print):
            f = FakeGrbl(speedup=150, carriage_mm=30.0)     # faster and the simulated homing overshoots
            self.fakes.append(f)
            c = grbl.Controller(f, kind, log=lambda *a: None)
            c.wake(wait=0.2)
            return c
        grbl.Controller.open = staticmethod(fake_open)
        self.addCleanup(setattr, grbl.Controller, 'open', orig)

    def run_cli(self, *argv):
        from sandtable import cli
        import contextlib
        import io
        buf = io.StringIO()
        with contextlib.redirect_stdout(buf):
            cli.main(['--port', 'sim', *argv])
        return buf.getvalue()

    def test_setup_writes_settings(self):
        out = self.run_cli('setup', '--yes')
        self.assertIn('written', out)
        self.assertEqual(self.fakes[-1].settings['$100'], '71.111')
        self.assertEqual(self.fakes[-1].settings['$22'], '0')

    def test_play_generated_pattern(self):
        self.run_cli('play', 'gen:spiral:loops=2', '--speed', '40')
        f = self.fakes[-1]
        self.assertTrue(any(l.startswith('$J=') for l in f.lines))          # homed first
        self.assertGreater(sum(1 for l in f.lines if l.startswith('G1X')), 100)
        self.assertLessEqual(f.max_rx, 128)


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