Add debugging for pathfinding

src/Dodge/Path.hs

@@ -4,6 +4,9 @@ module Dodge.Path
     , makePathBetweenPs
     , removePathsCrossing
     , pairsToGraph
+    , walkableNodeNear
+    , nodesNearL
+    , getNodePos
     ) where
 import Dodge.Data
 import Dodge.Base.Collide
@@ -18,21 +21,28 @@ import Data.List
 import Data.Graph.Inductive hiding ((&))
 import qualified Data.Set as Set
 import qualified Streaming.Prelude as S
+import StreamingHelp
 --import Data.Graph.Inductive.PatriciaTree
 --import Data.Graph.Inductive.Graph hiding ((&))
 
+getNodePos :: Int -> World -> Maybe Point2
+getNodePos i w = _pathGraph w `lab` i
+
 makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
-    na <- walkableNodeNear a
-    nb <- walkableNodeNear b
+    (na,_) <- walkableNodeNear a w
+    (nb,_) <- walkableNodeNear b w
     sp na nb (_pathGraph w)
-  where
-    --nodesNear p = concat $ lookLookups (zoneNearPointIP p) (_pathPoints w)
-    nodesNear p = runIdentity . S.toList_ $ nearPoint _phZoning p w
-    walkableNodeNear p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear p
 
---walkableNodeNear :: Point2 -> World -> Maybe Point2
---walkableNodeNear p w = insideCirc
+walkableNodeNear :: Point2 -> World -> Maybe (Int,Point2)
+walkableNodeNear p w = minStreamOn (dist p . snd) 
+    . S.filter (flip (isWalkable p) w . snd) $ nodesNear p w
+
+nodesNear :: Point2 -> World -> StreamOf (Int,Point2)
+nodesNear = aroundPoint _phZoning
+
+nodesNearL :: Point2 -> World -> [(Int,Point2)]
+nodesNearL p = runIdentity . S.toList_ . nodesNear p
 
 makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
 makePathBetweenPs a b w = mapMaybe (lab $ _pathGraph w) <$> makePathBetween a b w