module HigherBrain where
import Types hiding (Left,Right)
import Control.Concurrent.STM
import Control.Monad
import Data.Maybe
import GTypes
import qualified QuadTree as QT
import qualified QuadTree2 as QT2
import AStar

data HBState = HBState {
  hbCraters      :: QT.QuadTree Radius,
  hbBoulders     :: QT.QuadTree Radius,
  hbBlockedTerrain :: QT2.QuadTree
}

higherBrainLoop :: TMVar Initialization -> TVar Telemetry -> TVar Point -> IO ()
higherBrainLoop initV telemV pathV = do
    istate <- initialise
    loop istate 0
  where
    initialise = atomically $ do
      init <- readInit
      let r = (-(dx init) / 2,-(dy init) /2, dx init /2, dy init / 2)
      return HBState {
        hbCraters=QT.empty r,              
        hbBoulders=QT.empty r,              
        hbBlockedTerrain=QT2.empty r
      }

    loop st ts = do
        action <- atomically $ fmap Left readInit  `orElse` fmap Right (waitNextTelem ts)
        case action of
           Left init -> loop st 0
           Right telem -> process telem st ts

    readInit = takeTMVar initV

    waitNextTelem ts = do
        telem <- readTVar telemV
        when (timeStamp telem == ts) retry
        return telem
    
    process telem st ts = do 
        let (newCraters,newBoulders) = newObjects telem st
        let st' = updateState (newCraters,newBoulders) st

        let vs = vehicleState telem
        let loc = (vehicleX vs,vehicleY vs)
        let map = hbBlockedTerrain st'
        let path = findPath map targetPoint loc searchRes
        let astarSize = let (x1,_,x2,_) = QT2.boundingRect map in ((x2-x1) / fromIntegral searchRes)
        let nextPoint = case path of
                          Nothing           -> targetPoint    -- Not good, our A* search failed
                          (Just (p:[]))     -> p
                          (Just (p1:p2:ps)) -> if dist loc p1 < astarSize*2 then p1 else p2

        op <- atomically $ readTVar pathV
        when (nextPoint /= op) $ do
          putStrLn (show "loc = " ++ show loc ++ ",path = " ++ show path ++ ", nextpoint = " ++ show nextPoint ++
                    ", dist = " ++ show (dist loc nextPoint) ++ ", astarSize*2 = " ++ show (astarSize*2) )
                                 
        -- I want to make sure that whole calculation is done in this thread
        -- hence the ugly seqs below
        let (x,y) = nextPoint in x `seq` y `seq` (atomically $ writeTVar pathV nextPoint)

        loop st' (timeStamp telem)

type MObject = (Point,Radius)
newObjects :: Telemetry -> HBState -> ([MObject],[MObject])
newObjects telem st = (newCraters,newBoulders)
  where
    newCraters = filter (not.present (hbCraters st)) (getObjs Crater)
    newBoulders = filter (not.present (hbBoulders st)) (getObjs Boulder)
    getObjs otype = [((objectX o, objectY o), objectR o) | o <- objects telem, objectKind o == otype]
    present qt (p,_) = isJust (QT.search p qt)

updateState :: ([MObject],[MObject]) -> HBState -> HBState
updateState (newCraters,newBoulders) st = st {
    hbCraters = foldr (uncurry QT.insert) (hbCraters st) newCraters,
    hbBoulders = foldr (uncurry QT.insert) (hbBoulders st) newBoulders,
    hbBlockedTerrain = foldr (QT2.insertCircle blockedDepth) (hbBlockedTerrain st) (newCraters++newBoulders)
    }

targetPoint  = (0,0)  -- Where we are headed for
blockedDepth = 7      -- Our terrain map has resolution (2^blockedDepth)
searchRes    = 32     -- The resolution of our A* searc