module RoverStateMachine (
                          TurnState(..), MoveState(..),
                          RoverStateMachine(..),
                          parseRSM, transitionCmds, tests
                         ) where

import ClientMessages
import Test.HUnit

-- | Possible turning states the rover can be in
data TurnState = TurnHardLeft | TurnLeft | Straight | TurnRight | TurnHardRight
               deriving (Read, Show, Eq, Ord, Enum)
-- | Possible moving states the rover can be in
data MoveState = Brake | Roll | Accel
               deriving (Read, Show, Eq, Ord, Enum)

-- | Combination of both of the rover's state machines, turning and movement
data RoverStateMachine = RoverStateMachine {
      move :: MoveState,
      turn :: TurnState
    } deriving (Read, Show, Eq)

-- | parse the "vehicle-ctl" field as sent by the server
parseRSM :: String -> RoverStateMachine
parseRSM (m:t:[]) = RoverStateMachine
                    (case m of
                       'b' -> Brake
                       '-' -> Roll
                       'a' -> Accel
                    )
                    (case t of
                       'L' -> TurnHardLeft
                       'l' -> TurnLeft
                       '-' -> Straight
                       'r' -> TurnRight
                       'R' -> TurnHardRight
                    )
-- out-of-spec, first telemetry has "-" instead of "--"
parseRSM _ = RoverStateMachine Roll Straight

-- | Determine the commands necessary to switch between two rover states
transitionCmds :: RoverStateMachine -- ^ Initial state
               -> RoverStateMachine -- ^ Desired state
               -> [CmdMsg] -- ^ Commands necessary to make the transition
transitionCmds (RoverStateMachine m1 t1) (RoverStateMachine m2 t2) =
    (transitionAccel m1 m2) ++ (transitionTurn t1 t2)

-- | Determine the commands necessary to switch between two moving states
transitionAccel :: MoveState -> MoveState -> [CmdMsg]
transitionAccel m1 m2 = case (compare m1 m2) of
                          LT -> AccelCmd : (transitionAccel (succ m1) m2)
                          GT -> BrakeCmd : (transitionAccel (pred m1) m2)
                          EQ -> []

-- | Determine the commands necessary to switch between two turning states
transitionTurn :: TurnState -> TurnState -> [CmdMsg]
transitionTurn t1 t2 = case (compare t1 t2) of
                         LT -> RightCmd : (transitionTurn (succ t1) t2)
                         GT -> LeftCmd : (transitionTurn (pred t1) t2)
                         EQ -> []

tests = TestList [TestLabel "" testBrakeToAccel,
                  TestLabel "" testAccelToBrake,
                  TestLabel "" testLeftToRight,
                  TestLabel "" testHardLeftToHardRight,
                  TestLabel "" testAccelLeftToBrakeRight]


rsmTestCase name cmds start end =
    TestCase $
             do assertEqual name cmds (transitionCmds start end)

testBrakeToAccel = rsmTestCase "Brake to Accel"
                   [AccelCmd, AccelCmd]
                   (RoverStateMachine Brake Straight)
                   (RoverStateMachine Accel Straight)

testAccelToBrake = rsmTestCase "Accel to Brake"
                   [BrakeCmd, BrakeCmd]
                   (RoverStateMachine Accel Straight)
                   (RoverStateMachine Brake Straight)

testLeftToRight = rsmTestCase "Left to Right"
                  [RightCmd, RightCmd]
                  (RoverStateMachine Roll TurnLeft)
                  (RoverStateMachine Roll TurnRight)

testHardLeftToHardRight = rsmTestCase "Hard Left to Hard Right"
                          [RightCmd, RightCmd, RightCmd, RightCmd]
                          (RoverStateMachine Roll TurnHardLeft)
                          (RoverStateMachine Roll TurnHardRight)

-- assuming here that we manage accel before turning, for no good reason
testAccelLeftToBrakeRight = rsmTestCase "Accel Left to Brake Right"
                            [BrakeCmd, BrakeCmd, RightCmd, RightCmd]
                            (RoverStateMachine Accel TurnLeft)
                            (RoverStateMachine Brake TurnRight)