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([])
    # Use small penalties for testing
    evaluator = CostEvaluator(engine, danger_map, bend_penalty=10.0)

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

    h = evaluator.h_manhattan(p1, p2)
    # Manhattan distance = 20. 
    # Jog alignment penalty = 2*bp = 20.
    # Side check penalty = 2*bp = 20.
    # Total = 1.1 * (20 + 40) = 66.0
    assert abs(h - 66.0) < 1e-6

    # Orientation difference
    p3 = Port(10, 10, 90)
    h_90 = evaluator.h_manhattan(p1, p3)
    # diff = 90. penalty += 1*bp = 10.
    # Side check: 2*bp = 20. (Total penalty = 30)
    # Total = 1.1 * (20 + 30) = 55.0
    assert abs(h_90 - 55.0) < 1e-6

    # Traveling away
    p4 = Port(10, 10, 180)
    h_away = evaluator.h_manhattan(p1, p4)
    # diff = 180. penalty += 2*bp = 20.
    # Side check: 2*bp = 20.
    # Total = 1.1 * (20 + 40) = 66.0
    assert h_away >= h_90
