loop/src/Dodge/Path.hs

--{-# LANGUAGE TupleSections #-}
module Dodge.Path (
    nodesNear,
    pointTowardsImpulse,
    pointTowardsImpulse',
    makePathBetween,
    makePathBetweenPs,
    obstructPathsCrossing,
    getNodePos,
    walkableNodeNear,
    snapToGrid,
    pairsToIncGraph,
    getEdgesCrossing,
    pathEdgeObstructed,
) where

import Dodge.WorldEvent.ThingsHit
import qualified Data.Set as S
import qualified Algorithm.Search as AS
import Control.Lens
import Data.Foldable
import Data.List (sortOn)
import Data.Maybe
import Data.Set (Set)
import qualified Data.Set as Set
import qualified Data.Vector as V
import qualified Data.Vector.Unboxed as UV
import Dodge.Data.World
import Dodge.Zoning.Base
import Dodge.Zoning.Common
import Dodge.Zoning.Pathing
import Geometry
import qualified IntMapHelp as IM
import Linear

getNodePos :: Int -> World -> Point2
getNodePos i w = w ^?! cWorld . incNode . ix i

getEdgesCrossing :: Point2 -> Point2 -> World -> [(Int,Int)]
getEdgesCrossing s e w = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning s e w
  where
    inedgecrosses (i, j) = isJust $ intersectSegSeg s e (f i) (f j)
    f i = w ^?! cWorld . incNode . ix i

updateEdge ::
    (Set.Set EdgeObstacle -> Set.Set EdgeObstacle) ->
    (Int, Int) ->
    V.Vector (IM.IntMap SimpleEdge) ->
    V.Vector (IM.IntMap SimpleEdge)
{-# INLINE updateEdge #-}
updateEdge f (i, j) = ix i . ix j . seObstacles %~ f

makePathUsing :: (Set.Set EdgeObstacle -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
makePathUsing t s e w = do
    na <- walkableNodeNear w s
    nb <- walkableNodeNear w e
    let h i = distance (getn nb) (getn i)
    (na :) . snd <$> AS.aStarAssoc getes h (== nb) na
  where
    getes i = IM.toList 
        . IM.map (^. seDist)
        . IM.filter (^. seObstacles . to t) $ w ^?! cWorld . incGraph . ix i
    getn i = w ^?! cWorld . incNode . ix i

makePathUsing' :: (Point2 -> Point2 -> World -> Bool) 
    -> (Set.Set EdgeObstacle -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
makePathUsing' t1 t2 s e w = do
    na <- nodeNear t1 w s
    nb <- nodeNear t1 w e
    let h i = distance (getn nb) (getn i)
    (na :) . snd <$> AS.aStarAssoc getes h (== nb) na
  where
    getes i = IM.toList 
        . IM.map (^. seDist)
        . IM.filter (^. seObstacles . to t2) $ w ^?! cWorld . incGraph . ix i
    getn i = w ^?! cWorld . incNode . ix i



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

makePathBetween' :: (Point2 -> Point2 -> World -> Bool) -> (Set.Set EdgeObstacle -> Bool)
    -> Point2 -> Point2 -> World -> Maybe [Int]
makePathBetween' = makePathUsing'

pathEdgeObstructed :: Set.Set EdgeObstacle -> Bool
pathEdgeObstructed pe = any (`Set.member` pe) [WallObstacle WallNotAutoOpen, ChasmObstacle]

walkableNodeNear :: World -> Point2 -> Maybe Int
{-# INLINE walkableNodeNear #-}
walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear w p

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

nodesNear :: World -> Point2 -> [(Int, Point2)]
nodesNear w p = zonesExtract (w ^. incNodeZoning) . 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 = fmap (`getNodePos` w) <$> makePathBetween a b w

makePathBetweenPs' :: (Point2 -> Point2 -> World -> Bool) -> (Set.Set EdgeObstacle -> Bool)
  ->Point2 -> Point2 -> World -> Maybe [Point2]
makePathBetweenPs' t1 t2 a b w = fmap (`getNodePos` w) <$> makePathBetween' t1 t2 a b w

-- assumes that pathfinding is symmetric
pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs b a w

pointTowardsImpulse' ::  (Point2 -> Point2 -> World -> Bool) -> (Set.Set EdgeObstacle -> Bool)
    -> Point2 -> Point2 -> World -> Maybe Point2
pointTowardsImpulse' t1 t2 a b w = find (flip (t1 a) w) =<< makePathBetweenPs' t1 t2 b a w

pairsToIncGraph ::
    Set.Set (Point2, Point2) ->
    ( UV.Vector Point2
    , V.Vector (IM.IntMap SimpleEdge)
    , [(Int, Int)]
    )
pairsToIncGraph pairs =
    ( inodes
    , incgraph
    , Set.toList pairs & each . each %~ (\i -> pToNode ^?! ix i)
    )
  where
    incgraph :: V.Vector (IM.IntMap SimpleEdge)
    incgraph = V.generate (length im) (im IM.!)
-- I'm not fully happy with using the monoidal instance of IntMap here
-- especially seeing as there are two levels of IntMaps.
    im = IM.fromListWith (<>) . fmap toedge $ Set.toList pairs
    toedge (x, y) =
        ( pToNode ^?! ix x
        , IM.fromList [(pToNode ^?! ix y, SimpleEdge (distance x y) mempty)]
        )
    pToNode = IM.invertIntMapUnique . IM.fromDistinctAscList $ zip [0 ..] ps
    inodes = UV.generate (length ps) (ps !!)
    ps = Set.toList $ Set.map fst pairs <> Set.map snd pairs

-- consider updating all path edges at the end of world generation?
obstructPathsCrossing :: S.Set EdgeObstacle -> Point2 -> Point2 -> World -> World
obstructPathsCrossing obs s e w = w & cWorld . incGraph %~ updateincedges
  where
    updateincedge = flip $ updateEdge (S.union obs) 
    updateincedges gr = foldl' updateincedge gr inces
    inces = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning s e w
    inedgecrosses (i, j) = isJust $ intersectSegSeg s e (f i) (f j)
    f i = w ^?! cWorld . incNode . ix i

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