"""Tests for raygeo.geo.algo.fillet module."""

import math

from raygeo.geo.algo.fillet import (
    append_end_fillets,
    create_fillet_polyline,
    get_descending_radius_fillet,
    trim_to_safe_fillet_span,
    try_fillet_one_end,
)


def _arc_length(polyline: list[tuple[float, float]]) -> float:
    """Sum of segment lengths along an open polyline."""
    total = 0.0
    for i in range(len(polyline) - 1):
        dx = polyline[i + 1][0] - polyline[i][0]
        dy = polyline[i + 1][1] - polyline[i][1]
        total += math.sqrt(dx * dx + dy * dy)
    return total


class TestCreateFilletPolyline:
    def test_quarter_circle(self):
        """Default 90° quarter-circle has expected arc length."""
        p = (0.0, 0.0)
        dir_ = (1.0, 0.0)
        radius = 10.0
        sweep_angle = math.pi / 2
        center, polyline = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, False
        )
        # arc length ≈ πr/2 (polyline approximation with 5 segments)
        expected = math.pi * radius / 2
        assert abs(_arc_length(polyline) - expected) < 0.15
        # start point is p
        assert polyline[0] == p
        # centre distance = r
        cx, cy = center
        dx = polyline[0][0] - cx
        dy = polyline[0][1] - cy
        assert abs(math.sqrt(dx * dx + dy * dy) - radius) < 0.001

    def test_quarter_circle_reverse(self):
        """Reverse flag produces arc on the opposite side."""
        p = (0.0, 0.0)
        dir_ = (1.0, 0.0)
        radius = 10.0
        sweep_angle = math.pi / 2
        _, fwd = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, False
        )
        _, rev = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, True
        )
        # endpoints differ: forward ends at (r, r), reverse at (-r, r)
        assert abs(fwd[-1][0] - radius) < 0.001
        assert abs(fwd[-1][1] - radius) < 0.001
        assert abs(rev[-1][0] + radius) < 0.001
        assert abs(rev[-1][1] - radius) < 0.001

    def test_sign_negative(self):
        """Negative side places arc on opposite side of direction."""
        p = (0.0, 0.0)
        dir_ = (1.0, 0.0)
        radius = 10.0
        sweep_angle = math.pi / 2
        _, pos = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, False
        )
        _, neg = create_fillet_polyline(
            p, dir_, radius, sweep_angle, -1.0, False
        )
        # positive side → arc goes up (positive y)
        # negative side → arc goes down (negative y)
        mid_idx = len(pos) // 2
        assert pos[mid_idx][1] > 0
        assert neg[mid_idx][1] < 0

    def test_half_circle(self):
        """180° sweep produces a half-circle."""
        p = (0.0, 0.0)
        dir_ = (1.0, 0.0)
        radius = 10.0
        sweep_angle = math.pi
        center, polyline = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, False
        )
        # arc length ≈ πr (polyline approximation with 9 segments)
        expected = math.pi * radius
        assert abs(_arc_length(polyline) - expected) < 0.25
        # centre distance = r
        cx, cy = center
        for pt in polyline:
            dx = pt[0] - cx
            dy = pt[1] - cy
            assert abs(math.sqrt(dx * dx + dy * dy) - radius) < 0.001

    def test_45_degrees(self):
        """45° sweep has correct arc length."""
        p = (0.0, 0.0)
        dir_ = (1.0, 0.0)
        radius = 10.0
        sweep_angle = math.pi / 4
        _, polyline = create_fillet_polyline(
            p, dir_, radius, sweep_angle, 1.0, False
        )
        expected = math.pi * radius / 4
        assert abs(_arc_length(polyline) - expected) < 0.1

    def test_zero_radius(self):
        """Zero radius produces degenerate polyline (start == end)."""
        p = (5.0, 5.0)
        dir_ = (1.0, 0.0)
        center, polyline = create_fillet_polyline(
            p, dir_, 0.0, math.pi / 2, 1.0, False
        )
        assert center == (5.0, 5.0)
        assert all(pt == p for pt in polyline)

    def test_zero_angle(self):
        """Zero sweep angle produces collinear points along direction."""
        p = (5.0, 5.0)
        dir_ = (1.0, 0.0)
        _, polyline = create_fillet_polyline(p, dir_, 10.0, 0.0, 1.0, False)
        # n_arc is clamped to 4, so we get 5 points all at p
        assert len(polyline) == 5
        for pt in polyline:
            assert abs(pt[0] - p[0]) < 0.001
            assert abs(pt[1] - p[1]) < 0.001

    def test_degenerate_direction(self):
        """Zero-length direction defaults to +X."""
        p = (3.0, 4.0)
        dir_ = (0.0, 0.0)
        center, polyline = create_fillet_polyline(
            p, dir_, 5.0, math.pi / 2, 1.0, False
        )
        # should behave like dir=(1,0)
        expected_center = (3.0, 9.0)  # p + side * n * r where n=(0,1)
        assert abs(center[0] - expected_center[0]) < 0.001
        assert abs(center[1] - expected_center[1]) < 0.001
        assert len(polyline) > 1


class TestAppendEndFillets:
    def test_basic_polyline(self):
        """Fillets added to a 3-point polyline produce a longer path."""
        polyline = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        radius = 5.0
        result = append_end_fillets(polyline, radius, math.pi / 2, 1.0, 1.0)
        assert len(result) > len(polyline)
        # start point should be filleted backward
        assert result[0][0] < 0.0  # reversed fillet goes left of start
        # end point should be filleted forward
        assert result[-1][0] > 100.0  # forward fillet goes right of end

    def test_two_points(self):
        """Two-point polyline gets fillets at both ends (same direction)."""
        polyline = [(0.0, 0.0), (10.0, 0.0)]
        result = append_end_fillets(polyline, 5.0, math.pi / 2, 1.0, 1.0)
        assert len(result) > len(polyline)
        # reversed start fillet goes left of (0,0)
        assert result[0][0] < 0.0
        # forward end fillet goes right of (10,0)
        assert result[-1][0] > 10.0

    def test_single_point(self):
        """Single-point polyline returns as-is."""
        polyline = [(0.0, 0.0)]
        result = append_end_fillets(polyline, 5.0, math.pi / 2, 1.0, 1.0)
        assert result == polyline

    def test_empty(self):
        """Empty polyline returns empty."""
        result = append_end_fillets([], 5.0, math.pi / 2, 1.0, 1.0)
        assert result == []

    def test_opposite_sides(self):
        """Different start/end sides produce fillets on opposite sides."""
        # S-shaped polyline: start curves up, end curves down
        polyline = [
            (0.0, 0.0),
            (10.0, 5.0),
            (20.0, 5.0),
            (30.0, 0.0),
            (40.0, -5.0),
        ]
        result = append_end_fillets(polyline, 5.0, math.pi / 2, 1.0, -1.0)
        assert len(result) > len(polyline)


class TestTrimToSafeFilletSpan:
    def test_no_obstacles(self):
        """Without obstacles the full span is safe."""
        polyline = [(0.0, 0.0), (50.0, 10.0), (100.0, 0.0)]
        outer = [(-10.0, -20.0), (110.0, -20.0), (110.0, 30.0), (-10.0, 30.0)]
        result = trim_to_safe_fillet_span(polyline, outer, [], 5.0, 0.0)
        assert result is not None
        enter, exit_ = result
        assert abs(enter[0] - polyline[0][0]) < 0.01
        assert abs(exit_[0] - polyline[-1][0]) < 0.01

    def test_collision_trims(self):
        """Obstacle near start forces trim inward."""
        polyline = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [(-10.0, -10.0), (110.0, -10.0), (110.0, 10.0), (-10.0, 10.0)]
        # obstacle blocking the start fillet
        obstacles = [[(0.0, -2.0), (10.0, -2.0), (10.0, 5.0), (0.0, 5.0)]]
        result = trim_to_safe_fillet_span(polyline, outer, obstacles, 5.0, 0.0)
        assert result is not None
        enter, _ = result
        # enter should be past the obstacle
        assert enter[0] > polyline[0][0] + 1.0

    def test_fully_blocked_returns_none(self):
        """When there's no safe span, return None."""
        polyline = [(0.0, 0.0), (10.0, 0.0), (20.0, 0.0)]
        # tiny outer boundary that blocks everything
        outer = [(5.0, -2.0), (15.0, -2.0), (15.0, 2.0), (5.0, 2.0)]
        result = trim_to_safe_fillet_span(polyline, outer, [], 10.0, 0.0)
        assert result is None

    def test_short_polyline(self):
        """Fewer than 3 points returns None."""
        result = trim_to_safe_fillet_span(
            [(0.0, 0.0), (10.0, 0.0)], [], [], 5.0, 0.0
        )
        assert result is None

    def test_zero_radius(self):
        """Zero radius returns None."""
        polyline = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        result = trim_to_safe_fillet_span(polyline, [], [], 0.0, 0.0)
        assert result is None


class TestTryFilletOneEnd:
    """Tests for try_fillet_one_end function."""

    def test_short_arc_unchanged(self):
        """Arc with < 2 points returned as-is."""
        result = try_fillet_one_end([(0, 0)], [], [], 5.0, 0.0)
        assert result == [(0, 0)]

    def test_fillet_adds_points(self):
        """Fillet adds points to the arc."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        result = try_fillet_one_end(arc, [], [], 5.0, 0.0)
        assert len(result) > len(arc)
        # Start point is extended backward by the fillet
        assert result[0][0] < arc[0][0]
        assert result[-1] == arc[-1]

    def test_no_obstacles_adds_fillet(self):
        """Without obstacles, a fillet is added (extends start)."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        result = try_fillet_one_end(arc, [], [], 5.0, 0.0)
        assert len(result) > len(arc)
        # Start is extended left, end point preserved
        assert result[0][0] < 0.0
        assert result[-1] == arc[-1]

    def test_obstacle_blocks_start_fillet(self):
        """When start fillet collides, exit fillet is used instead."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [(-10.0, -10.0), (110.0, -10.0), (110.0, 10.0), (-10.0, 10.0)]
        obstacles = [[(0.0, -5.0), (5.0, -5.0), (5.0, 5.0), (0.0, 5.0)]]
        result = try_fillet_one_end(arc, outer, obstacles, 5.0, 0.0)
        # Start fillet is blocked, but end fillet should be added
        assert len(result) > len(arc)
        assert result[-1][0] > 100.0

    def test_both_blocked_returns_original(self):
        """When both fillets blocked, original arc returned."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [(0.0, -5.0), (100.0, -5.0), (100.0, 5.0), (0.0, 5.0)]
        obstacles = [[(0.0, -5.0), (5.0, -5.0), (5.0, 5.0), (0.0, 5.0)]]
        result = try_fillet_one_end(arc, outer, obstacles, 10.0, 0.0)
        assert result == arc


class TestDescendingRadiusFillet:
    """Tests for get_descending_radius_fillet."""

    def test_full_radius_fits(self):
        """Without obstacles the full-radius fillet is returned."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [(-30.0, -30.0), (130.0, -30.0), (130.0, 30.0), (-30.0, 30.0)]
        result = get_descending_radius_fillet(arc, outer, [], 10.0, 0.0)
        # Should be longer than original (fillets at both ends)
        assert len(result) > len(arc)
        # Start should extend leftward
        assert result[0][0] < 0.0
        # End should extend rightward
        assert result[-1][0] > 100.0

    def test_descending_radius_with_obstacle(self):
        """Obstacle forces radius reduction — result still avoids it."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [(-5.0, -30.0), (120.0, -30.0), (120.0, 30.0), (-5.0, 30.0)]
        obstacles = [[(-4.0, -5.0), (0.0, -5.0), (0.0, 5.0), (-4.0, 5.0)]]
        result = get_descending_radius_fillet(arc, outer, obstacles, 20.0, 0.0)
        # A fillet should still be found (at reduced radius)
        assert len(result) > 0
        # The result should avoid the obstacle at (0,0) start — the
        # full-radius fillet would sweep through it, but the descending
        # logic should shrink r until it fits.

    def test_safety_distance_preserved(self):
        """Safety distance stays fixed as fillet radius shrinks.

        With margin = 5 and radius = 20, the effective safety distance is
        25.  The boundary at x = -24 forces the fillet to start no closer
        than 25 units — the result's first point verifies this.
        """
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        outer = [
            (-24.0, -30.0),
            (120.0, -30.0),
            (120.0, 30.0),
            (-24.0, 30.0),
        ]
        result = get_descending_radius_fillet(arc, outer, [], 20.0, 5.0)
        assert len(result) > 0
        # The first point of the result should maintain the full
        # safety distance (20 + 5 = 25) from the boundary at x = -24.
        clearance = result[0][0] - (-24.0)
        assert abs(clearance - 25.0) < 1.0

    def test_fully_blocked_returns_empty(self):
        """When no radius can fit, returns empty."""
        arc = [(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)]
        # Tiny boundary that blocks everything
        outer = [(5.0, -2.0), (95.0, -2.0), (95.0, 2.0), (5.0, 2.0)]
        result = get_descending_radius_fillet(arc, outer, [], 10.0, 0.0)
        assert len(result) == 0

    def test_short_arc_returns_fillet(self):
        """Arc with 2 points is still valid and gets filleted."""
        result = get_descending_radius_fillet(
            [(0.0, 0.0), (10.0, 0.0)], [], [], 5.0, 0.0
        )
        assert len(result) > 2
