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

import Geometry

import Control.Monad

import Data.List
import Data.Maybe
import Data.Function
import Data.Graph.Inductive.Graph
import qualified Data.HashSet as HS
import qualified Data.Heap as HP

import qualified Data.Map as M

import Data.Graph.Inductive.Graph
import Data.Graph.Inductive.PatriciaTree
import Data.Graph.Inductive.Query.SP

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)) 
                       $ fmap fromIntegral ns
    where numPoints = floor $ dist a b / x
          ns = [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' <$> return (\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' = listToMaybe $ sortBy (compare `on` dist a) $ ns
          --b' = listToMaybe $ sortBy (compare `on` dist b) $ ns
          a' = listToMaybe $ filter (flip (isWalkable a) w)  nsa
          b' = listToMaybe $ filter (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' = listToMaybe $ sortBy (compare `on` dist a . snd) $ filter (flip (isWalkable a) w . snd) ns
--          b' = listToMaybe $ sortBy (compare `on` dist b . snd) $ filter (flip (isWalkable b) w . snd) ns
          a' = listToMaybe $ filter (flip (isWalkable a) w . snd) nsa
          b' = listToMaybe $ filter (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) 
                --             a' = case listToMaybe 
                                                   $ 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) 
                --             b' = case listToMaybe 
                                                   $ 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 = pathBetween a b
makePathBetweenPs a b w =  fmap (mapMaybe (lab g)) $ makePathBetween b a w
    where g  = _pathGraph w

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


pointTowardsGoal :: Point2 -> Point2 -> World -> Maybe Point2
pointTowardsGoal a b w = join $ fmap (listToMaybe . filter (flip (isWalkable a) w))
--                              $ pathBetween a b w
                                $ makePathBetweenPs a b w
--
    
pointTowardsGoal' :: Point2 -> Point2 -> World -> Either String Point2
pointTowardsGoal' a b w = join $ fmap (maybeToEither "NOSEEPATH" . listToMaybe . filter (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

pairsToIncidence :: (Eq a,Ord a) => [(a,a)] -> [(a,[a])]
pairsToIncidence = map ((\(xs,ys) -> (head xs,ys)) . unzip)
                     . groupBy ( (==) `on` fst)
                     . sort

incidenceToFunction :: Eq a => [(a,[a])] -> a -> [a]
incidenceToFunction xs a = case lookup a xs of Just ys -> ys
                                               Nothing -> []