From 6ec953b76e8167c775e4ad179b4411fadf723eba Mon Sep 17 00:00:00 2001
From: Jan Petykiewicz <jan@mpxd.net>
Date: Wed, 18 Mar 2026 20:45:16 -0700
Subject: [PATCH] improve heuristic

---
 inire/router/config.py |  1 +
 inire/router/cost.py   | 50 +++++++++++++++++++++++++++++++++++++++---
 2 files changed, 48 insertions(+), 3 deletions(-)

diff --git a/inire/router/config.py b/inire/router/config.py
index 00cc6ad..b240ba9 100644
--- a/inire/router/config.py
+++ b/inire/router/config.py
@@ -41,3 +41,4 @@ class CostConfig:
     congestion_penalty: float = 10000.0
     bend_penalty: float = 250.0
     sbend_penalty: float = 500.0
+    min_bend_radius: float = 50.0
diff --git a/inire/router/cost.py b/inire/router/cost.py
index d5f5825..0465bc4 100644
--- a/inire/router/cost.py
+++ b/inire/router/cost.py
@@ -2,6 +2,7 @@ from __future__ import annotations
 
 from typing import TYPE_CHECKING
 
+import numpy as np
 from inire.router.config import CostConfig
 
 if TYPE_CHECKING:
@@ -41,6 +42,7 @@ class CostEvaluator:
             congestion_penalty: float = 10000.0,
             bend_penalty: float = 250.0,
             sbend_penalty: float = 500.0,
+            min_bend_radius: float = 50.0,
             ) -> None:
         """
         Initialize the Cost Evaluator.
@@ -53,6 +55,7 @@ class CostEvaluator:
             congestion_penalty: Multiplier for path overlaps in negotiated congestion.
             bend_penalty: Base cost for 90-degree bends.
             sbend_penalty: Base cost for parametric S-bends.
+            min_bend_radius: Minimum radius for 90-degree bends (used for alignment heuristic).
         """
         self.collision_engine = collision_engine
         self.danger_map = danger_map
@@ -62,6 +65,7 @@ class CostEvaluator:
             congestion_penalty=congestion_penalty,
             bend_penalty=bend_penalty,
             sbend_penalty=sbend_penalty,
+            min_bend_radius=min_bend_radius,
         )
 
         # Use config values
@@ -90,10 +94,50 @@ class CostEvaluator:
         dy = abs(current.y - target.y)
         dist = dx + dy
 
+        bp = self.config.bend_penalty
         penalty = 0.0
-        if abs(current.orientation - target.orientation) > 0.1:
-            # Needs at least 1 bend
-            penalty += 10.0 + self.config.bend_penalty * 0.1
+
+        # 1. Orientation Difference
+        # diff in degrees, normalized to [0, 360)
+        diff = abs(current.orientation - target.orientation) % 360
+        if diff > 0.1:
+            if abs(diff - 180) < 0.1:
+                penalty += 2 * bp
+            else: # 90 or 270 degree rotation
+                penalty += 1 * bp
+
+        # 2. Side Check (Entry half-plane)
+        target_rad = np.radians(target.orientation)
+        # Vector from current to target
+        v_dx = target.x - current.x
+        v_dy = target.y - current.y
+        # Projection of current->target onto target orientation vector
+        # Should be positive if we are on the "entry" side of the port
+        side_proj = v_dx * np.cos(target_rad) + v_dy * np.sin(target_rad)
+        
+        # Perpendicular distance to the target's travel line
+        perp_dist = abs(v_dx * np.sin(target_rad) - v_dy * np.cos(target_rad))
+        min_radius = self.config.min_bend_radius
+        
+        if side_proj < -0.1 or (side_proj < min_radius and perp_dist > 0.1):
+            # Wrong side or too close to turn into port
+            penalty += 2 * bp
+
+        # 3. Traveling Away
+        curr_rad = np.radians(current.orientation)
+        # Projection of current->target onto current orientation vector
+        # Should be positive if we are moving towards the target's general location
+        move_proj = v_dx * np.cos(curr_rad) + v_dy * np.sin(curr_rad)
+        if move_proj < -0.1:
+            # Traveling away from the port
+            penalty += 2 * bp
+
+        # 4. Jog Alignment
+        # If orientations match, check if we are on the same line (jog alignment)
+        if diff < 0.1:
+            if perp_dist > 0.1:
+                # Same orientation but different jog coordinate needs 2 bends (S-turn)
+                penalty += 2 * bp
 
         return self.greedy_h_weight * (dist + penalty)