{-# OPTIONS_GHC -fglasgow-exts #-}
 
module AStar where
import Control.Monad (guard, liftM2)
import Control.Monad.Instances
import Data.List (findIndex)
import qualified Data.Set as S
import qualified Data.Map as M
import qualified PriorityQueue as Q
import qualified QuadTree2  as QT
import GTypes

type Cell = (Int, Int)
data Map = Map {
    m_tree :: QT.QuadTree,
    m_xmin :: Double,
    m_ymin :: Double,
    m_xsize :: Double,
    m_ysize :: Double,
    m_width :: Int,
    m_height :: Int
}
  deriving (Show)

openCell :: Map -> Cell -> Bool
openCell m (u_,v_) = not occupied
  where
    occupied = QT.intersect r (m_tree m)
    r = (xmin + u*xsize, ymin + v*ysize, xmin + (u+1)*xsize, ymin + (v+1)*ysize)
    xmin = m_xmin m
    ymin = m_ymin m
    xsize = m_xsize m
    ysize = m_ysize m
    u = fromIntegral u_
    v = fromIntegral v_
    

manhattan :: Cell -> Cell -> Int
manhattan (x, y) (u, v) = (abs (x - u) `max` abs (y - v))
 
successor :: Map -> Cell -> [Cell]
successor m (x,y) = do u <- [x + 1, x, x - 1]
                       v <- [y + 1, y, y - 1]
                       guard (0 <= u && u < m_width m)
                       guard (0 <= v && v < m_height m)
                       guard (u /= x || y /= v)
                       guard (openCell m (u,v))
                       return (u, v)

astar :: (Ord b, Num b, Ord a) =>
         a -> (a -> [a]) -> (a -> Bool) -> (a -> a -> b) -> (a -> b) -> Maybe [a]
astar start succ end cost heur 
    = astar' (S.singleton start) (Q.singleton (heur start) [start])
 where
 astar' seen q
    | Q.null q  = Nothing
    | end n     = Just (next)
    | otherwise = astar' seen' q'
  where
  ((c,next), dq) = Q.deleteFindMin q
  n     = head next
  succs = filter (`S.notMember` seen) $ succ n
  costs = map ((+ c) . (subtract $ heur n) . liftM2 (+) (cost n) heur) succs
  q'    = dq `Q.union` Q.fromList (zip costs (map (:next) succs))
  seen' = seen `S.union` S.fromList succs


findPath :: QT.QuadTree -> Point -> Point -> Int -> Maybe [Point]
findPath qt start target nBlocks = do
    path <- astar startCell (successor m) (==endCell) cost heuristic
    return (map (pointFromCell m) path)
  where
    cost (u,v) (u',v') | u == u' || v == v' = 10
    cost (u,v) (u',v') | otherwise          = 14
    heuristic c = 10 * manhattan endCell c
    m = Map qt  x1 y1 ((x2-x1)/fromIntegral nBlocks) ((y2-y1)/fromIntegral nBlocks) nBlocks nBlocks
    (x1,y1,x2,y2) = QT.boundingRect qt
    startCell = cellFromPoint m start
    endCell = cellFromPoint m target


cellFromPoint :: Map -> Point -> Cell
cellFromPoint m (x,y) = (floor ((x-x1)/m_xsize m), floor ((y-y1)/m_ysize m))
  where
    (x1,y1,x2,y2) = QT.boundingRect (m_tree m)

pointFromCell :: Map -> Cell -> Point
pointFromCell m (u,v) =(x1 + m_xsize m * (0.5 + fromIntegral u), y1 + m_ysize m * (0.5 + fromIntegral v))
   where
    (x1,y1,x2,y2) = QT.boundingRect (m_tree m)