from shapely.geometry import Polygon

from inire import CongestionOptions, NetSpec, RoutingOptions, RoutingProblem, SearchOptions
from inire.geometry.collision import RoutingWorld
from inire.geometry.primitives import Port
from inire.router._astar_types import AStarContext, AStarMetrics
from inire.router._router import PathFinder
from inire.router.cost import CostEvaluator
from inire.router.danger_map import DangerMap
from inire.utils.visualization import plot_routing_results


def main() -> None:
    print("Running Example 08: Custom Bend Geometry...")

    bounds = (0, 0, 150, 150)
    engine = RoutingWorld(clearance=2.0)
    danger_map = DangerMap(bounds=bounds)
    danger_map.precompute([])
    evaluator = CostEvaluator(engine, danger_map, bend_penalty=50.0, sbend_penalty=150.0)
    metrics = AStarMetrics()
    start = Port(20, 20, 0)
    target = Port(100, 100, 90)

    print("Routing with standard arc...")
    results_std = PathFinder(
        AStarContext(
            evaluator,
            RoutingProblem(
                bounds=bounds,
                nets=(NetSpec("custom_bend", start, target, width=2.0),),
            ),
            RoutingOptions(
                search=SearchOptions(bend_radii=(10.0,), sbend_radii=()),
                congestion=CongestionOptions(max_iterations=1),
            ),
        ),
        metrics=metrics,
    ).route_all()

    custom_poly = Polygon([(-10, -10), (10, -10), (10, 10), (-10, 10)])

    print("Routing with custom collision model...")
    results_custom = PathFinder(
        AStarContext(
            evaluator,
            RoutingProblem(
                bounds=bounds,
                nets=(NetSpec("custom_model", start, target, width=2.0),),
            ),
            RoutingOptions(
                search=SearchOptions(
                    bend_radii=(10.0,),
                    bend_collision_type=custom_poly,
                    sbend_radii=(),
                ),
                congestion=CongestionOptions(max_iterations=1, use_tiered_strategy=False),
            ),
        ),
        metrics=AStarMetrics(),
        use_tiered_strategy=False,
    ).route_all()

    all_results = {**results_std, **results_custom}
    fig, _ax = plot_routing_results(
        all_results,
        [],
        bounds,
        netlist={
            "custom_bend": (start, target),
            "custom_model": (start, target),
        },
    )
    fig.savefig("examples/08_custom_bend_geometry.png")
    print("Saved plot to examples/08_custom_bend_geometry.png")


if __name__ == "__main__":
    main()