import pytest
from shapely.geometry import Polygon

from inire.geometry.collision import CollisionEngine
from inire.geometry.primitives import Port
from inire.router.astar import AStarRouter
from inire.router.cost import CostEvaluator
from inire.router.danger_map import DangerMap
from inire.router.pathfinder import PathFinder


@pytest.fixture
def basic_evaluator() -> CostEvaluator:
    engine = CollisionEngine(clearance=2.0)
    # Wider bounds to allow going around (y from -40 to 40)
    danger_map = DangerMap(bounds=(0, -40, 100, 40))
    danger_map.precompute([])
    return CostEvaluator(engine, danger_map, bend_penalty=50.0, sbend_penalty=150.0)


def test_astar_sbend(basic_evaluator: CostEvaluator) -> None:
    router = AStarRouter(basic_evaluator, snap_size=1.0, sbend_offsets=[2.0, 5.0])
    # Start at (0,0), target at (50, 2) -> 2um lateral offset
    # This matches one of our discretized SBend offsets.
    start = Port(0, 0, 0)
    target = Port(50, 2, 0)
    path = router.route(start, target, net_width=2.0)

    assert path is not None
    # Check if any component in the path is an SBend
    found_sbend = False
    for res in path:
        # Check if the end port orientation is same as start
        # and it's not a single straight (which would have y=0)
        if abs(res.end_port.y - start.y) > 0.1 and abs(res.end_port.orientation - start.orientation) < 0.1:
            found_sbend = True
            break
    assert found_sbend


def test_pathfinder_negotiated_congestion_resolution(basic_evaluator: CostEvaluator) -> None:
    router = AStarRouter(basic_evaluator, snap_size=1.0, bend_radii=[5.0, 10.0], sbend_radii=[5.0, 10.0])
    # Increase base penalty to force detour immediately
    pf = PathFinder(router, basic_evaluator, max_iterations=10, base_congestion_penalty=1000.0)

    netlist = {
        "net1": (Port(0, 0, 0), Port(50, 0, 0)),
        "net2": (Port(0, 10, 0), Port(50, 10, 0)),
    }
    net_widths = {"net1": 2.0, "net2": 2.0}

    # Force them into a narrow corridor that only fits ONE.
    obs_top = Polygon([(20, 6), (30, 6), (30, 15), (20, 10)]) # Lower wall
    obs_bottom = Polygon([(20, 4), (30, 4), (30, -15), (20, -10)])

    basic_evaluator.collision_engine.add_static_obstacle(obs_top)
    basic_evaluator.collision_engine.add_static_obstacle(obs_bottom)
    basic_evaluator.danger_map.precompute([obs_top, obs_bottom])

    results = pf.route_all(netlist, net_widths)

    assert results["net1"].is_valid
    assert results["net2"].is_valid
