content: {-# LANGUAGE TupleSections #-}
  -- |Functions for fusing 'Topic's based on TimeStamp fields of the
  -- underlying messages. This module shadows some of the functionality
  -- of the "Ros.TopicUtil" module. The difference is that the functions
  -- exported by this module use time stamps to correlate two
  -- 'Topic's. 
  -- 
  -- The correlation uses a bracketing pair of values from one 'Topic'
  -- to pick a correspondance for each value from the other
  -- 'Topic'. This bracketing approach induces some latency. The most
  -- common use case is calling the 'bothNew' function with a 'Topic'
  -- that produces very quickly (faster than the minimum required update
  -- rate), and another 'Topic' that imposes a rate limit.
  module Ros.Topic.Stamped (everyNew, interpolate, batch) where
  import Data.Time.Clock (getCurrentTime, diffUTCTime)
  import System.Timeout
  import qualified Ros.Topic as T
  import Ros.Topic (Topic(..), metamorphM, yieldM)
  import qualified Ros.Topic.Util as T
  import Ros.Internal.Msg.HeaderSupport
  import Ros.Internal.RosTime
  
  -- |Given two consecutive values, pick the one with the closest time
  -- stamp to another value.
  pickNearest :: (HasHeader a, HasHeader b) => a -> a -> b -> a
  pickNearest x1 x2 y
    | ty <= t1 = x1
    | t2 <= ty = x2
    | d1 < d2 = x1
    | otherwise = x2
    where t1 = getStamp x1
          t2 = getStamp x2
          ty = getStamp y
          d1 = diffROSTime ty t1
          d2 = diffROSTime t2 ty
  
  -- |@findBrackets t1 t2@ Pairs each element of @t2@ with the pair of
  -- consecutive elements from @t1@ that brackets it in time, and the 
  -- time interval in seconds covered by that bracket.
  findBrackets :: (HasHeader a, HasHeader b) =>
                  Topic IO a -> Topic IO b -> Topic IO ((a,a,Double), b)
  findBrackets t1 t2 = T.concats . metamorphM (go t2) $ T.consecutive t1
    where go t (x,y) = let start = getStamp x
                           stop = getStamp y
                           bracket = (x,y, diffSeconds stop start)
                       in do (items, rest) <- T.break ((< stop) . getStamp) $
                                              T.dropWhile ((< start) . getStamp) t
                             let items' = map (bracket,) items
                             yieldM items' (go rest)
  
  -- |Remove an element from a 'Topic' if the next element from that
  -- 'Topic' is composed of elements bearing the exact same sequence
  -- IDs in their headers (as obtained by 'getSequence').
  removeDups :: (Functor m, Monad m, HasHeader a, HasHeader b) =>
                Topic m (a,b) -> Topic m (a,b)
  removeDups = T.catMaybes . fmap check . T.consecutive
    where check ((x1,y1), x@(x2,y2))
            | getSequence x1 == getSequence x2 && 
              getSequence y1 == getSequence y2 = Nothing
            | otherwise = Just x
  
  -- |Returns a 'Topic' that produces a new pair for every value
  -- produced by either of the component 'Topic's. The value of the
  -- other element of the pair will be the element from the other
  -- 'Topic' with the nearest time stamp. The resulting 'Topic' will
  -- produce a new value at the rate of the faster component 'Topic'.
  everyNew :: (HasHeader a, HasHeader b) => 
              Topic IO a -> Topic IO b -> IO (Topic IO (a,b))
  everyNew t1 t2 = 
    do (t1a, t1b) <- T.tee t1
       (t2a, t2b) <- T.tee t2
       let bracketLeft = pickLeft `fmap` findBrackets t1a t2a
           bracketRight = pickRight `fmap` findBrackets t2b t1b
       return . removeDups $ bracketLeft `T.merge` bracketRight
    where pickLeft ((x1,x2,_), y) = (pickNearest x1 x2 y, y)
          pickRight ((y1,y2,_), x) = (x, pickNearest y1 y2 x)
  
  -- |The application @interpolate f t1 t2@ produces a new 'Topic' that
  -- pairs every element of @t2@ with an interpolation of two temporally
  -- bracketing values from @t1@. The interpolation is effected with the
  -- supplied function, @f@, that is given the two values to interpolate
  -- and the linear ratio to find between them. This ratio is determined
  -- by the time stamp of the intervening element of @t2@.
  interpolate :: (HasHeader a, HasHeader b) => 
                 (a -> a -> Double -> a) -> Topic IO a -> Topic IO b -> 
                 Topic IO (a,b)
  interpolate f t1 t2 = interp `fmap` findBrackets t1 t2
    where interp ((x1,x2,dt),y) = let tx1 = getStamp x1
                                      ty = getStamp y
                                  in (f x1 x2 (diffSeconds ty tx1 / dt), y)
  
  -- |Batch 'Topic' values that arrive within the given time window
  -- (expressed in seconds). When a value arrives, the window opens and
  -- all values received within that window are returned in a list, then
  -- the next value is awaited before opening the window again. Intended
  -- usage is to gather approximately simultaneous events into
  -- batches. Note that the times used to batch messages are arrival
  -- times rather than time stamps. This is what lets us close the
  -- window, rather than having to admit any message that ever arrives
  -- with a compatible time stamp.
  batch :: Double -> Topic IO a -> Topic IO [a]
  batch timeWindow t0 = 
    Topic $ do (x,t') <- runTopic t0
               start <- getCurrentTime
               let go acc t = do now <- getCurrentTime
                                 let dt = fromRational . toRational $
                                          diffUTCTime now start
                                     dMs = floor $ (timeWindow - dt) * 1000000
                                 if dMs == 0
                                   then return (reverse acc, k t)
                                   else do r <- timeout dMs $ runTopic t
                                           case r of
                                             Just (x',t'') -> go (x':acc) t''
                                             Nothing -> return (reverse acc, k t)
               go [x] t'
      where k = batch timeWindow
  