--{-# LANGUAGE TupleSections #-}
module Dodge.Path 
    ( pointTowardsImpulse
    , makePathBetween
    , makePathBetweenPs
<<<<<<< HEAD
    , pairsToGraph
    , walkableNodeNear
    , nodesNearL
    , getNodePos
    , addObstacleCrossing'
=======
--    , removePathsCrossing
    , obstructPathsCrossing
    , pairsToGraph
    , getNodePos
    , walkableNodeNear
>>>>>>> efficientRuntime
    ) where
import Dodge.Data
import Dodge.Base.Collide
import Dodge.Zone
import Geometry.Data
import Geometry

import Control.Lens
import Data.Maybe
import Data.List
--import qualified IntMapHelp as IM
import Data.Graph.Inductive hiding ((&))
import Data.Set (Set)
import qualified Data.Set as Set
<<<<<<< HEAD
import Data.Map.Strict (Map)
import qualified Data.Map.Strict as M
=======
import qualified Data.Map.Strict as M
import Data.Map.Strict (Map)
import StreamingHelp
>>>>>>> efficientRuntime
import qualified Streaming.Prelude as S
import StreamingHelp
--import Data.Graph.Inductive.PatriciaTree
--import Data.Graph.Inductive.Graph hiding ((&))

getNodePos :: Int -> World -> Maybe Point2
<<<<<<< HEAD
getNodePos i w = _pgGraph (_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 w
    (nb,_) <- walkableNodeNear b w
    sp na nb (second pathDist $ efilter noobstacle $ _pgGraph $ _pathGraph w)
  where
    noobstacle (_,_,PathEdge _ _ s) = null s

pathDist :: PathEdge -> Float
pathDist pe = dist (_peStart pe) (_peEnd pe)

walkableNodeNear :: Point2 -> World -> Maybe (Int,Point2)
{-# INLINE walkableNodeNear #-}
walkableNodeNear p w = minStreamOn (dist p . snd) 
    . S.filter (flip (isWalkable p) w . snd) $ nodesNear p w

nodesNear :: Point2 -> World -> StreamOf (Int,Point2)
{-# INLINE nodesNear #-}
nodesNear = aroundPoint _pnZoning

nodesNearL :: Point2 -> World -> [(Int,Point2)]
nodesNearL p = runIdentity . S.toList_ . nodesNear p
=======
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 w a
    nb <- walkableNodeNear w b
    sp na nb (second _peDist (_pathGraph w))
  where
    --nodesNear p = concat $ lookLookups (zoneNearPointIP p) (_pathPoints w)
--    nodesNear p = runIdentity . S.toList_ $ nearPoint _pnZoning p w
--    walkableNodeNear p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear p

walkableNodeNear :: World -> Point2 -> Maybe Int
{-# INLINE walkableNodeNear #-}
walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
  where
    --nodesNear = runIdentity . S.toList_ $ nearPoint _pnZoning p w
    nodesNear = runIdentity . S.toList_ $ aroundPoint _pnZoning p w
>>>>>>> efficientRuntime

makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
makePathBetweenPs a b w = mapMaybe (lab $ _pgGraph $ _pathGraph w) <$> makePathBetween a b w

pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a b w

------ continues a walk from a list of points, without repetitions
------ supposes that the list is non-empty
--randomGraphWalk :: RandomGen g => [Int] -> Gr a b -> State g [Int]
--randomGraphWalk (n:ns) g = do 
--    next' <- randomGraphStepRestricted n ns g
--    case next' of 
--        Nothing -> return (n:ns)
--        Just n' -> randomGraphWalk (n':n:ns) g
--randomGraphWalk _ _ = error "Trying to walk in an empty list"
--
--randomPointXStepsFrom :: Int -> Point2 -> World -> Point2
--randomPointXStepsFrom i p w = 
--    let g  = _pathGraph w
--        ns = labNodes g
--        mp = listToMaybe $ sortBy (compare `on` dist p . snd) $ filter (flip (isWalkable p) w . snd) ns
--    in case mp of 
--        Nothing -> p
--        Just (n,_) -> fromJust 
--            $ lab g (last $ take i $ randomGraphWalk [n] g Data.Function.& evalState  $ _randGen w)
--
--randomPointsXStepsFrom :: Int -> Point2 -> World -> [Point2]
--randomPointsXStepsFrom i p w = 
--    let g  = _pathGraph w
--        ns = labNodes g
--        mp = listToMaybe $ sortBy (compare `on` dist p . snd) $ filter (flip (isWalkable p) w . snd) ns
--    in case mp of 
--        Nothing -> [p]
--        Just (n,_) -> mapMaybe (lab g) (take i $ randomGraphWalk [n] g Data.Function.& evalState $ _randGen w)
--
--randomGraphStep :: RandomGen g => Int -> Gr a b -> State g (Maybe Int)
--randomGraphStep n g = 
--    do let ns = neighbors g n
--       i  <- state $ randomR (0,length ns - 1)
--       case ns of [] -> return Nothing
--                  _  -> return $ Just $ ns !! i
--randomGraphStepRestricted :: RandomGen g => Int -> [Int] -> Gr a b -> State g (Maybe Int)
--randomGraphStepRestricted n notns g = do 
--    let ns = neighbors g n \\ notns
--    i  <- state $ randomR (0,length ns - 1)
--    case ns of 
--        [] -> return Nothing
--        _  -> return $ Just $ ns !! i
--
<<<<<<< HEAD

pairsToGraph :: Set.Set (Point2,Point2) -> PathGraph
pairsToGraph pairset = PathGraph gr' nodemap ncount edgemap
  where
    (gr,nodemap,ncount) = insertNodes pairset
    (gr',edgemap) = insertEdges toPathEdge gr nodemap pairset

toPathEdge :: Point2 -> Point2 -> PathEdge
toPathEdge sp' ep = PathEdge sp' ep mempty

insertEdges :: (Point2 -> Point2 -> b) 
    -> Gr Point2 b -> Map Point2 Int 
    -> Set (Point2,Point2) 
    -> (Gr Point2 b, Map (V2 Point2) (Int,Int,b))
insertEdges efunc gr nm = runIdentity . S.fold_ (insertEdge efunc nm) (gr,mempty) id . S.each

insertEdge :: Ord a => (a -> a -> c) -> Map a Int -> (Gr b c,Map (V2 a) (Int,Int,c))
    -> (a,a)
    -> (Gr b c,Map (V2 a) (Int,Int,c))
insertEdge efunc nm (gr',em) (a,b) = (insEdge (f a,f b,efunc a b) gr'
    , M.insert (V2 a b) (f a,f b,efunc a b) em)
  where 
    f x = nm M.! x

insertNodes :: Set (Point2,Point2) -> (Gr Point2 b, Map Point2 Int, Int)
insertNodes pairset = runIdentity $ S.fold_ insertnode (Data.Graph.Inductive.empty,mempty,0) id nodestream
  where
    nodestream :: StreamOf Point2
    nodestream = S.each . runIdentity $ S.fold_ getnode Set.empty id $ S.each pairset
    getnode strm (x,y) = Set.insert x $ Set.insert y strm
    insertnode :: (Gr Point2 b,Map Point2 Int,Int) -> Point2 -> (Gr Point2 b,Map Point2 Int,Int)
    insertnode (gr,nm,i) n = case M.lookup n nm of
        Nothing -> (insNode (j,n) gr, M.insert n j nm, j)
        Just _  -> (gr,nm,i)
      where
        j = i + 1

--pairsToGraph :: (Ord a, Ord b) => (a -> a -> b) -> Set.Set (a,a) -> Gr a b
--pairsToGraph f pairs = undir
--    $ run_ Data.Graph.Inductive.empty 
--    $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
--  where
--    nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
--    pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs

addObstacleCrossing :: World -> EdgeObstacle -> Point2 -> Point2 
    -> Gr Point2 PathEdge
    -> Gr Point2 PathEdge
addObstacleCrossing w eo a b gr = runIdentity $ S.fold_ updateedge gr id paths
  where
    updateedge gr' (xi,yi,PathEdge x y obs) = insEdge (xi,yi,PathEdge x y (Set.insert eo obs))
        $ delEdge (xi,yi) gr'
    paths = S.filter f $ nearSeg _peZoning a b w
    f (_,_,PathEdge x y _) = isJust $ intersectSegSeg x y a b

addObstacleCrossing' :: EdgeObstacle -> Point2 -> Point2 
    -> World
    -> World
addObstacleCrossing' eo a b w = w & pathGraph . pgGraph %~ addObstacleCrossing w eo a b
=======
pairsToGraph :: Set.Set (Point2,Point2) -> (Map (V2 Point2) (Int,Int,PathEdge),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) 
    -> (Map (V2 Point2) (Int,Int,PathEdge) , Gr Point2 PathEdge)
addEdges nodemap gr = runIdentity . S.fold_ f (mempty,gr) id
  where
    f (edgemap,gr') (a,b) = (M.insert (V2 a b) theedge edgemap
        , insEdge theedge gr'
        )
      where
        theedge = (g a,g b,PathEdge a b (dist a b) mempty)
    g a = nodemap M.! a

obstructPathsCrossing :: Point2 -> Point2 -> World -> ( World, [(Int,Int,PathEdge)])
obstructPathsCrossing sp ep w = 
    ( w & pathGraph %~ updateedges
    , runIdentity $ S.toList_ edges
    )
  where
    edges = S.filter edgecrosses $ nearSeg _peZoning sp ep w
    edgecrosses (_,_,pe) = isJust $ intersectSegSeg sp ep (_peStart pe) (_peEnd pe)
    updateedges gr = runIdentity $ S.fold_ updateedge gr id edges
    updateedge gr (x,y,pe) 
        = insEdge (x,y,pe & peObstacles . at BlockObstacle .~ Just ()) $ delEdge (x,y) gr

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 pg'
>>>>>>> efficientRuntime