{- |
Interpolation buffers.
-}

{-# LANGUAGE Arrows #-}
{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE RecordWildCards #-}
{-# LANGUAGE TypeFamilies #-}
module FRP.Rhine.ResamplingBuffer.Interpolation where

-- containers
import Data.Sequence

-- simple-affine-space
import Data.VectorSpace

-- rhine
import FRP.Rhine.ClSF
import FRP.Rhine.ResamplingBuffer
import FRP.Rhine.ResamplingBuffer.Util
import FRP.Rhine.ResamplingBuffer.KeepLast

-- | A simple linear interpolation based on the last calculated position and velocity.
linear
  :: ( Monad m, Clock m cl1, Clock m cl2
     , VectorSpace v s
     , s ~ Diff (Time cl1)
     , s ~ Diff (Time cl2)
     )
  => v -- ^ The initial velocity (derivative of the signal)
  -> v -- ^ The initial position
  -> ResamplingBuffer m cl1 cl2 v v
linear :: v -> v -> ResamplingBuffer m cl1 cl2 v v
linear v
initVelocity v
initPosition
  =    (v -> BehaviorF m (Time cl1) v v
forall (m :: Type -> Type) v s td.
(Monad m, VectorSpace v s, s ~ Diff td) =>
v -> BehaviorF m td v v
derivativeFrom v
initPosition ClSF m cl1 v v
-> ClSF m cl1 v v -> MSF (ReaderT (TimeInfo cl1) m) v (v, v)
forall (a :: Type -> Type -> Type) b c c'.
Arrow a =>
a b c -> a b c' -> a b (c, c')
&&& ClSF m cl1 v v
forall (m :: Type -> Type) cl a. Monad m => ClSF m cl a a
clId) MSF (ReaderT (TimeInfo cl1) m) v (v, v)
-> MSF (ReaderT (TimeInfo cl1) m) v s
-> MSF (ReaderT (TimeInfo cl1) m) v ((v, v), s)
forall (a :: Type -> Type -> Type) b c c'.
Arrow a =>
a b c -> a b c' -> a b (c, c')
&&& (TimeInfo cl1 -> s) -> MSF (ReaderT (TimeInfo cl1) m) v s
forall (m :: Type -> Type) cl b a.
Monad m =>
(TimeInfo cl -> b) -> ClSF m cl a b
timeInfoOf TimeInfo cl1 -> s
forall cl. TimeInfo cl -> Diff (Time cl)
sinceInit
  MSF (ReaderT (TimeInfo cl1) m) v ((v, v), s)
-> ResamplingBuffer m cl1 cl2 ((v, v), s) v
-> ResamplingBuffer m cl1 cl2 v v
forall (m :: Type -> Type) cl1 a b cl2 c.
Monad m =>
ClSF m cl1 a b
-> ResamplingBuffer m cl1 cl2 b c -> ResamplingBuffer m cl1 cl2 a c
^->> ((v, v), s) -> ResamplingBuffer m cl1 cl2 ((v, v), s) ((v, v), s)
forall (m :: Type -> Type) a cl1 cl2.
Monad m =>
a -> ResamplingBuffer m cl1 cl2 a a
keepLast ((v
initVelocity, v
initPosition), s
0)
  ResamplingBuffer m cl1 cl2 ((v, v), s) ((v, v), s)
-> ClSF m cl2 ((v, v), s) v
-> ResamplingBuffer m cl1 cl2 ((v, v), s) v
forall (m :: Type -> Type) cl1 cl2 a b c.
Monad m =>
ResamplingBuffer m cl1 cl2 a b
-> ClSF m cl2 b c -> ResamplingBuffer m cl1 cl2 a c
>>-^ proc ((v
velocity, v
lastPosition), s
sinceInit1) -> do
    s
sinceInit2 <- (TimeInfo cl2 -> s) -> ClSF m cl2 () s
forall (m :: Type -> Type) cl b a.
Monad m =>
(TimeInfo cl -> b) -> ClSF m cl a b
timeInfoOf TimeInfo cl2 -> s
forall cl. TimeInfo cl -> Diff (Time cl)
sinceInit -< ()
    let diff :: s
diff = s
sinceInit2 s -> s -> s
forall a. Num a => a -> a -> a
- s
sinceInit1
    MSF (ReaderT (TimeInfo cl2) m) v v
forall (a :: Type -> Type -> Type) b. Arrow a => a b b
returnA -< v
lastPosition v -> v -> v
forall v a. VectorSpace v a => v -> v -> v
^+^ s
diff s -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^ v
velocity

{- |
sinc-Interpolation, or Whittaker-Shannon-Interpolation.

The incoming signal is strictly bandlimited
by the frequency at which @cl1@ ticks.
Each incoming value is hulled in a sinc function,
these are added and sampled at @cl2@'s ticks.
In order not to produce a space leak,
the buffer only remembers the past values within a given window,
which should be chosen much larger than the average time between @cl1@'s ticks.
-}
sinc
  :: ( Monad m, Clock m cl1, Clock m cl2
     , VectorSpace v s
     , Ord (s)
     , Floating (s)
     , s ~ Diff (Time cl1)
     , s ~ Diff (Time cl2)
     )
  => s
  -- ^ The size of the interpolation window
  --   (for how long in the past to remember incoming values)
  -> ResamplingBuffer m cl1 cl2 v v
sinc :: s -> ResamplingBuffer m cl1 cl2 v v
sinc s
windowSize = Diff (Time cl1) -> ClSF m cl1 v (Seq (TimeInfo cl1, v))
forall (m :: Type -> Type) cl a.
(Monad m, Ord (Diff (Time cl)), TimeDomain (Time cl)) =>
Diff (Time cl) -> ClSF m cl a (Seq (TimeInfo cl, a))
historySince s
Diff (Time cl1)
windowSize ClSF m cl1 v (Seq (TimeInfo cl1, v))
-> ResamplingBuffer m cl1 cl2 (Seq (TimeInfo cl1, v)) v
-> ResamplingBuffer m cl1 cl2 v v
forall (m :: Type -> Type) cl1 a b cl2 c.
Monad m =>
ClSF m cl1 a b
-> ResamplingBuffer m cl1 cl2 b c -> ResamplingBuffer m cl1 cl2 a c
^->> Seq (TimeInfo cl1, v)
-> ResamplingBuffer
     m cl1 cl2 (Seq (TimeInfo cl1, v)) (Seq (TimeInfo cl1, v))
forall (m :: Type -> Type) a cl1 cl2.
Monad m =>
a -> ResamplingBuffer m cl1 cl2 a a
keepLast Seq (TimeInfo cl1, v)
forall a. Seq a
empty ResamplingBuffer
  m cl1 cl2 (Seq (TimeInfo cl1, v)) (Seq (TimeInfo cl1, v))
-> ClSF m cl2 (Seq (TimeInfo cl1, v)) v
-> ResamplingBuffer m cl1 cl2 (Seq (TimeInfo cl1, v)) v
forall (m :: Type -> Type) cl1 cl2 a b c.
Monad m =>
ResamplingBuffer m cl1 cl2 a b
-> ClSF m cl2 b c -> ResamplingBuffer m cl1 cl2 a c
>>-^ proc Seq (TimeInfo cl1, v)
as -> do
  s
sinceInit2 <- MSF (ReaderT (TimeInfo cl2) m) () s
forall (m :: Type -> Type) cl a.
Monad m =>
ClSF m cl a (Diff (Time cl))
sinceInitS -< ()
  MSF (ReaderT (TimeInfo cl2) m) v v
forall (a :: Type -> Type -> Type) b. Arrow a => a b b
returnA                  -< Seq v -> v
vectorSum (Seq v -> v) -> Seq v -> v
forall a b. (a -> b) -> a -> b
$ Diff (Time cl1) -> (TimeInfo cl1, v) -> v
forall cl v.
VectorSpace v (Diff (Time cl)) =>
Diff (Time cl) -> (TimeInfo cl, v) -> v
mkSinc s
Diff (Time cl1)
sinceInit2 ((TimeInfo cl1, v) -> v) -> Seq (TimeInfo cl1, v) -> Seq v
forall (f :: Type -> Type) a b. Functor f => (a -> b) -> f a -> f b
<$> Seq (TimeInfo cl1, v)
as
  where
    mkSinc :: Diff (Time cl) -> (TimeInfo cl, v) -> v
mkSinc Diff (Time cl)
sinceInit2 (TimeInfo {Diff (Time cl)
Time cl
Tag cl
tag :: forall cl. TimeInfo cl -> Tag cl
absolute :: forall cl. TimeInfo cl -> Time cl
sinceLast :: forall cl. TimeInfo cl -> Diff (Time cl)
tag :: Tag cl
absolute :: Time cl
sinceInit :: Diff (Time cl)
sinceLast :: Diff (Time cl)
sinceInit :: forall cl. TimeInfo cl -> Diff (Time cl)
..}, v
as)
      = let t :: Diff (Time cl)
t = Diff (Time cl)
forall a. Floating a => a
pi Diff (Time cl) -> Diff (Time cl) -> Diff (Time cl)
forall a. Num a => a -> a -> a
* (Diff (Time cl)
sinceInit2 Diff (Time cl) -> Diff (Time cl) -> Diff (Time cl)
forall a. Num a => a -> a -> a
- Diff (Time cl)
sinceInit) Diff (Time cl) -> Diff (Time cl) -> Diff (Time cl)
forall a. Fractional a => a -> a -> a
/ Diff (Time cl)
sinceLast
        in  (Diff (Time cl) -> Diff (Time cl)
forall a. Floating a => a -> a
sin Diff (Time cl)
t Diff (Time cl) -> Diff (Time cl) -> Diff (Time cl)
forall a. Fractional a => a -> a -> a
/ Diff (Time cl)
t) Diff (Time cl) -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^ v
as
    vectorSum :: Seq v -> v
vectorSum = (v -> v -> v) -> v -> Seq v -> v
forall (t :: Type -> Type) a b.
Foldable t =>
(a -> b -> b) -> b -> t a -> b
foldr v -> v -> v
forall v a. VectorSpace v a => v -> v -> v
(^+^) v
forall v a. VectorSpace v a => v
zeroVector

-- TODO Do we want to give initial values?
-- | Interpolates the signal with Hermite splines,
--   using 'threePointDerivative'.
--
--   Caution: In order to calculate the derivatives of the incoming signal,
--   it has to be delayed by two ticks of @cl1@.
--   In a non-realtime situation, a higher quality is achieved
--   if the ticks of @cl2@ are delayed by two ticks of @cl1@.
cubic
  :: ( Monad m
     , VectorSpace v s
     , Floating v, Eq v
     , s ~ Diff (Time cl1)
     , s ~ Diff (Time cl2)
     )
  => ResamplingBuffer m cl1 cl2 v v
cubic :: ResamplingBuffer m cl1 cl2 v v
cubic = ((v -> MSF (ReaderT (TimeInfo cl1) m) v v
forall (m :: Type -> Type) a. Monad m => a -> MSF m a a
iPre v
forall v a. VectorSpace v a => v
zeroVector MSF (ReaderT (TimeInfo cl1) m) v v
-> MSF (ReaderT (TimeInfo cl1) m) v v
-> MSF (ReaderT (TimeInfo cl1) m) v (v, v)
forall (a :: Type -> Type -> Type) b c c'.
Arrow a =>
a b c -> a b c' -> a b (c, c')
&&& MSF (ReaderT (TimeInfo cl1) m) v v
forall (m :: Type -> Type) v s td.
(Monad m, VectorSpace v s, s ~ Diff td) =>
BehaviorF m td v v
threePointDerivative) MSF (ReaderT (TimeInfo cl1) m) v (v, v)
-> MSF (ReaderT (TimeInfo cl1) m) v s
-> MSF (ReaderT (TimeInfo cl1) m) v ((v, v), s)
forall (a :: Type -> Type -> Type) b c c'.
Arrow a =>
a b c -> a b c' -> a b (c, c')
&&& (MSF (ReaderT (TimeInfo cl1) m) v s
forall (m :: Type -> Type) cl a.
Monad m =>
ClSF m cl a (Diff (Time cl))
sinceInitS MSF (ReaderT (TimeInfo cl1) m) v s
-> MSF (ReaderT (TimeInfo cl1) m) s s
-> MSF (ReaderT (TimeInfo cl1) m) v s
forall (cat :: Type -> Type -> Type) a b c.
Category cat =>
cat a b -> cat b c -> cat a c
>-> s -> MSF (ReaderT (TimeInfo cl1) m) s s
forall (m :: Type -> Type) a. Monad m => a -> MSF m a a
iPre s
0))
    MSF (ReaderT (TimeInfo cl1) m) v ((v, v), s)
-> MSF
     (ReaderT (TimeInfo cl1) m) ((v, v), s) (((v, v), s), ((v, v), s))
-> MSF (ReaderT (TimeInfo cl1) m) v (((v, v), s), ((v, v), s))
forall (cat :: Type -> Type -> Type) a b c.
Category cat =>
cat a b -> cat b c -> cat a c
>-> (ClSF m cl1 ((v, v), s) ((v, v), s)
forall (m :: Type -> Type) cl a. Monad m => ClSF m cl a a
clId ClSF m cl1 ((v, v), s) ((v, v), s)
-> ClSF m cl1 ((v, v), s) ((v, v), s)
-> MSF
     (ReaderT (TimeInfo cl1) m) ((v, v), s) (((v, v), s), ((v, v), s))
forall (a :: Type -> Type -> Type) b c c'.
Arrow a =>
a b c -> a b c' -> a b (c, c')
&&& ((v, v), s) -> ClSF m cl1 ((v, v), s) ((v, v), s)
forall (m :: Type -> Type) a. Monad m => a -> MSF m a a
iPre ((v, v)
forall v a. VectorSpace v a => v
zeroVector, s
0))
   MSF (ReaderT (TimeInfo cl1) m) v (((v, v), s), ((v, v), s))
-> ResamplingBuffer m cl1 cl2 (((v, v), s), ((v, v), s)) v
-> ResamplingBuffer m cl1 cl2 v v
forall (m :: Type -> Type) cl1 a b cl2 c.
Monad m =>
ClSF m cl1 a b
-> ResamplingBuffer m cl1 cl2 b c -> ResamplingBuffer m cl1 cl2 a c
^->> (((v, v), s), ((v, v), s))
-> ResamplingBuffer
     m cl1 cl2 (((v, v), s), ((v, v), s)) (((v, v), s), ((v, v), s))
forall (m :: Type -> Type) a cl1 cl2.
Monad m =>
a -> ResamplingBuffer m cl1 cl2 a a
keepLast (((v, v)
forall v a. VectorSpace v a => v
zeroVector, s
0), ((v, v)
forall v a. VectorSpace v a => v
zeroVector, s
0))
   ResamplingBuffer
  m cl1 cl2 (((v, v), s), ((v, v), s)) (((v, v), s), ((v, v), s))
-> ClSF m cl2 (((v, v), s), ((v, v), s)) v
-> ResamplingBuffer m cl1 cl2 (((v, v), s), ((v, v), s)) v
forall (m :: Type -> Type) cl1 cl2 a b c.
Monad m =>
ResamplingBuffer m cl1 cl2 a b
-> ClSF m cl2 b c -> ResamplingBuffer m cl1 cl2 a c
>>-^ proc (((v
dv, v
v), s
t1), ((v
dv', v
v'), s
t1')) -> do
     s
t2 <- MSF (ReaderT (TimeInfo cl2) m) () s
forall (m :: Type -> Type) cl a.
Monad m =>
ClSF m cl a (Diff (Time cl))
sinceInitS -< ()
     let
       t :: s
t        = (s
t1 s -> s -> s
forall a. Num a => a -> a -> a
- s
t1') s -> s -> s
forall a. Fractional a => a -> a -> a
/ (s
t2 s -> s -> s
forall a. Num a => a -> a -> a
- s
t1')
       tsquared :: s
tsquared = s
t s -> Integer -> s
forall a b. (Num a, Integral b) => a -> b -> a
^ Integer
2
       tcubed :: s
tcubed   = s
t s -> Integer -> s
forall a b. (Num a, Integral b) => a -> b -> a
^ Integer
3
       vInter :: v
vInter   = ( s
2 s -> s -> s
forall a. Num a => a -> a -> a
* s
tcubed s -> s -> s
forall a. Num a => a -> a -> a
- s
3 s -> s -> s
forall a. Num a => a -> a -> a
* s
tsquared     s -> s -> s
forall a. Num a => a -> a -> a
+ s
1) s -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^  v
v'
              v -> v -> v
forall v a. VectorSpace v a => v -> v -> v
^+^ (     s
tcubed s -> s -> s
forall a. Num a => a -> a -> a
- s
2 s -> s -> s
forall a. Num a => a -> a -> a
* s
tsquared s -> s -> s
forall a. Num a => a -> a -> a
+ s
t    ) s -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^ v
dv'
              v -> v -> v
forall v a. VectorSpace v a => v -> v -> v
^+^ (-s
2 s -> s -> s
forall a. Num a => a -> a -> a
* s
tcubed s -> s -> s
forall a. Num a => a -> a -> a
+ s
3 s -> s -> s
forall a. Num a => a -> a -> a
* s
tsquared        ) s -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^  v
v
              v -> v -> v
forall v a. VectorSpace v a => v -> v -> v
^+^ (     s
tcubed s -> s -> s
forall a. Num a => a -> a -> a
-     s
tsquared        ) s -> v -> v
forall v a. VectorSpace v a => a -> v -> v
*^ v
dv
     MSF (ReaderT (TimeInfo cl2) m) v v
forall (a :: Type -> Type -> Type) b. Arrow a => a b b
returnA -< v
vInter