module Dodge.Path where
import Dodge.Data
import Dodge.Base
import Dodge.Graph
import Geometry

import Data.List
import Data.Maybe
import Data.Function
import Data.Graph.Inductive.PatriciaTree
import Data.Graph.Inductive.Query.SP
import Data.Graph.Inductive.Graph hiding ((&))
import qualified Data.HashSet as HS
import qualified Data.Heap as HP
import qualified Data.Map as M
import Control.Monad
import Control.Monad.State
import System.Random

worldGraph :: World -> Point2 -> HS.HashSet Point2
worldGraph w p = HS.unions $ fmap (\q -> HS.fromList $ pointsAlong w p (p +.+ q)) 
                               [(200,0),(-200,0),(0,200),(0,-200)]

pointsAlong :: World -> Point2 -> Point2 -> [Point2]
pointsAlong w p q = divideLineFixed 50 p p'
  where 
    p' = furthestPointWalkable p q $ wallsAlongLine p q w

divideLineFixed :: Float -> Point2 -> Point2 -> [Point2]
divideLineFixed x a b = fmap 
    ( \i -> a +.+ i * x *.* normalizeV (b -.- a) )
    ns
  where 
    numPoints = floor $ dist a b / x
    ns = map fromIntegral [1 .. numPoints]

-- ok, astar or something like it

type SearchedNodes = (HP.MinHeap (Float,(Float,[Point2])), [Point2])

stripRight :: Either a b -> b
stripRight (Right x) = x

stepPath :: (Point2 -> [Point2]) -> Point2 -> SearchedNodes -> Either [Point2] SearchedNodes
stepPath f p (nextNodes, seenNodes) = case HP.view nextNodes of
    Nothing -> Left []
    Just ((_,(cost,q:qs)), nextNodes') 
        | q == p -> Left (q:qs)
        | otherwise -> 
            let rs' = f q
                rs = rs' \\ seenNodes
                newNodes = map (\r -> (cost + dist q r + dist r p , (cost + dist q r , r:q:qs))) rs
            in  Right (foldr HP.insert nextNodes' newNodes , rs ++ seenNodes)

stepPath' :: (Point2 -> [Point2]) -> Point2 -> SearchedNodes -> [Point2]
stepPath' f p s = case stepPath f p s of 
    Left ps -> ps
    Right s' -> stepPath' f p s'

makePath' :: (Point2 -> [Point2]) -> Point2 -> Point2 -> [Point2]
makePath' f s e = stepPath' f e (HP.singleton (0,(0,[s])) , [])

makeNode :: Point2 -> SearchedNodes
makeNode e = (HP.singleton (0,(0,[e])) , [])


tp1,tp2,tp3 :: Point2
tp1 = (0,1)
tp2 = (0,20)
tp3 = (30,40)

f = incidenceToFunction $ pairsToIncidence 
    [(tp1,tp2)
    ,(tp2,tp3)
    ,(tp2,tp1)
    ,(tp1,tp3)
    ]
g = pairsToIncidence 
    [(tp1,tp2)
    ,(tp2,tp3)
    ,(tp2,tp1)
    ,(tp1,tp3)
    ]

pathBetween :: Point2 -> Point2 -> World -> Maybe [Point2]
pathBetween a b w = (makePath' $ \p -> _pathInc w M.! p) <$> a' <*> b'
  where
    nsa :: [Point2]
    nsa = map snd $ concat $ lookLookups (zoneAroundPoint a) (_pathPoints w)
    nsb = map snd $ concat $ lookLookups (zoneAroundPoint b) (_pathPoints w)
    a' = find (flip (isWalkable a) w)  nsa
    b' = find (flip (isWalkable b) w)  nsb

makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
makePathBetween a b w =  join $ sp <$> fmap fst a' <*> fmap fst b' <*> return g
  where
    g  = _pathGraph w
    nsa = concat $ lookLookups (zoneAroundPoint a) (_pathPoints w)
    nsb = concat $ lookLookups (zoneAroundPoint b) (_pathPoints w)
    a' = find (flip (isWalkable a) w . snd) nsa
    b' = find (flip (isWalkable b) w . snd) nsb

ezipWith :: Monoid a => (b -> c -> d) -> Either a b -> Either a c -> Either a d
ezipWith f (Right x) (Right y) = Right (f x y)
ezipWith f (Left  x) (Right _) = Left x
ezipWith f (Right _) (Left  y) = Left y
ezipWith f (Left  x) (Left  y) = Left (mappend x y)

makePathBetween' :: Point2 -> Point2 -> World -> Either String [Int]
makePathBetween' a b w = 
    let g  = _pathGraph w
        ns = labNodes g
        nsa = _pathPoints w `ixNZ` a
        nsb = _pathPoints w `ixNZ` b
        a' = case listToMaybe $ sortBy (compare `on` dist a . snd) $ filter (flip (isWalkable a) w . snd) ns of
            Just p -> Right $ fst p
            _      -> Left "FIRST POINT UNSEEN"
        b' = case listToMaybe $ sortBy (compare `on` dist b . snd) $ filter (flip (isWalkable b) w . snd) ns of
            Just p -> Right $ fst p
            _      -> Left $ "SECOND POINT UNSEEN" ++ show b
    in case ezipWith (\x y -> sp x y g) a' b' of
            Right (Just xs) -> Right xs
            Right Nothing   -> Left $ "NO PATH" ++ show a ++ show b ++ show a' ++ show b'
            Left m          -> Left m

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

makePathBetweenPs' :: Point2 -> Point2 -> World -> Either String [Point2]
makePathBetweenPs' a b w = mapMaybe (lab g) <$> makePathBetween' a b w
  where 
    g  = _pathGraph w

pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a b w
    
pointTowardsImpulse' :: Point2 -> Point2 -> World -> Either String Point2
pointTowardsImpulse' a b w = (maybeToEither "NOSEEPATH" . find (flip (isWalkable a) w)) =<< makePathBetweenPs' b a w

maybeToEither :: a -> Maybe b -> Either a b
maybeToEither _ (Just x) = Right x
maybeToEither y Nothing  = Left y

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

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

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)