module Rover (
              initstate, acceptMessageAndMove, SRC
             ) where

import Control.Monad.State.Lazy
import Data.List

import ServerMessages
import ClientMessages
import RoverStateMachine as RSM

import Debug.Trace
import Test.QuickCheck

type Point = (Float, Float)

-- | Rover position state
data RoverSt = RoverSt
    {
      initmsg :: MarsMsg, -- ^ Initialization message sent at beginning of trials
      telem :: MarsMsg, -- ^ Latest telemetry reading from rover
      prev_telem :: MarsMsg, -- ^ Previous telemetry reading
      accel :: Float, -- ^ Experiementally determined rover acceleration
      brake :: Float, -- ^ Experiementally determined rover braking
      rsm :: RoverStateMachine -- ^ Final desired rover controller status
    }

type SRC = State RoverSt [CmdMsg]

-- an initial state for the rover
initstate = RoverSt
            {
              initmsg = blankInitMsg,
              telem = blankTelemMsg,
              prev_telem = blankTelemMsg,
              accel = 0,
              brake = 0,
              rsm = RoverStateMachine {turn = Straight, move = Roll}
            }

acceptMessage :: MarsMsg -> SRC
acceptMessage m =
    case m of
      i@(InitMsg _ _ _ _ _ _ _ _) -> do modify (\x -> x {initmsg = i})
                                        return []
      t@(TelemMsg _ _ _ _ _ _ _) -> do modify (\x -> x {telem = t})
                                       return []
      otherwise -> do return []

acceptMessageAndMove :: MarsMsg -> SRC
acceptMessageAndMove m = do
  acceptMessage m
  simpleGoalSeek
  maintainSafeSpeed
  objectAvoidance 10 10
  saveOldTelem
  commandsFromState

commandsFromState :: SRC
commandsFromState = do st <- get
                       let t = telem st
                           sd = vehicle_ctl t -- previous state description
                           oldstate = parseRSM sd
                           newstate = rsm st
                       return (transitionCmds oldstate newstate)

-- When traveling, we want to maintain a speed that is fast, but not
-- faster than our vision will allow us to spot obstacles.  This
-- maintains a determined 'safe' speed.  Until we can determine
-- inertial constants, we'll just use some fixed safety factor.
maintainSafeSpeed :: SRC
maintainSafeSpeed = do st <- get
                       let i = initmsg st
                           t = telem st
                           travel_distance = (vehicle_speed t) * telemUpdateTime
                       case (compare (travel_distance * visionSafety) (max_sensor i)) of
                         GT -> setMoveState Roll
                         otherwise -> setMoveState Accel

visionSafety = 50
telemUpdateTime = 0.1 -- seconds between updates

-- simple strategy for seeking towards the base.
simpleGoalSeek :: SRC
simpleGoalSeek = do st <- get
                    let t = telem st
                        pt = prev_telem st
                        rdir = vehicle_dir t
                        bdir = snd $ evalBaseVector t
                        prev_bdir = snd $ evalBaseVector pt
                        degToBase = degreeDiff rdir bdir
                        prev_degToBase = degreeDiff (vehicle_dir pt) (prev_bdir)
                    case (degToBase > prev_degToBase) of
                      True -> setMoveState Brake
                      False -> setMoveState Accel
                    case (rdir > bdir) of
                      True -> setTurnState TurnRight
                      otherwise -> setTurnState TurnLeft


objectAvoidance :: Float -> Float -> SRC
objectAvoidance margin distance =
    do st <- get
       let t = telem st
           vehicle = ((vehicle_x t), (vehicle_y t))
           proj = (vehicle, (projectPointToBase vehicle distance))
           int = map (\o -> (o, (distanceFromLine proj ((object_x o), (object_y o)))))
                 (filter isBadObject $ objects t)
           dangerous = filter (\(o,d) -> case o of
                                           Martian _ _ _ _ -> True
                                           Object 'b' _ _ r -> trace ("d for object" ++ (show o) ++ " is " ++ (show d)) 
                                                               (d < r + ((0.4)))
                                           Object 'c' _ _ r -> False
                                           _ -> False
                              ) int
       case dangerous of
         [] -> return []
         otherwise -> trace "OA -> Left" $ setTurnState TurnLeft

setTurnState :: TurnState -> SRC
setTurnState ts = do st <- get
                     let oldst = rsm st
                         newst = oldst {turn = ts}
                     put (st {rsm = newst})
                     return []

setMoveState :: MoveState -> SRC
setMoveState ms = do st <- get
                     let oldst = rsm st
                         newst = oldst {move = ms}
                     put (st {rsm = newst})
                     return []

objectDir :: (Float, Float) -- ^ Fixed position
          -> (Float, Float) -- ^ Absolute object position
          -> Float -- ^ direction in degrees from the fixed position
objectDir (x1,y1) (x2,y2) =
    let rel_x2 = x2 - x1
        rel_y2 = y2 - y1
    in findAngle (sqrt ((rel_x2 ^ 2) + (rel_y2 ^ 2))) rel_x2 rel_y2

objectDistance :: (Float, Float) -- ^ Fixed position
               -> (Float, Float) -- ^ Absolute object position
               -> Float -- ^ distance to the object
objectDistance (x1,y1) (x2,y2) =
    let rel_x2 = x2 - x1
        rel_y2 = y2 - y1
    in sqrt ((rel_x2 ^ 2) + (rel_y2 ^ 2))

-- | Determine how close a point is to a line segment.
distanceFromLine :: (Point, Point) -> Point -> Float
distanceFromLine (a,b) c =
    let d = lineLength a b
        e = lineLength b c
        f = lineLength a c
        t = angleFromSides f d e
    in abs $ (e * (sin t)) -- law of sines

radToDegree = (*) (180/pi)
degreeToRad = (*) (pi/180)

-- | Determine angle of an oblique triangle given 3 sides.  Angle returned is opposite from the last side given.
angleFromSides :: Float -> Float -> Float -> Float
angleFromSides a b c = (acos ((a^2 + b^2 - c^2) / (2 * a * b))) -- law of cosines

lineLength :: Point -> Point -> Float
lineLength (a1,b1) (a2,b2) = sqrt ((a1-a2)^2 + (b1-b2)^2)

-- | Project a point a given distance towards base, but not past it.
projectPointToBase :: Point -- ^ starting point
                   -> Float -- ^ distance to project
                   -> Point -- ^ new point
projectPointToBase a@(x,y) d =
    let scale = d / (lineLength a (0,0))
        x' = x - (x * scale)
        y' = y - (y * scale)
    in (dontPassOrigin x x' , dontPassOrigin y y')
        where
          dontPassOrigin z z' =
              case (z > 0) of
                True -> max z' 0
                False -> min z' 0

-- determine difference in degrees (negative, left; positive, right)
-- from rover's perspective of an object
degreeDiff :: Float -- rover direction
           -> Float -- object direction
           -> Float
degreeDiff r o = (wrap360(r + 180 - o) - 180)

wrap360 :: Float -> Float
wrap360 v = case (v > 360) of
              True -> wrap360 (v - 360)
              False -> v

-- This hangs... why??
wrap360' :: Float -> Float
wrap360' v = head $ filter (<= 360) $ iterate (\x -> x - 360) v
-- QC says they're the same
testWrap360 = quickCheck (\s -> (wrap360' s) == (wrap360 s))

saveOldTelem = (modify (\s -> s {prev_telem = (telem s)}) >> return []) :: SRC

evalBaseVector :: MarsMsg -> (Float, Float)
evalBaseVector t = let distance = sqrt $ ((vehicle_x t) ^ 2) +  ((vehicle_y t) ^ 2)
                       a = vehicle_dir t
                       direction = findAngle distance ((-1) * (vehicle_x t)) ((-1) * (vehicle_y t))
                       in (distance, direction)

findAngle :: (Floating a, Ord a) => a -> a -> a -> a
findAngle dist vx vy = (180.0 / pi) *
                       case (vx > 0) of
                         True -> asin (vy / dist)
                         False -> case (vy > 0) of
                                    True ->  pi - (asin (vy / dist))
                                    False -> (asin (vx / dist)) - (pi / 2)