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
from inire.utils.visualization import plot_routing_results


def main() -> None:
    print("Running Example 06: Bend Collision Models...")

    # 1. Setup Environment
    # Give room for 10um bends near the edges
    bounds = (-20, -20, 170, 170)
    engine = CollisionEngine(clearance=2.0)
    danger_map = DangerMap(bounds=bounds)

    # Create three scenarios with identical obstacles
    # We'll space them out vertically
    obs_arc = Polygon([(40, 110), (60, 110), (60, 130), (40, 130)])
    obs_bbox = Polygon([(40, 60), (60, 60), (60, 80), (40, 80)])
    obs_clipped = Polygon([(40, 10), (60, 10), (60, 30), (40, 30)])

    obstacles = [obs_arc, obs_bbox, obs_clipped]
    for obs in obstacles:
        engine.add_static_obstacle(obs)
    danger_map.precompute(obstacles)

    # We'll run three separate routers since collision_type is a router-level config
    evaluator = CostEvaluator(engine, danger_map, bend_penalty=50.0, sbend_penalty=150.0)

    # Scenario 1: Standard 'arc' model (High fidelity)
    router_arc = AStarRouter(evaluator, snap_size=1.0, straight_lengths=[1.0, 5.0, 25.0], bend_radii=[10.0], bend_collision_type="arc")
    netlist_arc = {"arc_model": (Port(10, 120, 0), Port(90, 140, 90))}

    # Scenario 2: 'bbox' model (Conservative axis-aligned box)
    router_bbox = AStarRouter(evaluator, snap_size=1.0, straight_lengths=[1.0, 5.0, 25.0], bend_radii=[10.0], bend_collision_type="bbox")
    netlist_bbox = {"bbox_model": (Port(10, 70, 0), Port(90, 90, 90))}

    # Scenario 3: 'clipped_bbox' model (Balanced)
    router_clipped = AStarRouter(evaluator, snap_size=1.0, straight_lengths=[1.0, 5.0, 25.0], bend_radii=[10.0], bend_collision_type="clipped_bbox", bend_clip_margin=1.0)
    netlist_clipped = {"clipped_model": (Port(10, 20, 0), Port(90, 40, 90))}

    # 2. Route each scenario
    print("Routing Scenario 1 (Arc)...")
    res_arc = PathFinder(router_arc, evaluator, use_tiered_strategy=False).route_all(netlist_arc, {"arc_model": 2.0})

    print("Routing Scenario 2 (BBox)...")
    res_bbox = PathFinder(router_bbox, evaluator, use_tiered_strategy=False).route_all(netlist_bbox, {"bbox_model": 2.0})

    print("Routing Scenario 3 (Clipped BBox)...")
    res_clipped = PathFinder(router_clipped, evaluator, use_tiered_strategy=False).route_all(netlist_clipped, {"clipped_model": 2.0})

    # 3. Combine results for visualization
    all_results = {**res_arc, **res_bbox, **res_clipped}
    all_netlists = {**netlist_arc, **netlist_bbox, **netlist_clipped}

    # 4. Visualize
    # Note: plot_routing_results will show the 'collision geometry' used by the router
    # since that's what's stored in res.path[i].geometry
    fig, ax = plot_routing_results(all_results, obstacles, bounds, netlist=all_netlists)
    fig.savefig("examples/06_bend_collision_models.png")
    print("Saved plot to examples/06_bend_collision_models.png")


if __name__ == "__main__":
    main()
