From 66fe1abc6f96bbbc3a8571ffda9cb6613691615e Mon Sep 17 00:00:00 2001
From: justin <justin.g.kruger@gmail.com>
Date: Fri, 1 Jul 2022 23:17:41 +0100
Subject: [PATCH] Add PathEdge data to pathfinding graph

---
 src/Dodge/Data.hs           |  4 +++-
 src/Dodge/Data/PathGraph.hs |  1 +
 src/Dodge/Layout.hs         |  2 +-
 src/Dodge/Path.hs           | 45 +++++++++++++++++++++++++------------
 4 files changed, 36 insertions(+), 16 deletions(-)

diff --git a/src/Dodge/Data.hs b/src/Dodge/Data.hs
index f3c6c590b..5505dedd1 100644
--- a/src/Dodge/Data.hs
+++ b/src/Dodge/Data.hs
@@ -32,8 +32,10 @@ module Dodge.Data
     , module Dodge.Data.ItemAmount
     , module Dodge.RoomCluster.Data
     , module Dodge.Data.Room
+    , module Dodge.Data.PathGraph
     ) where
 import Dodge.Data.Room
+import Dodge.Data.PathGraph
 import Dodge.Data.Zoning
 import Dodge.Data.ForegroundShape
 import Dodge.Data.LoadAction
@@ -146,7 +148,7 @@ data World = World
     , _shapes               :: IM.IntMap ForegroundShape
     , _corpses              :: IM.IntMap Corpse
     , _clickMousePos        :: Point2
-    , _pathGraph            :: Gr Point2 Float
+    , _pathGraph            :: Gr Point2 PathEdge
     , _pathGraphP           :: S.Set (Point2,Point2)
     , _phZoning             :: Zoning [] (Int,Point2)
     , _hud                  :: HUD
diff --git a/src/Dodge/Data/PathGraph.hs b/src/Dodge/Data/PathGraph.hs
index d6f751844..e6984fc53 100644
--- a/src/Dodge/Data/PathGraph.hs
+++ b/src/Dodge/Data/PathGraph.hs
@@ -15,6 +15,7 @@ data PathGraph = PathGraph
 data PathEdge = PathEdge
     {_peStart :: Point2
     ,_peEnd   :: Point2
+    ,_peDist  :: Float
     ,_peObstacles :: Set.Set EdgeObstacle
     }
   deriving (Eq,Ord,Show)
diff --git a/src/Dodge/Layout.hs b/src/Dodge/Layout.hs
index 235f1d797..3c49ee669 100644
--- a/src/Dodge/Layout.hs
+++ b/src/Dodge/Layout.hs
@@ -45,7 +45,7 @@ generateLevelFromRoomList gr' w = initWallZoning
         , _pathGraphP   = pairPath
         }
   where
-    path = pairsToGraph dist pairPath
+    path = pairsToGraph pairPath
     pairPath = foldMap _rmPath rs
     rs = map doRoomShift $ IM.elems rs'
     rs'= mapM shuffleRoomPos gr' & evalState $ _randGen w
diff --git a/src/Dodge/Path.hs b/src/Dodge/Path.hs
index ffa337bf5..315d6d09d 100644
--- a/src/Dodge/Path.hs
+++ b/src/Dodge/Path.hs
@@ -17,6 +17,9 @@ import Data.List
 --import qualified Data.IntMap.Strict as IM
 import Data.Graph.Inductive hiding ((&))
 import qualified Data.Set as Set
+import qualified Data.Map.Strict as M
+import Data.Map.Strict (Map)
+import StreamingHelp
 import qualified Streaming.Prelude as S
 --import Data.Graph.Inductive.PatriciaTree
 --import Data.Graph.Inductive.Graph hiding ((&))
@@ -25,7 +28,7 @@ makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
     na <- walkableNodeNear a
     nb <- walkableNodeNear b
-    sp na nb (_pathGraph w)
+    sp na nb (second _peDist (_pathGraph w))
   where
     --nodesNear p = concat $ lookLookups (zoneNearPointIP p) (_pathPoints w)
     nodesNear p = runIdentity . S.toList_ $ nearPoint _phZoning p w
@@ -83,19 +86,33 @@ pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a
 --        [] -> return Nothing
 --        _  -> return $ Just $ ns !! i
 --
-pairsToGraph :: (Ord a, Ord b) => (a -> a -> b) -> Set.Set (a,a) -> Gr a b
-pairsToGraph f pairs = 
-    let nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
-        pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
-    in undir $ run_ Data.Graph.Inductive.empty $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
+pairsToGraph :: Set.Set (Point2,Point2) -> Gr Point2 PathEdge
+pairsToGraph pairs = addEdges nodemap gr $ S.each pairs
+  where
+    (nodemap,_,gr) = addNodes $ S.map fst (S.each pairs) <> S.map snd (S.each pairs)
+--    let nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
+--        pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
+--    in undir $ run_ Data.Graph.Inductive.empty $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
+
+addNodes :: StreamOf Point2 -> (Map Point2 Int,Int,Gr Point2 PathEdge)
+addNodes = runIdentity . S.fold_ f (mempty,0,Data.Graph.Inductive.empty) id
+  where
+    f (nodemap,i,gr) p = case nodemap M.!? p of
+        Just _  -> (nodemap,i,gr)
+        Nothing -> (nodemap & at p ?~i ,i+1, insNode (i,p) gr)
+addEdges :: Map Point2 Int -> Gr Point2 PathEdge -> StreamOf (Point2,Point2) -> Gr Point2 PathEdge
+addEdges nodemap gr = runIdentity . S.fold_ f gr id
+  where
+    f gr' (a,b) = insEdge (g a,g b,PathEdge a b (dist a b) mempty) gr'
+    g a = nodemap M.! a
 
 removePathsCrossing :: Point2 -> Point2 -> World -> World
 removePathsCrossing a b w = w
-    & pathGraph  .~ newGraph 
-    & pathGraphP .~ pg'
-    & phZoning   %~ \zn -> foldl' (flip $ updateZoning (:)) (zn & znObjects .~ mempty)
-        (labNodes newGraph)
-  where 
-    pg' = Set.filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
-   -- insertPoint pp@(_,p) = insertInZoneWith (wlZoneOfPoint p) (++) [pp]
-    newGraph = pairsToGraph dist pg'
+--    & pathGraph  .~ newGraph 
+--    & pathGraphP .~ pg'
+--    & phZoning   %~ \zn -> foldl' (flip $ updateZoning (:)) (zn & znObjects .~ mempty)
+--        (labNodes newGraph)
+--  where 
+--    pg' = Set.filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
+--   -- insertPoint pp@(_,p) = insertInZoneWith (wlZoneOfPoint p) (++) [pp]
+--    newGraph = pairsToGraph pg'