module Main where
import Types
import GTypes
import HigherBrain
import LowerBrain
import MessageParser
import System.IO
import Prelude hiding(Left, Right)
import Control.Concurrent.STM
import Data.IORef
import Control.Concurrent
import Control.Monad
import System.IO.Unsafe
import Network.Socket
import qualified QuadTree as QT

data StateV = StateV {
    initV :: TMVar Initialization,
    telemV :: TVar Telemetry,
    vsV :: TMVar VehicleState,
    targetV :: TVar Point
}

main = do
  hSetBuffering stdout NoBuffering

  initV <- atomically $ newEmptyTMVar
  telemV <- atomically $ newTVar emptyTelem
  vsV <- atomically $ newEmptyTMVar
  targetV <- atomically $ newTVar (0,0)
  let sv = StateV initV telemV vsV targetV

  h <- getNetworkHandle
  forkIO $ readLoop h sv
  forkIO $ higherBrainLoop initV telemV targetV
  sendMessageLoop h vsV targetV

-- The thread that listens for network input and updates appropriate state vars
readLoop :: Handle -> StateV -> IO ()
readLoop h sv = forever $ do
  msg <- getMessage (hGetChar h)
  case msg of
    (Init init) -> atomically $ putTMVar (initV sv) init
    (Telem telemData)  -> do
        atomically $  writeTVar (telemV sv) telemData
        atomically $  putTMVar (vsV sv) (Types.vehicleState telemData)
    Bounce   -> return ()
    msg   -> putStrLn (show msg)

getMessage :: IO Char -> IO Message
getMessage getc = getMessage' getc ""

getMessage' :: IO Char -> String -> IO Message
getMessage' getc str = do
  c <- getc
  if (c == ';') then return (parseMessage (reverse str))
                else getMessage' getc (c:str)

sendMessageLoop h vsV targetV = forever $ do
  vs <- atomically $ takeTMVar vsV
  targetPoint <- atomically $ readTVar targetV
  let cs = getControlState vs targetPoint
  hSendMessage h (vehicleCtl vs) cs

e c = hPutStr stderr [c,';']
-- |Send a message to the server
hSendMessage :: Handle -> VehicleControl -> VehicleControl -> IO ()
hSendMessage h vc0 vc1 = do
    when (cmd /= "") $ do
        putStrLn ("cmd :" ++ show vc1)                       
        hPutStr h (cmd++";")
        hFlush h
  where
    cmd = acc (vcAcc vc0) (vcAcc vc1) ++ dir (vcDir vc0) (vcDir vc1)

    acc Brake Accelerate = "a"
    acc Brake Roll = "a"
    acc Roll Brake = "b"
    acc Roll Accelerate = "a"
    acc Accelerate Roll = "b"
    acc Accelerate Brake = "b"
    acc _ _ = ""

    dir HardLeft HardLeft = ""
    dir HardLeft _ = "r"
    dir Left HardLeft = "l"
    dir Left Left = ""
    dir Left _ = "r"
    dir Straight HardLeft  = "l"
    dir Straight Left  = "l"
    dir Straight Straight  = ""
    dir Straight Right  = "r"
    dir Straight HardRight  = "r"
    dir Right HardRight = "r"
    dir Right Right = ""
    dir Right _ = "l"
    dir HardRight HardRight = ""
    dir HardRight _ = "l"

    lPuts str = do
      -- putStrLn ("sending: " ++ str)
      hPutStr h str
      hFlush h

getNetworkHandle :: IO Handle
getNetworkHandle = do
  sock <- socket AF_INET Stream defaultProtocol
  hostAddr <- inet_addr "127.0.0.1"
  connect sock (SockAddrInet 17676 hostAddr)
  result <- socketToHandle sock ReadWriteMode
  hSetBuffering result LineBuffering
  return result