Retry with (and len(seg) < best_len) removed

app.py

@@ -170,58 +170,63 @@ def highlight_water_mask_on_frame(frame, binary_mask, color=(255, 0, 0), alpha=0
 
 # ── A* and KNN over binary water grid ─────────────────────────────────
 def astar(start, goal, occ):
-    h = lambda a, b: abs(a[0] - b[0]) + abs(a[1] - b[1])
-    N8 = [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
-    openq = [(0, start)]
-    g = {start: 0}
-    came = {}; visited = set() # Flag visited for path optimization
-    H, W = occ.shape
+    h   = lambda a,b: abs(a[0]-b[0])+abs(a[1]-b[1])
+    N8  = [(-1,-1),(-1,0),(-1,1),(0,-1),(0,1),(1,-1),(1,0),(1,1)]
+    openq=[(0,start)]; g={start:0}; came={}
     while openq:
-        _, cur = heapq.heappop(openq)
-        if cur == goal:
-            p = [cur]
-            while cur in came:
-                cur = came[cur]
-                p.append(cur)
+        _,cur=heapq.heappop(openq)
+        if cur==goal:
+            p=[cur];                   # reconstruct
+            while cur in came: cur=came[cur]; p.append(cur)
             return p[::-1]
-        if cur in visited:
-            continue
-        visited.add(cur)
-        for dx, dy in N8:
-            nx, ny = cur[0] + dx, cur[1] + dy
-            if not (0 <= nx < W and 0 <= ny < H): continue
-            if occ[ny, nx] == 0: continue
-            # prevent corner-cutting on diagonals
-            if abs(dx) == 1 and abs(dy) == 1:
-                if occ[cur[1] + dy, cur[0]] == 0 or occ[cur[1], cur[0] + dx] == 0:
+        for dx,dy in N8:
+            nx,ny=cur[0]+dx,cur[1]+dy
+            # out-of-bounds / blocked
+            if not (0<=nx<640 and 0<=ny<640) or occ[ny,nx]==0: continue
+            # if diagonal, ensure both orthogonals are free
+            if abs(dx)==1 and abs(dy)==1:
+                if occ[cur[1]+dy, cur[0]]==0 or occ[cur[1], cur[0]+dx]==0:
                     continue
-            next_node = (nx, ny)
-            ng = g[cur] + 1
-            if next_node not in g or ng < g[next_node]:
-                g[next_node] = ng
-                f = ng + h(next_node, goal)
-                heapq.heappush(openq, (f, next_node))
-                came[next_node] = cur
-    print(f"[A*] No path from {start} to {goal}")
+            ng=g[cur]+1
+            if (nx,ny) not in g or ng<g[(nx,ny)]:
+                g[(nx,ny)]=ng
+                f=ng+h((nx,ny),goal)
+                heapq.heappush(openq,(f,(nx,ny)))
+                came[(nx,ny)]=cur
     return []
-
 # KNN fit optimal path
 def knn_path(start, targets, occ):
     todo = targets[:]; path=[]
     cur  = tuple(start)
-    reachable = []; unreachable = [] # flag unreachable apart
-    for t in todo:
-        seg = astar(cur, tuple(t), occ)
-        if seg:
-            if path and path[-1] == seg[0]:
-                seg = seg[1:]
-            path.extend(seg)
-            reachable.append(t)
-            cur = tuple(t)
-        else:
-            unreachable.append(t)
-            print(f"⚠️ Unreachable garbage at {t}")
-    return path, reachable
+    reachable = []; unreachable = []
+    while todo:
+        # KNN follow a Greedy approach, which may not guarantee shortest path, hence only use A*
+        # nbrs = NearestNeighbors(n_neighbors=1).fit(todo)
+        # _,idx = nbrs.kneighbors([cur]); nxt=tuple(todo[idx[0][0]])
+        # seg  = astar(cur, nxt, occ)
+        # if seg:
+        #     if path and seg[0]==path[-1]: seg=seg[1:]
+        #     path.extend(seg)
+        # cur  = nxt; todo.remove(list(nxt))
+        best = None
+        best_len = float('inf')
+        best_seg = []
+        # Try A* to each target, find shortest actual path
+        for t in todo:
+            seg = astar(cur, tuple(t), occ)
+            if seg: # and len(seg) < best_len: # index error?
+                best = tuple(t)
+                best_len = len(seg)
+                best_seg = seg
+        if not best:
+            print("⚠️ Some garbage unreachable")
+            break  # stop if no reachable targets left
+        if path and path[-1] == best_seg[0]:
+            best_seg = best_seg[1:]  # avoid duplicate point
+        path.extend(best_seg)
+        cur = best
+        todo.remove(list(best))
+    return path
 
 
 # ── Robot sprite/class -──────────────────────────────────────────────────
@@ -483,11 +488,7 @@ def _pipeline(uid,img_path):
     robot = Robot(SPRITE)
     # Robot will be spawn on the closest movable mask to top-left
     robot.pos = [spawn_x, spawn_y]
-    # Unreachable targets flagged
-    path, unreachable_targets = knn_path(robot.pos, centres, movable_mask)
-    objs = [{"pos": p, "col": False, "unreachable": False} for p in centres if p not in unreachable_targets]
-    objs += [{"pos": p, "col": False, "unreachable": True} for p in unreachable_targets]
-
+    path  = knn_path(robot.pos, centres, movable_mask)
 
     # 4- Video synthesis
     out_tmp=f"{OUTPUT_DIR}/{uid}_tmp.mp4"