fix: Add vehicle capacity constraint, priority-aware routing, traffic integration, cost model to VRP solver

brain/app/services/route_optimization_engine.py

@@ -1,16 +1,19 @@
 """
-Route Optimization Engine — Full VRP/TSP Implementation
-========================================================
+Route Optimization Engine — Full VRP/TSP Implementation (v2)
+=============================================================
 
 Addresses Challenge #4: "Suboptimal route selection increasing total travel distance"
 
 Features:
 1. Multi-stop TSP within routes (OR-Tools Routing + 2-opt local search)
 2. Time window constraints per stop (AddDimension + SetRange)
-3. Before/after route comparison with distance savings
-4. DBSCAN clustering option (discovers K automatically, handles arbitrary shapes)
-5. Dynamic re-routing via cheapest-insertion heuristic
-6. Traffic-aware effective speed (via traffic_integration)
+3. Vehicle capacity constraint (AddDimension weight enforcement)
+4. Priority-aware routing (HIGH/EXPRESS stops penalized if placed late)
+5. Traffic-aware speed via OLA Maps integration
+6. Cost model (distance × fuel cost + toll + time-based labor)
+7. Before/after route comparison with distance/time/CO₂/cost savings
+8. DBSCAN clustering option (discovers K automatically, handles arbitrary shapes)
+9. Dynamic re-routing via cheapest-insertion heuristic
 
 This module is called AFTER clustering assigns packages to routes,
 to optimize the STOP ORDER within each route.
@@ -25,22 +28,53 @@ from dataclasses import dataclass, field
 logger = logging.getLogger("fairrelay.route_optimizer")
 
 
+# ═══════════════════════════════════════════════════════════════
+# COST MODEL CONSTANTS (Indian logistics, configurable)
+# ═══════════════════════════════════════════════════════════════
+
+FUEL_COST_PER_KM = 8.5         # ₹/km (diesel truck avg India 2026)
+TOLL_COST_PER_KM = 1.2         # ₹/km (avg toll on state highways)
+DRIVER_LABOR_PER_HOUR = 125.0  # ₹/hour (avg driver wage)
+CO2_KG_PER_KM = 0.21           # kg CO₂ per km (diesel)
+ROAD_FACTOR = 1.35             # Haversine to road distance multiplier (Indian roads)
+
+# Priority penalty multiplier for late delivery of high-priority items
+PRIORITY_PENALTY = {
+    "express": 3.0,   # 3x cost penalty if EXPRESS is placed late in route
+    "high": 2.0,      # 2x cost penalty if HIGH is placed late
+    "normal": 1.0,    # No penalty
+    "low": 0.8,       # Slight discount — can be delivered last
+}
+
+
 # ═══════════════════════════════════════════════════════════════
 # DATA STRUCTURES
 # ═══════════════════════════════════════════════════════════════
 
 @dataclass
 class Stop:
-    """A delivery stop with coordinates and optional time window."""
+    """A delivery stop with coordinates, capacity, time window, and priority."""
     id: str
     lat: float
     lng: float
     address: str = ""
     weight_kg: float = 0.0
-    service_time_min: float = 5.0  # Time spent at stop
+    volume_m3: float = 0.0
+    service_time_min: float = 5.0
     time_window_start: Optional[int] = None  # Minutes from route start
-    time_window_end: Optional[int] = None    # Minutes from route start
-    priority: str = "normal"
+    time_window_end: Optional[int] = None
+    priority: str = "normal"         # "express" | "high" | "normal" | "low"
+    is_hazmat: bool = False          # Hazardous material flag
+
+
+@dataclass
+class VehicleConfig:
+    """Vehicle capacity and cost configuration."""
+    max_weight_kg: float = 1000.0
+    max_volume_m3: float = 8.0
+    fuel_cost_per_km: float = FUEL_COST_PER_KM
+    co2_per_km: float = CO2_KG_PER_KM
+    vehicle_type: str = "diesel"     # "diesel" | "ev" | "cng"
 
 
 @dataclass
@@ -49,13 +83,17 @@ class RouteOptResult:
     ordered_stops: List[Stop]
     total_distance_km: float
     total_time_minutes: float
-    naive_distance_km: float       # Before optimization
-    distance_saved_km: float       # Improvement
-    distance_saved_pct: float      # % improvement
-    optimization_method: str       # "or_tools_vrp" | "2_opt" | "nearest_neighbor"
+    total_cost_inr: float            # NEW: Total route cost in ₹
+    naive_distance_km: float
+    distance_saved_km: float
+    distance_saved_pct: float
+    cost_saved_inr: float            # NEW: Cost savings
+    optimization_method: str
     time_windows_respected: bool
+    capacity_respected: bool         # NEW: Vehicle capacity check
+    priority_score: float            # NEW: 0-100 (how well priorities honored)
     num_stops: int
-    polyline_points: List[Tuple[float, float]]  # Ordered (lat, lng) for map
+    polyline_points: List[Tuple[float, float]]
 
 
 @dataclass
@@ -68,7 +106,7 @@ class RouteComparison:
 
 
 # ════���══════════════════════════════════════════════════════════
-# CORE: HAVERSINE DISTANCE
+# CORE: DISTANCE + COST
 # ═══════════════════════════════════════════════════════════════
 
 def haversine_km(lat1: float, lng1: float, lat2: float, lng2: float) -> float:
@@ -80,6 +118,35 @@ def haversine_km(lat1: float, lng1: float, lat2: float, lng2: float) -> float:
     return R * 2 * math.asin(math.sqrt(a))
 
 
+def road_distance_km(lat1: float, lng1: float, lat2: float, lng2: float) -> float:
+    """Estimated road distance (Haversine × road factor for India)."""
+    return haversine_km(lat1, lng1, lat2, lng2) * ROAD_FACTOR
+
+
+def compute_cost(distance_km: float, time_hours: float, vehicle: VehicleConfig = None) -> float:
+    """Compute route cost: fuel + toll + labor."""
+    v = vehicle or VehicleConfig()
+    fuel = distance_km * v.fuel_cost_per_km
+    toll = distance_km * TOLL_COST_PER_KM
+    labor = time_hours * DRIVER_LABOR_PER_HOUR
+    return round(fuel + toll + labor, 2)
+
+
+def get_traffic_speed(lat1: float, lng1: float, lat2: float, lng2: float) -> float:
+    """Get traffic-aware speed. Uses traffic_integration if available."""
+    try:
+        from app.services.traffic_integration import get_effective_speed
+        return get_effective_speed(lat1, lng1, lat2, lng2)
+    except (ImportError, Exception):
+        # Fallback: static Indian urban speed
+        hour = time.localtime().tm_hour
+        if 7 <= hour <= 10 or 17 <= hour <= 20:
+            return 18.0  # Peak hours
+        elif 22 <= hour or hour <= 5:
+            return 40.0  # Night
+        return 28.0  # Off-peak
+
+
 def build_distance_matrix(stops: List[Stop], depot_lat: float, depot_lng: float) -> List[List[int]]:
     """Build distance matrix (in meters) with depot at index 0."""
     all_points = [(depot_lat, depot_lng)] + [(s.lat, s.lng) for s in stops]
@@ -88,81 +155,148 @@ def build_distance_matrix(stops: List[Stop], depot_lat: float, depot_lng: float)
     for i in range(n):
         for j in range(n):
             if i != j:
-                d = haversine_km(all_points[i][0], all_points[i][1], all_points[j][0], all_points[j][1])
-                matrix[i][j] = int(d * 1000)  # Convert to meters for OR-Tools
+                d = road_distance_km(all_points[i][0], all_points[i][1], all_points[j][0], all_points[j][1])
+                matrix[i][j] = int(d * 1000)  # meters for OR-Tools
     return matrix
 
 
 # ═══════════════════════════════════════════════════════════════
-# METHOD 1: OR-TOOLS VRP WITH TIME WINDOWS
+# PRIORITY SCORING
+# ═══════════════════════════════════════════════════════════════
+
+def compute_priority_score(stops: List[Stop], order: List[int]) -> float:
+    """
+    Score how well priorities are honored (0-100).
+    HIGH/EXPRESS stops should be delivered EARLY in the route.
+    Score = 100 means all high-priority stops are in first half.
+    """
+    if not order:
+        return 100.0
+
+    n = len(order)
+    total_penalty = 0.0
+    max_penalty = 0.0
+
+    for position, idx in enumerate(order):
+        stop = stops[idx]
+        priority_weight = PRIORITY_PENALTY.get(stop.priority.lower(), 1.0)
+
+        if priority_weight > 1.0:
+            # High-priority stop — penalty increases with position
+            normalized_position = position / max(n - 1, 1)  # 0.0 (first) to 1.0 (last)
+            total_penalty += normalized_position * priority_weight
+            max_penalty += 1.0 * priority_weight  # Worst case: all at end
+
+    if max_penalty == 0:
+        return 100.0
+
+    # Invert: lower penalty = higher score
+    return round(max(0, (1 - total_penalty / max_penalty)) * 100, 1)
+
+
+# ═══════════════════════════════════════════════════════════════
+# METHOD 1: OR-TOOLS VRP WITH CAPACITY + TIME WINDOWS + PRIORITY
 # ═══════════════════════════════════════════════════════════════
 
 def solve_vrp_ortools(
     stops: List[Stop],
     depot_lat: float,
     depot_lng: float,
+    vehicle: VehicleConfig = None,
     speed_kmh: float = 30.0,
     max_time_seconds: int = 5,
 ) -> Optional[List[int]]:
     """
-    Solve TSP/VRP using OR-Tools Routing Library with time windows.
-    
-    Returns ordered indices into stops list, or None if solver fails.
+    Solve TSP/VRP using OR-Tools with:
+    - Distance minimization
+    - Time window constraints (AddDimension 'Time')
+    - Vehicle capacity constraint (AddDimension 'Capacity')
+    - Priority-aware arc costs (high-priority stops penalized if late)
     """
     try:
         from ortools.constraint_solver import routing_enums_pb2, pywrapcp
     except ImportError:
         return None
-    
+
+    v = vehicle or VehicleConfig()
     n = len(stops) + 1  # +1 for depot
     if n <= 2:
         return list(range(len(stops)))
-    
-    # Build distance matrix
+
     dist_matrix = build_distance_matrix(stops, depot_lat, depot_lng)
-    
-    # Create routing index manager (1 vehicle, depot at node 0)
+
     manager = pywrapcp.RoutingIndexManager(n, 1, 0)
     routing = pywrapcp.RoutingModel(manager)
-    
-    # Distance callback
+
+    # ── Distance callback with priority-aware cost ──
     def distance_callback(from_index, to_index):
         from_node = manager.IndexToNode(from_index)
         to_node = manager.IndexToNode(to_index)
-        return dist_matrix[from_node][to_node]
-    
+        base_cost = dist_matrix[from_node][to_node]
+
+        # Apply priority penalty: delivering high-priority late costs more
+        if to_node > 0:
+            stop = stops[to_node - 1]
+            multiplier = PRIORITY_PENALTY.get(stop.priority.lower(), 1.0)
+            # Only penalize if this would be a "late" delivery (heuristic: further from depot)
+            if multiplier > 1.0:
+                depot_dist = dist_matrix[0][to_node]
+                if base_cost > depot_dist * 0.5:
+                    base_cost = int(base_cost * (1 + (multiplier - 1) * 0.3))
+
+        return base_cost
+
     transit_callback_index = routing.RegisterTransitCallback(distance_callback)
     routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)
-    
-    # Time dimension (for time windows)
+
+    # ── Capacity dimension (weight) ──
+    total_weight = sum(s.weight_kg for s in stops)
+    if total_weight > 0:
+        def demand_callback(from_index):
+            node = manager.IndexToNode(from_index)
+            if node == 0:
+                return 0  # Depot has no demand
+            return int(stops[node - 1].weight_kg * 100)  # Scale to int (100g units)
+
+        demand_callback_index = routing.RegisterUnaryTransitCallback(demand_callback)
+        routing.AddDimensionWithVehicleCapacity(
+            demand_callback_index,
+            0,  # No slack
+            [int(v.max_weight_kg * 100)],  # Vehicle capacity (in 100g units)
+            True,  # Start cumul at zero
+            'Capacity'
+        )
+
+    # ── Time dimension (for time windows) ──
     has_time_windows = any(s.time_window_start is not None for s in stops)
-    
+
+    def time_callback(from_index, to_index):
+        from_node = manager.IndexToNode(from_index)
+        to_node = manager.IndexToNode(to_index)
+        dist_km = dist_matrix[from_node][to_node] / 1000
+        # Use traffic-aware speed
+        if from_node > 0 and to_node > 0:
+            spd = get_traffic_speed(stops[from_node-1].lat, stops[from_node-1].lng, stops[to_node-1].lat, stops[to_node-1].lng)
+        else:
+            spd = speed_kmh
+        travel_min = (dist_km / max(spd, 5)) * 60
+        if to_node > 0:
+            travel_min += stops[to_node - 1].service_time_min
+        return int(travel_min)
+
+    time_callback_index = routing.RegisterTransitCallback(time_callback)
+
+    max_route_time = 720  # 12 hours
+    routing.AddDimension(
+        time_callback_index,
+        30,             # Slack
+        max_route_time,
+        False,
+        'Time'
+    )
+    time_dimension = routing.GetDimensionOrDie('Time')
+
     if has_time_windows:
-        def time_callback(from_index, to_index):
-            from_node = manager.IndexToNode(from_index)
-            to_node = manager.IndexToNode(to_index)
-            # Travel time in minutes
-            dist_km = dist_matrix[from_node][to_node] / 1000
-            travel_min = (dist_km / speed_kmh) * 60
-            # Add service time at destination
-            if to_node > 0:
-                travel_min += stops[to_node - 1].service_time_min
-            return int(travel_min)
-        
-        time_callback_index = routing.RegisterTransitCallback(time_callback)
-        
-        # Add time dimension
-        max_route_time = 720  # 12 hours max
-        routing.AddDimension(
-            time_callback_index,
-            30,             # Slack (waiting time allowed)
-            max_route_time, # Max cumulative time
-            False,          # Don't force start at 0
-            'Time'
-        )
-        time_dimension = routing.GetDimensionOrDie('Time')
-        
-        # Set time windows
         for i, stop in enumerate(stops):
             node_index = manager.NodeToIndex(i + 1)
             if stop.time_window_start is not None and stop.time_window_end is not None:
@@ -170,34 +304,32 @@ def solve_vrp_ortools(
                     int(stop.time_window_start),
                     int(stop.time_window_end)
                 )
-        
-        # Depot time window (start at 0, end at max)
-        time_dimension.CumulVar(routing.Start(0)).SetRange(0, max_route_time)
-    
-    # Solve with time limit
+
+    time_dimension.CumulVar(routing.Start(0)).SetRange(0, max_route_time)
+
+    # ── Solve ──
     search_params = pywrapcp.DefaultRoutingSearchParameters()
     search_params.first_solution_strategy = routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC
     search_params.local_search_metaheuristic = routing_enums_pb2.LocalSearchMetaheuristic.GUIDED_LOCAL_SEARCH
     search_params.time_limit.FromSeconds(max_time_seconds)
-    
+
     solution = routing.SolveWithParameters(search_params)
-    
+
     if solution:
-        # Extract route order
         order = []
         index = routing.Start(0)
         while not routing.IsEnd(index):
             node = manager.IndexToNode(index)
-            if node > 0:  # Skip depot
-                order.append(node - 1)  # Map back to stops index
+            if node > 0:
+                order.append(node - 1)
             index = solution.Value(routing.NextVar(index))
         return order
-    
+
     return None
 
 
 # ═══════════════════════════════════════════════════════════════
-# METHOD 2: 2-OPT LOCAL SEARCH IMPROVEMENT
+# METHOD 2: 2-OPT LOCAL SEARCH (PRIORITY-AWARE)
 # ═══════════════════════════════════════════════════════════════
 
 def two_opt_improve(
@@ -208,67 +340,78 @@ def two_opt_improve(
     max_iterations: int = 1000,
 ) -> List[int]:
     """
-    Apply 2-opt improvement to a route order.
-    
-    2-opt reverses segments of the route to reduce total distance.
-    Guaranteed to converge to a local optimum.
+    2-opt improvement with priority-aware cost function.
+    High-priority stops incur penalty when placed late.
     """
-    def route_distance(order: List[int]) -> float:
-        """Total route distance including depot→first and last→depot."""
+    def route_cost(order: List[int]) -> float:
         if not order:
             return 0.0
-        # Depot to first stop
-        total = haversine_km(depot_lat, depot_lng, stops[order[0]].lat, stops[order[0]].lng)
-        # Between stops
+        total = road_distance_km(depot_lat, depot_lng, stops[order[0]].lat, stops[order[0]].lng)
         for i in range(len(order) - 1):
-            total += haversine_km(stops[order[i]].lat, stops[order[i]].lng, stops[order[i+1]].lat, stops[order[i+1]].lng)
-        # Last stop back to depot
-        total += haversine_km(stops[order[-1]].lat, stops[order[-1]].lng, depot_lat, depot_lng)
+            total += road_distance_km(stops[order[i]].lat, stops[order[i]].lng, stops[order[i+1]].lat, stops[order[i+1]].lng)
+        total += road_distance_km(stops[order[-1]].lat, stops[order[-1]].lng, depot_lat, depot_lng)
+
+        # Priority penalty: HIGH/EXPRESS stops later = higher cost
+        n = len(order)
+        for pos, idx in enumerate(order):
+            mult = PRIORITY_PENALTY.get(stops[idx].priority.lower(), 1.0)
+            if mult > 1.0:
+                position_factor = pos / max(n - 1, 1)
+                total += position_factor * mult * 0.5  # Small penalty in km-equivalent
+
         return total
-    
+
     best_order = list(initial_order)
-    best_dist = route_distance(best_order)
+    best_cost = route_cost(best_order)
     improved = True
     iterations = 0
-    
+
     while improved and iterations < max_iterations:
         improved = False
         iterations += 1
         for i in range(len(best_order) - 1):
             for j in range(i + 1, len(best_order)):
-                # Reverse segment [i:j+1]
                 new_order = best_order[:i] + best_order[i:j+1][::-1] + best_order[j+1:]
-                new_dist = route_distance(new_order)
-                if new_dist < best_dist - 0.01:  # 10m improvement threshold
+                new_cost = route_cost(new_order)
+                if new_cost < best_cost - 0.01:
                     best_order = new_order
-                    best_dist = new_dist
+                    best_cost = new_cost
                     improved = True
                     break
             if improved:
                 break
-    
+
     return best_order
 
 
 # ═══════════════════════════════════════════════════════════════
-# METHOD 3: NEAREST NEIGHBOR (BASELINE)
+# METHOD 3: NEAREST NEIGHBOR (PRIORITY-FIRST)
 # ═══════════════════════════════════════════════════════════════
 
 def nearest_neighbor_order(stops: List[Stop], depot_lat: float, depot_lng: float) -> List[int]:
-    """Simple nearest-neighbor heuristic as baseline."""
+    """
+    Priority-aware nearest-neighbor: HIGH/EXPRESS stops get preference
+    when multiple stops are roughly equidistant.
+    """
     if not stops:
         return []
-    
+
     remaining = list(range(len(stops)))
     order = []
     curr_lat, curr_lng = depot_lat, depot_lng
-    
+
     while remaining:
-        nearest_idx = min(remaining, key=lambda i: haversine_km(curr_lat, curr_lng, stops[i].lat, stops[i].lng))
-        order.append(nearest_idx)
-        remaining.remove(nearest_idx)
-        curr_lat, curr_lng = stops[nearest_idx].lat, stops[nearest_idx].lng
-    
+        # Score = distance / priority_weight (lower = better)
+        def score(i):
+            dist = road_distance_km(curr_lat, curr_lng, stops[i].lat, stops[i].lng)
+            priority_boost = PRIORITY_PENALTY.get(stops[i].priority.lower(), 1.0)
+            return dist / max(priority_boost, 0.5)
+
+        best_idx = min(remaining, key=score)
+        order.append(best_idx)
+        remaining.remove(best_idx)
+        curr_lat, curr_lng = stops[best_idx].lat, stops[best_idx].lng
+
     return order
 
 
@@ -283,136 +426,150 @@ def cheapest_insertion(
     depot_lat: float,
     depot_lng: float,
 ) -> List[int]:
-    """
-    Insert a new stop into an existing route at the cheapest position.
-    Used for dynamic re-routing when new packages arrive mid-route.
-    """
+    """Insert a new stop at the cheapest position (priority-aware)."""
     if not existing_order:
         return [new_stop_idx]
-    
-    def segment_cost(from_lat, from_lng, to_lat, to_lng):
-        return haversine_km(from_lat, from_lng, to_lat, to_lng)
-    
+
     new_stop = stops[new_stop_idx]
     best_position = 0
     best_cost_increase = float('inf')
-    
-    # Try inserting at each position
+
+    # High-priority stops prefer earlier positions
+    priority_mult = PRIORITY_PENALTY.get(new_stop.priority.lower(), 1.0)
+
     for pos in range(len(existing_order) + 1):
         if pos == 0:
             prev_lat, prev_lng = depot_lat, depot_lng
         else:
             prev_stop = stops[existing_order[pos - 1]]
             prev_lat, prev_lng = prev_stop.lat, prev_stop.lng
-        
+
         if pos == len(existing_order):
             next_lat, next_lng = depot_lat, depot_lng
         else:
             next_stop = stops[existing_order[pos]]
             next_lat, next_lng = next_stop.lat, next_stop.lng
-        
-        # Cost of current segment (prev → next)
-        current_cost = segment_cost(prev_lat, prev_lng, next_lat, next_lng)
-        # Cost after insertion (prev → new → next)
-        new_cost = (segment_cost(prev_lat, prev_lng, new_stop.lat, new_stop.lng) +
-                    segment_cost(new_stop.lat, new_stop.lng, next_lat, next_lng))
-        
+
+        current_cost = road_distance_km(prev_lat, prev_lng, next_lat, next_lng)
+        new_cost = (road_distance_km(prev_lat, prev_lng, new_stop.lat, new_stop.lng) +
+                    road_distance_km(new_stop.lat, new_stop.lng, next_lat, next_lng))
+
         cost_increase = new_cost - current_cost
+        # Priority discount for earlier positions
+        if priority_mult > 1.0:
+            position_penalty = pos / max(len(existing_order), 1) * (priority_mult - 1)
+            cost_increase += position_penalty
+
         if cost_increase < best_cost_increase:
             best_cost_increase = cost_increase
             best_position = pos
-    
+
     result = list(existing_order)
     result.insert(best_position, new_stop_idx)
     return result
 
 
 # ═══════════════════════════════════════════════════════════════
-# MAIN OPTIMIZER: COMBINES ALL METHODS
+# MAIN OPTIMIZER
 # ═══════════════════════════════════════════════════════════════
 
 def optimize_route(
     stops: List[Stop],
     depot_lat: float,
     depot_lng: float,
-    speed_kmh: float = 30.0,
+    vehicle: VehicleConfig = None,
+    speed_kmh: float = None,
     use_time_windows: bool = True,
     max_solver_time: int = 5,
 ) -> RouteOptResult:
     """
     Full route optimization pipeline:
-    1. Try OR-Tools VRP with time windows (best quality)
+    1. Try OR-Tools VRP with capacity + time windows + priority
     2. Apply 2-opt local search improvement
-    3. Fallback: nearest-neighbor + 2-opt
-    
-    Returns optimized route with before/after comparison.
+    3. Fallback: priority-aware nearest-neighbor + 2-opt
+    4. Compute cost model (fuel + toll + labor)
+    5. Check capacity and priority compliance
     """
+    v = vehicle or VehicleConfig()
+
     if not stops:
         return RouteOptResult(
-            ordered_stops=[], total_distance_km=0, total_time_minutes=0,
-            naive_distance_km=0, distance_saved_km=0, distance_saved_pct=0,
+            ordered_stops=[], total_distance_km=0, total_time_minutes=0, total_cost_inr=0,
+            naive_distance_km=0, distance_saved_km=0, distance_saved_pct=0, cost_saved_inr=0,
             optimization_method="empty", time_windows_respected=True,
+            capacity_respected=True, priority_score=100.0,
             num_stops=0, polyline_points=[],
         )
-    
-    t0 = time.time()
-    
-    # Step 1: Compute naive (input order) distance as baseline
+
+    # Get traffic-aware speed
+    if speed_kmh is None:
+        speed_kmh = get_traffic_speed(depot_lat, depot_lng, stops[0].lat, stops[0].lng)
+
+    # Step 1: Naive baseline
     naive_order = list(range(len(stops)))
     naive_dist = _compute_route_distance(stops, naive_order, depot_lat, depot_lng)
-    
-    # Step 2: Try OR-Tools VRP
+
+    # Step 2: OR-Tools VRP with capacity + time + priority
     method = "nearest_neighbor"
     ortools_order = None
-    
+
     if len(stops) >= 3:
-        ortools_order = solve_vrp_ortools(
-            stops, depot_lat, depot_lng, speed_kmh, max_solver_time
-        )
-    
+        ortools_order = solve_vrp_ortools(stops, depot_lat, depot_lng, v, speed_kmh, max_solver_time)
+
     if ortools_order:
         method = "or_tools_vrp"
         best_order = ortools_order
     else:
-        # Fallback: nearest neighbor
         best_order = nearest_neighbor_order(stops, depot_lat, depot_lng)
-    
-    # Step 3: Apply 2-opt improvement (always)
+
+    # Step 3: 2-opt improvement
     if len(stops) >= 4:
         improved_order = two_opt_improve(stops, depot_lat, depot_lng, best_order)
-        improved_dist = _compute_route_distance(stops, improved_order, depot_lat, depot_lng)
-        current_dist = _compute_route_distance(stops, best_order, depot_lat, depot_lng)
-        
-        if improved_dist < current_dist:
+        if _compute_route_distance(stops, improved_order, depot_lat, depot_lng) < _compute_route_distance(stops, best_order, depot_lat, depot_lng):
             best_order = improved_order
             method = f"{method}+2opt"
-    
-    # Step 4: Compute final metrics
+
+    # Step 4: Compute metrics
     optimized_dist = _compute_route_distance(stops, best_order, depot_lat, depot_lng)
     distance_saved = naive_dist - optimized_dist
     saved_pct = (distance_saved / naive_dist * 100) if naive_dist > 0 else 0
-    
-    # Compute total time (distance/speed + service times)
-    total_time = (optimized_dist / speed_kmh) * 60 + sum(stops[i].service_time_min for i in best_order)
-    
-    # Check time windows respected
+
+    total_time_hours = (optimized_dist / max(speed_kmh, 5)) + sum(stops[i].service_time_min for i in best_order) / 60
+    total_time_min = total_time_hours * 60
+
+    # Cost model
+    optimized_cost = compute_cost(optimized_dist, total_time_hours, v)
+    naive_time_hours = (naive_dist / max(speed_kmh, 5)) + sum(s.service_time_min for s in stops) / 60
+    naive_cost = compute_cost(naive_dist, naive_time_hours, v)
+    cost_saved = naive_cost - optimized_cost
+
+    # Capacity check
+    total_weight = sum(stops[i].weight_kg for i in best_order)
+    capacity_ok = total_weight <= v.max_weight_kg
+
+    # Priority score
+    priority_score = compute_priority_score(stops, best_order)
+
+    # Time windows check
     tw_respected = _check_time_windows(stops, best_order, depot_lat, depot_lng, speed_kmh)
-    
-    # Build polyline
+
+    # Polyline
     polyline = [(depot_lat, depot_lng)] + [(stops[i].lat, stops[i].lng) for i in best_order] + [(depot_lat, depot_lng)]
-    
-    # Order stops
     ordered_stops = [stops[i] for i in best_order]
-    
+
     return RouteOptResult(
         ordered_stops=ordered_stops,
         total_distance_km=round(optimized_dist, 2),
-        total_time_minutes=round(total_time, 1),
+        total_time_minutes=round(total_time_min, 1),
+        total_cost_inr=optimized_cost,
         naive_distance_km=round(naive_dist, 2),
         distance_saved_km=round(max(0, distance_saved), 2),
         distance_saved_pct=round(max(0, saved_pct), 1),
+        cost_saved_inr=round(max(0, cost_saved), 2),
         optimization_method=method,
         time_windows_respected=tw_respected,
+        capacity_respected=capacity_ok,
+        priority_score=priority_score,
         num_stops=len(stops),
         polyline_points=polyline,
     )
@@ -422,26 +579,16 @@ def compare_routes(
     routes: List[Dict[str, Any]],
     depot_lat: float,
     depot_lng: float,
-    speed_kmh: float = 30.0,
+    speed_kmh: float = None,
+    vehicle: VehicleConfig = None,
 ) -> List[RouteComparison]:
-    """
-    Generate before/after route comparison for Challenge #4.
-    
-    Args:
-        routes: List of route dicts with "stops" (list of stop dicts)
-        depot_lat, depot_lng: Warehouse coordinates
-        speed_kmh: Average speed
-    
-    Returns:
-        List of RouteComparison objects showing improvement per route
-    """
+    """Generate before/after route comparison for Challenge #4."""
     comparisons = []
-    
+
     for route in routes:
         route_id = route.get("id", f"route_{len(comparisons)}")
         raw_stops = route.get("stops", route.get("packages", []))
-        
-        # Convert to Stop objects
+
         stops = [
             Stop(
                 id=s.get("id", f"stop_{i}"),
@@ -449,49 +596,63 @@ def compare_routes(
                 lng=s.get("longitude", s.get("lng", 0)),
                 address=s.get("address", ""),
                 weight_kg=s.get("weight_kg", 0),
+                volume_m3=s.get("volume_m3", 0),
                 service_time_min=s.get("service_time_min", 5),
                 time_window_start=s.get("time_window_start"),
                 time_window_end=s.get("time_window_end"),
                 priority=s.get("priority", "normal"),
+                is_hazmat=s.get("is_hazmat", False),
             )
             for i, s in enumerate(raw_stops)
         ]
-        
+
         if not stops:
             continue
-        
-        # Before: naive order (as received)
+
+        spd = speed_kmh or get_traffic_speed(depot_lat, depot_lng, stops[0].lat, stops[0].lng)
+        v = vehicle or VehicleConfig()
+
+        # Before
         naive_dist = _compute_route_distance(stops, list(range(len(stops))), depot_lat, depot_lng)
-        naive_time = (naive_dist / speed_kmh) * 60 + sum(s.service_time_min for s in stops)
-        
-        # After: optimized
-        result = optimize_route(stops, depot_lat, depot_lng, speed_kmh)
-        
+        naive_time_h = (naive_dist / max(spd, 5)) + sum(s.service_time_min for s in stops) / 60
+        naive_cost = compute_cost(naive_dist, naive_time_h, v)
+        naive_priority = compute_priority_score(stops, list(range(len(stops))))
+
+        # After
+        result = optimize_route(stops, depot_lat, depot_lng, v, spd)
+
         comparisons.append(RouteComparison(
             route_id=route_id,
             before={
                 "distance_km": round(naive_dist, 2),
-                "time_minutes": round(naive_time, 1),
+                "time_minutes": round(naive_time_h * 60, 1),
+                "cost_inr": naive_cost,
+                "co2_kg": round(naive_dist * CO2_KG_PER_KM, 2),
+                "priority_score": naive_priority,
                 "stop_order": [s.id for s in stops],
-                "co2_kg": round(naive_dist * 0.21, 2),
             },
             after={
                 "distance_km": result.total_distance_km,
                 "time_minutes": result.total_time_minutes,
+                "cost_inr": result.total_cost_inr,
+                "co2_kg": round(result.total_distance_km * CO2_KG_PER_KM, 2),
+                "priority_score": result.priority_score,
                 "stop_order": [s.id for s in result.ordered_stops],
-                "co2_kg": round(result.total_distance_km * 0.21, 2),
                 "method": result.optimization_method,
+                "capacity_ok": result.capacity_respected,
                 "polyline": result.polyline_points,
             },
             improvement={
                 "distance_saved_km": result.distance_saved_km,
                 "distance_saved_pct": result.distance_saved_pct,
-                "time_saved_minutes": round(naive_time - result.total_time_minutes, 1),
-                "co2_saved_kg": round((naive_dist - result.total_distance_km) * 0.21, 2),
+                "time_saved_minutes": round(naive_time_h * 60 - result.total_time_minutes, 1),
+                "cost_saved_inr": result.cost_saved_inr,
+                "co2_saved_kg": round((naive_dist - result.total_distance_km) * CO2_KG_PER_KM, 2),
+                "priority_improved": result.priority_score > naive_priority,
                 "time_windows_respected": result.time_windows_respected,
             },
         ))
-    
+
     return comparisons
 
 
@@ -506,97 +667,78 @@ def cluster_packages_dbscan(
     max_cluster_size: int = 30,
 ) -> List[List[Dict[str, Any]]]:
     """
-    DBSCAN-based clustering that discovers K automatically.
-    Handles arbitrary cluster shapes (unlike KMeans).
-    
-    Fixes: Noise points (-1 label) are merged into nearest cluster.
-    
-    Args:
-        packages: List of package dicts with latitude, longitude
-        eps_km: Max distance between points in same cluster (km)
-        min_samples: Min points to form a cluster
-        max_cluster_size: Split clusters exceeding this
-    
-    Returns:
-        List of clusters (each cluster is a list of package dicts)
+    DBSCAN clustering — auto K, arbitrary shapes, noise merged.
+    Hazmat packages are isolated into their own clusters.
     """
     import numpy as np
     from sklearn.cluster import DBSCAN
-    from sklearn.metrics.pairwise import haversine_distances
-    
+
     if not packages:
         return []
-    
     if len(packages) <= min_samples:
         return [packages]
-    
-    # Convert to radians for haversine
+
+    # Separate hazmat packages (must be isolated)
+    hazmat = [p for p in packages if p.get("is_hazmat", False)]
+    normal = [p for p in packages if not p.get("is_hazmat", False)]
+
+    if not normal:
+        return [[p] for p in hazmat]  # Each hazmat gets its own route
+
     coords_rad = np.array([
         [math.radians(p["latitude"]), math.radians(p["longitude"])]
-        for p in packages
+        for p in normal
     ])
-    
-    # eps in radians (eps_km / Earth radius)
+
     eps_rad = eps_km / 6371.0
-    
-    # Run DBSCAN
     db = DBSCAN(eps=eps_rad, min_samples=min_samples, metric='haversine')
     labels = db.fit_predict(coords_rad)
-    
-    # Group by cluster
+
     clusters: Dict[int, List[int]] = {}
     noise_indices: List[int] = []
-    
+
     for idx, label in enumerate(labels):
         if label == -1:
             noise_indices.append(idx)
         else:
-            if label not in clusters:
-                clusters[label] = []
-            clusters[label].append(idx)
-    
-    # FIX: Merge noise points into nearest cluster
+            clusters.setdefault(label, []).append(idx)
+
+    # Merge noise into nearest cluster
     if noise_indices and clusters:
-        # Compute cluster centroids
-        cluster_centroids = {}
+        centroids = {}
         for label, indices in clusters.items():
-            lats = [packages[i]["latitude"] for i in indices]
-            lngs = [packages[i]["longitude"] for i in indices]
-            cluster_centroids[label] = (sum(lats)/len(lats), sum(lngs)/len(lngs))
-        
-        for noise_idx in noise_indices:
-            pkg = packages[noise_idx]
-            # Find nearest cluster
-            nearest_label = min(
-                cluster_centroids.keys(),
-                key=lambda l: haversine_km(
-                    pkg["latitude"], pkg["longitude"],
-                    cluster_centroids[l][0], cluster_centroids[l][1]
-                )
-            )
-            clusters[nearest_label].append(noise_idx)
-    elif noise_indices and not clusters:
-        # All points are noise — treat as one cluster
+            lats = [normal[i]["latitude"] for i in indices]
+            lngs = [normal[i]["longitude"] for i in indices]
+            centroids[label] = (sum(lats)/len(lats), sum(lngs)/len(lngs))
+
+        for ni in noise_indices:
+            pkg = normal[ni]
+            nearest = min(centroids.keys(), key=lambda l: haversine_km(pkg["latitude"], pkg["longitude"], centroids[l][0], centroids[l][1]))
+            clusters[nearest].append(ni)
+    elif noise_indices:
         clusters[0] = noise_indices
-    
-    # Split oversized clusters
-    final_clusters = []
+
+    # Split oversized + build final
+    final = []
     for label, indices in clusters.items():
         if len(indices) <= max_cluster_size:
-            final_clusters.append([packages[i] for i in indices])
+            final.append([normal[i] for i in indices])
         else:
-            # Split using KMeans
             from sklearn.cluster import KMeans
-            sub_coords = np.array([[packages[i]["latitude"], packages[i]["longitude"]] for i in indices])
+            sub_coords = np.array([[normal[i]["latitude"], normal[i]["longitude"]] for i in indices])
             n_sub = max(2, len(indices) // max_cluster_size + 1)
             km = KMeans(n_clusters=n_sub, random_state=42, n_init=5)
             sub_labels = km.fit_predict(sub_coords)
             for sl in range(n_sub):
-                sub_indices = [indices[j] for j in range(len(indices)) if sub_labels[j] == sl]
-                if sub_indices:
-                    final_clusters.append([packages[i] for i in sub_indices])
-    
-    return final_clusters
+                sub = [indices[j] for j in range(len(indices)) if sub_labels[j] == sl]
+                if sub:
+                    final.append([normal[i] for i in sub])
+
+    # Add hazmat as separate clusters
+    for h in hazmat:
+        final.append([h])
+
+    return final
 
 
 # ═══════════════════════════════════════════════════════════════
@@ -604,41 +746,36 @@ def cluster_packages_dbscan(
 # ═══════════════════════════════════════════════════════════════
 
 def _compute_route_distance(stops: List[Stop], order: List[int], depot_lat: float, depot_lng: float) -> float:
-    """Compute total route distance for a given stop order."""
+    """Compute total road distance for a given stop order."""
     if not order:
         return 0.0
-    total = haversine_km(depot_lat, depot_lng, stops[order[0]].lat, stops[order[0]].lng)
+    total = road_distance_km(depot_lat, depot_lng, stops[order[0]].lat, stops[order[0]].lng)
     for i in range(len(order) - 1):
-        total += haversine_km(stops[order[i]].lat, stops[order[i]].lng, stops[order[i+1]].lat, stops[order[i+1]].lng)
-    total += haversine_km(stops[order[-1]].lat, stops[order[-1]].lng, depot_lat, depot_lng)
+        total += road_distance_km(stops[order[i]].lat, stops[order[i]].lng, stops[order[i+1]].lat, stops[order[i+1]].lng)
+    total += road_distance_km(stops[order[-1]].lat, stops[order[-1]].lng, depot_lat, depot_lng)
     return total
 
 
 def _check_time_windows(stops: List[Stop], order: List[int], depot_lat: float, depot_lng: float, speed_kmh: float) -> bool:
-    """Check if all time windows are respected in the given order."""
+    """Check if all time windows are respected."""
     if not order:
         return True
-    
-    current_time = 0.0  # Minutes from departure
+
+    current_time = 0.0
     current_lat, current_lng = depot_lat, depot_lng
-    
+
     for idx in order:
         stop = stops[idx]
-        # Travel time to this stop
-        dist = haversine_km(current_lat, current_lng, stop.lat, stop.lng)
-        travel_time = (dist / speed_kmh) * 60
+        dist = road_distance_km(current_lat, current_lng, stop.lat, stop.lng)
+        travel_time = (dist / max(speed_kmh, 5)) * 60
         current_time += travel_time
-        
-        # Check time window
+
         if stop.time_window_end is not None and current_time > stop.time_window_end:
             return False
-        
-        # Wait if arrived too early
         if stop.time_window_start is not None and current_time < stop.time_window_start:
             current_time = stop.time_window_start
-        
-        # Service time
+
         current_time += stop.service_time_min
         current_lat, current_lng = stop.lat, stop.lng
-    
+
     return True