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.config import CostConfig, RouterConfig
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 04: S-Bends and Multiple Radii...")

    # 1. Setup Environment
    bounds = (0, 0, 150, 100)
    engine = CollisionEngine(clearance=2.0)
    danger_map = DangerMap(bounds=bounds)
    
    # Create obstacles that force S-bends and turns
    # Obstacle 1: Forces a vertical jog (S-bend)
    obs1 = Polygon([(40, 20), (60, 20), (60, 60), (40, 60)])
    
    # Obstacle 2: Forces a large radius turn
    obs2 = Polygon([(80, 0), (100, 0), (100, 40), (80, 40)])
    
    obstacles = [obs1, obs2]
    for obs in obstacles:
        engine.add_static_obstacle(obs)
    
    danger_map.precompute(obstacles)
    
    # 2. Configure Router with custom parameters (Directly via constructor)
    evaluator = CostEvaluator(
        engine, 
        danger_map,
        unit_length_cost=1.0,
        greedy_h_weight=1.2,
    )
    
    router = AStarRouter(
        evaluator,
        node_limit=500000,
        bend_radii=[10.0, 30.0],  # Allow standard and large bends
        sbend_offsets=[-10.0, -5.0, 5.0, 10.0], # Allow larger S-bend offsets
        sbend_radii=[20.0, 50.0], # Large S-bends
        bend_penalty=10.0, # Lower penalty to encourage using the right bend
    )
    
    pf = PathFinder(router, evaluator)
    
    # 3. Define Netlist
    # Net 1: Needs to S-bend around obs1 (gap at y=60-100? No, obs1 is y=20-60).
    # Start at (10, 40), End at (140, 40).
    # Obstacle 1 blocks 40-60. Net must go above or below.
    # Obstacle 2 blocks 80-100 x 0-40.
    
    # Let's force a path that requires a large bend.
    netlist = {
        "large_bend_net": (Port(10, 10, 0), Port(140, 80, 0)),
        "sbend_net": (Port(10, 50, 0), Port(70, 70, 0)), 
    }
    net_widths = {"large_bend_net": 2.0, "sbend_net": 2.0}

    # 4. Route
    results = pf.route_all(netlist, net_widths)

    # 5. Check Results
    for nid, res in results.items():
        status = "Success" if res.is_valid else "Failed"
        print(f"{nid}: {status}, collisions={res.collisions}")

    # 6. Visualize
    fig, ax = plot_routing_results(results, obstacles, bounds)
    fig.savefig("examples/sbends_radii.png")
    print("Saved plot to examples/sbends_radii.png")


if __name__ == "__main__":
    main()