--{-# LANGUAGE TupleSections #-}
module Dodge.Path (
    pointTowardsImpulse,
    makePathBetween,
    makePathBetweenPs,
    --    , removePathsCrossing
    obstructPathsCrossing,
    pairsToGraph,
    getNodePos,
    walkableNodeNear,
    bfsNodePoints,
    snapToGrid,
    pairsToIncGraph,
) where

import qualified Data.Vector as V
import qualified Data.Vector.Unboxed as UV
import Control.Lens
import Data.Bifunctor
import Data.Foldable
import Data.Graph.Inductive hiding ((&))
import Data.List (sortOn)
import Data.Map.Strict (Map)
import qualified Data.Map.Strict as M
import Data.Maybe
import Data.Set (Set)
import qualified Data.Set as Set
import Dodge.Base.Collide
import Dodge.Data.World
import Dodge.Zoning.Base
import Dodge.Zoning.Pathing
import Geometry
import Linear
import qualified IntMapHelp as IM

getNodePos :: Int -> World -> Maybe Point2
getNodePos i w = (w ^. cWorld . pathGraph) `lab` i

makePathUsing :: (PathEdge -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
makePathUsing t s e w = do
    na <- walkableNodeNear w s
    nb <- walkableNodeNear w e
    sp na nb . second _peDist . efilter (^. _3 . to t) $ w ^. cWorld . pathGraph

makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
makePathBetween = makePathUsing $ not . pathEdgeObstructed

pathEdgeObstructed :: PathEdge -> Bool
pathEdgeObstructed pe =
    any
        (`Set.member` _peObstacles pe)
        [DoorObstacle, BlockObstacle, ChasmObstacle]

walkableNodeNear :: World -> Point2 -> Maybe Int
{-# INLINE walkableNodeNear #-}
walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
  where
    nodesNear = zonesExtract (w ^. pnZoning) . snailAround $ zoneOfPoint pnZoneSize p

snailAround :: Int2 -> [Int2]
snailAround x = (x +) <$> smallSnailInt2

smallSnailInt2 :: [Int2]
smallSnailInt2 =
    sortOn
        (distance (V2 0 (0 :: Float)) . fmap fromIntegral)
        [V2 x y | x <- [-2 .. 2], y <- [-2 .. 2]]

makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
makePathBetweenPs a b w = mapMaybe (lab $ w ^. cWorld . pathGraph) <$> makePathBetween a b w

bfsNodePoints :: Int -> World -> [Point2]
bfsNodePoints n w = mapMaybe (lab g) $ bfs n g
  where
    g = w ^. cWorld . pathGraph

pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
pointTowardsImpulse a b w =
--    (find (flip (isWalkable a) w) . reverse)
--        =<< (makePathBetweenPs a b w <&> (<> [b]))
    (find (flip (isWalkable a) w) . reverse)
        =<< makePathBetweenPs a b w -- <&> (<> [b]))

------ 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
--

pairsToIncGraph :: Set.Set (Point2,Point2) 
    -> (UV.Vector Point2
        , V.Vector [(Int,SimpleEdge)]
        , [(Int,Int)]
    )
pairsToIncGraph pairs = (inodes,incgraph,undefined)
  where
    incgraph = V.generate (length im) (\i -> im ^?! ix i)
    im = IM.fromListWith (<>) . fmap toedge $ Set.toList pairs
    toedge (x,y) = (pstons ^?! ix x . _head
        , [(pstons ^?! ix y . _head, SimpleEdge (distance x y) mempty)])
    pstons = IM.invertIntMap . IM.fromList $ zip [0..] ps
    inodes = UV.generate (length ps) (ps !!)
    ps = Set.toList $ Set.map fst pairs <> Set.map snd pairs

pairsToGraph ::
    Set.Set (Point2, Point2) ->
    (Map (V2 Point2) PathEdgeNodes, Gr Point2 PathEdge)
pairsToGraph pairs = addEdges nodemap gr pairs
  where
    (nodemap, _, gr) = addNodes $ Set.toList $ Set.map fst pairs <> Set.map snd pairs

addNodes :: [Point2] -> (Map Point2 Int, Int, Gr Point2 PathEdge)
addNodes = foldl' f (mempty, 0, Data.Graph.Inductive.empty)
  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 ->
    Set.Set (Point2, Point2) ->
    (Map (V2 Point2) PathEdgeNodes, Gr Point2 PathEdge)
addEdges nodemap gr = foldl' f (mempty, gr)
  where
    f (edgemap, gr') (a, b) =
        ( M.insert (V2 a b) theedgedata edgemap
        , insEdge theedgetup gr'
        )
      where
        theedgetup = (g a, g b, PathEdge a b (dist a b) mempty)
        theedgedata = PathEdgeNodes (g a) (g b) (PathEdge a b (dist a b) mempty)
    g a = nodemap M.! a

obstructPathsCrossing ::
    EdgeObstacle ->
    Point2 ->
    Point2 ->
    World ->
    (World, Set PathEdgeNodes)
obstructPathsCrossing obstacletype sp' ep w =
    ( w & cWorld . pathGraph %~ updateedges
    , es
    )
  where
    es = Set.filter edgecrosses $ pesNearSeg sp' ep w
    edgecrosses (PathEdgeNodes _ _ pe) =
        isJust $ intersectSegSeg sp' ep (_peStart pe) (_peEnd pe)
    updateedges gr = foldl' updateedge gr es
    updateedge gr (PathEdgeNodes x y pe) =
        insEdge (x, y, pe & peObstacles . at obstacletype ?~ ()) $ delEdge (x, y) gr

snapToGrid :: Set (Point2, Point2) -> Set (Point2, Point2)
snapToGrid = Set.map (over each (fmap (fromIntegral . f)))
  where
    f :: Float -> Int
    f = round