import math
import random
import unittest

from geometry import Measurement, solve, smallest_enclosing_circle
from geometry.localization import halfplanes, distance


class GeometryTests(unittest.TestCase):
    def test_equilateral_from_legal_bearings(self):
        h = 20 * math.sqrt(3)
        stations = [Measurement(-1000, 0, 1),
                    Measurement(540, -500 * math.sqrt(3), 121),
                    Measurement(520, h + 500 * math.sqrt(3), 241)]
        result = solve(stations)
        self.assertEqual(result['status'], 'polygon')
        self.assertEqual(len(result['vertices']), 3)
        self.assertAlmostEqual(result['diameter_m'], 40, places=7)
        self.assertAlmostEqual(result['minimum_enclosing_circle']['radius_m'], 40/math.sqrt(3), places=7)
        self.assertFalse(result['diameter_circle']['covers'])
        self.assertFalse(result['guaranteed_clearable_at_mec_center'])

    def test_no_observations_and_single_bearing(self):
        self.assertEqual(solve([])['status'], 'unbounded')
        r = solve([Measurement(20, 30, 359.8)])
        self.assertEqual(r['status'], 'unbounded')
        self.assertTrue(r['diameter_infinite'])
        self.assertIsNone(r['diameter_m'])

    def test_inconsistent(self):
        result = solve([Measurement(-100, 0, 180), Measurement(100, 0, 0)])
        self.assertEqual(result['status'], 'empty')

    def test_exact_segment_and_point(self):
        segment = [Measurement(0, 0, 0, 0), Measurement(100, 0, 180, 0)]
        r = solve(segment)
        self.assertEqual(r['status'], 'segment')
        self.assertAlmostEqual(r['diameter_m'], 100)
        self.assertTrue(r['diameter_circle']['covers'])
        r = solve(segment + [Measurement(30, -30, 90, 0)])
        self.assertEqual(r['status'], 'point')
        self.assertAlmostEqual(r['vertices'][0][0], 30)
        self.assertAlmostEqual(r['diameter_m'], 0)

    def test_forward_ray_not_line(self):
        r = solve([Measurement(0, 0, 0, 0), Measurement(-1, 0, 180, 0)])
        self.assertEqual(r['status'], 'empty')

    def test_angle_wrap_and_repeated_station(self):
        a = [Measurement(-500, -1, 359.9), Measurement(0, -500, 90)]
        b = [Measurement(-500, -1, -.1), Measurement(0, -500, 450)]
        self.assertAlmostEqual(solve(a)['diameter_m'], solve(b)['diameter_m'])
        self.assertAlmostEqual(solve(a+a)['diameter_m'], solve(a)['diameter_m'])

    def test_close_parallel_remains_finite(self):
        # Two nearly parallel wedges can still be bounded, very far away.
        r = solve([Measurement(0, 0, 1), Measurement(0, 100, -1.000001)])
        self.assertEqual(r['status'], 'polygon')
        self.assertGreater(r['diameter_m'], 1e9)

    def test_circle_known_shapes(self):
        c, r = smallest_enclosing_circle([(0,0),(2,0),(2,2),(0,2)])
        self.assertEqual(c, (1,1))
        self.assertAlmostEqual(r, math.sqrt(2))
        c, r = smallest_enclosing_circle([(0,0),(2,0),(.4,.2)])
        self.assertAlmostEqual(r, 1)
        c, r = smallest_enclosing_circle([(0,0),(1,0),(3,0)])
        self.assertAlmostEqual(r, 1.5)

    def test_random_bounded_truth_containment_and_monotonicity(self):
        rng = random.Random(20260910)
        for _ in range(100):
            target = (rng.uniform(-300,300), rng.uniform(-300,300))
            ms = []
            for i in range(4):
                ang = (i*90 + rng.uniform(-20,20))*math.pi/180
                radius = rng.uniform(300,1200)
                p = (target[0]+radius*math.cos(ang), target[1]+radius*math.sin(ang))
                bearing = math.degrees(math.atan2(target[1]-p[1],target[0]-p[0]))
                ms.append(Measurement(*p, bearing+rng.uniform(-.99,.99)))
            r3, r4 = solve(ms[:3]), solve(ms)
            self.assertEqual(r4['status'], 'polygon')
            self.assertLessEqual(r4['diameter_m'], r3['diameter_m']+1e-5)
            for m in ms:
                for a,b,c in halfplanes(m):
                    self.assertLessEqual(a*target[0]+b*target[1], c+1e-7)
            mec = r4['minimum_enclosing_circle']
            self.assertLessEqual(distance(mec['center'], target), mec['radius_m']+1e-5)
            for v in r4['vertices']:
                self.assertLessEqual(distance(mec['center'], v), mec['radius_m']+1e-5)

    def test_translation_rotation_scaling(self):
        ms = [Measurement(-500,0,1), Measurement(0,-500,89), Measurement(400,400,225)]
        original = solve(ms)
        a=.37
        changed = [Measurement(1e6+3*(m.x*math.cos(a)-m.y*math.sin(a)),
                               -2e6+3*(m.x*math.sin(a)+m.y*math.cos(a)),
                               m.bearing_deg+math.degrees(a)) for m in ms]
        moved=solve(changed)
        self.assertAlmostEqual(moved['diameter_m'], original['diameter_m']*3, places=6)

    def test_invalid_input(self):
        for args in [(float('nan'),0,0),(0,0,float('inf')),(0,0,0,90),(0,0,0,-1),(True,0,0)]:
            with self.assertRaises(ValueError): Measurement(*args)

    def test_independent_rectangle_clipping_reference(self):
        # A second construction: clip a demonstrably inactive finite box.
        # The production algorithm never uses this box.
        rng=random.Random(4001)
        for _ in range(60):
            ms=[]
            for x,y in [(-500,-300),(600,-200),(0,700)]:
                ms.append(Measurement(x,y,math.degrees(math.atan2(-y,-x))+rng.uniform(-.9,.9)))
            poly=[(-2000.,-2000.),(2000.,-2000.),(2000.,2000.),(-2000.,2000.)]
            for m in ms:
                for a,b,c in halfplanes(m):
                    out=[]
                    for p,q in zip(poly,poly[1:]+poly[:1]):
                        f=a*p[0]+b*p[1]-c;g=a*q[0]+b*q[1]-c
                        if f<=0:out.append(p)
                        if f*g<0:
                            t=f/(f-g);out.append((p[0]+t*(q[0]-p[0]),p[1]+t*(q[1]-p[1])))
                    poly=out
            self.assertTrue(poly)
            self.assertTrue(all(max(abs(x),abs(y))<1900 for x,y in poly))
            expected=max(math.dist(p,q) for p in poly for q in poly)
            self.assertAlmostEqual(solve(ms)['diameter_m'],expected,places=6)


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