content: -- |Provides a rate limiting mechanism that can be used to control the
  -- rate at which 'IO' actions produce values.
  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
  
  -- |Produces an action that runs the supplied 'IO' action no faster
  -- than given rate in Hz.
  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
  