module Ros.Rate (rateLimiter) where
import Control.Concurrent (threadDelay)
import Control.Monad (when)
import Data.IORef (newIORef, readIORef, writeIORef)
import Data.Time.Clock (UTCTime, getCurrentTime, diffUTCTime)
import Ros.Util.PID
timeDiff :: UTCTime -> UTCTime -> Double
timeDiff = curry $ realToFrac . uncurry diffUTCTime
rateLimiter :: Double -> IO a -> IO (IO a)
rateLimiter hz action = do control' <- pidWithTimeIO (0.2) (0.02) (0.01)
let control = control' period
prevDelay <- newIORef period
prevTime <- getCurrentTime >>= newIORef
start <- getCurrentTime
return $ do t1 <- getCurrentTime
t0 <- readIORef prevTime
let tdiff = timeDiff t1 t0 * 1000000
t1' = realToFrac $
diffUTCTime t1 start
change <- control (t1', tdiff)
delay <- readIORef prevDelay
let delay' = delay + change
when (delay' > 0)
(threadDelay $ truncate delay')
x <- action
writeIORef prevDelay delay'
writeIORef prevTime t1
return x
where period = 1000000 / hz