module Cdl.Cow where
import Control.Monad.State
import Data.Fixed
import Data.List

data Coord = Coord
    { longitude :: Float
    , latitude :: Float
    }

data CowState = CowState
    { id :: Int
    , samplingFrequency :: Float
    , movementSpeed :: Float
    , movementDirection :: Float
    , position :: Coord
    , time :: Float
    , dataPoints :: [Coord]
    }

type CowBehavior = State CowState

type Collar = (Int, Float, Coord, CowBehavior ())
type DataPoint = (Int, Coord)

emitPoint :: Coord -> CowBehavior ()
emitPoint c = modify (\s -> s { dataPoints = c : dataPoints s })

setPosition :: Coord -> CowBehavior ()
setPosition c = modify (\s -> s { position = c })

setDirection :: Float -> CowBehavior ()
setDirection d = modify (\s -> s { movementDirection = d })

increaseTime :: Float -> CowBehavior ()
increaseTime t = modify (\s -> s { time = time s + t })

repeatUntil :: (CowState -> Bool) -> CowBehavior a -> CowBehavior a
repeatUntil p m = ((,) <$> m <*> gets p) >>= (\(a,b) -> if b then return a else m)

repeatTimes :: Int -> CowBehavior a -> CowBehavior ()
repeatTimes n m = sequence_ $ replicate n m

addOffset :: (Float, Float) -> Coord -> Coord
addOffset (x, y) c = Coord (longitude c + longitudeDelta) (latitude c + latitudeDelta)
    where
        latitudeDelta = y / 111111
        longitudeDelta = x / (111111 * cos (latitude c))

samplingDeltas :: Float -> CowBehavior [Float]
samplingDeltas t = do
    currentTime <- gets time
    frequency <- gets samplingFrequency
    let firstDelta = (frequency * fromIntegral (ceiling (currentTime/frequency))) - currentTime
    return $ takeWhile (< t) [firstDelta + frequency*i | i <- [0..]]

move :: Float -> CowBehavior ()
move d = do
    speed <- gets movementSpeed
    dir <- gets movementDirection
    pos <- gets position
    let movementTime = d/speed
    deltas <- samplingDeltas movementTime
    let coords = map (\d -> addOffset (speed * d * cos dir, speed * d * sin dir) pos) deltas
    let finalPos = addOffset (d * cos dir, d * sin dir) pos
    mapM emitPoint coords
    increaseTime movementTime
    setPosition finalPos

turn :: Float -> CowBehavior ()
turn a = do
    dir <- gets movementDirection
    let newDir = dir + (a/180 * pi)
    setDirection (newDir `mod'` (2*pi))

wait :: Float -> CowBehavior ()
wait t = do
    deltas <- samplingDeltas t
    pos <- gets position
    mapM (const $ emitPoint pos) deltas
    increaseTime t

runCow :: Float -> Collar -> [DataPoint]
runCow f (id, speed, pos, behavior) = zip (cycle [id]) coords
    where
        coords = reverse (position finalState : dataPoints finalState)
        finalState = snd $ runState behavior initialState
        initialState = CowState
            { Cdl.Cow.id = id
            , samplingFrequency = f
            , movementSpeed = speed
            , movementDirection = 0
            , position = pos
            , time = 0
            , dataPoints = []
            }

runCows :: Float -> [Collar] -> [[DataPoint]]
runCows f cs = transpose $ map (runCow f) $ cs