100 lines
3.0 KiB
Haskell
100 lines
3.0 KiB
Haskell
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
|