from inire.geometry.collision import CollisionEngine
from inire.geometry.primitives import Port
from inire.router.cost import CostEvaluator
from inire.router.danger_map import DangerMap


def test_cost_calculation() -> None:
    engine = CollisionEngine(clearance=2.0)
    # 50x50 um area, 1um resolution
    danger_map = DangerMap(bounds=(0, 0, 50, 50))
    danger_map.precompute([])
    evaluator = CostEvaluator(engine, danger_map)

    p1 = Port(0, 0, 0)
    p2 = Port(10, 10, 0)

    h = evaluator.h_manhattan(p1, p2)
    # Manhattan distance = 20. Orientation penalty = 0.
    # Weighted by 1.1 -> 22.0
    assert abs(h - 22.0) < 1e-6

    # Orientation penalty
    p3 = Port(10, 10, 90)
    h_wrong = evaluator.h_manhattan(p1, p3)
    assert h_wrong > h
