{-# LANGUAGE NoImplicitPrelude #-}
{-# LANGUAGE MultiParamTypeClasses #-}
{-# LANGUAGE FlexibleInstances #-}
{- |
Copyright   :  (c) Henning Thielemann 2008-2012
License     :  GPL

Maintainer  :  synthesizer@henning-thielemann.de
Stability   :  provisional
Portability :  requires multi-parameter type classes

State variable filter.
One filter that generates lowpass, bandpass, highpass, bandlimit at once.
-}
module Synthesizer.Plain.Filter.Recursive.Universal (
   Parameter(..),
   Result(..),
   State,
   causal,
   modifier,
   modifierInit,
   parameter,
   parameterToSecondOrderLowpass,
   run,
   runInit,
   step,

   -- for testing
   parameterAlt,
   parameterOld,
   ) where

import Synthesizer.Plain.Filter.Recursive (Pole(..))
import qualified Synthesizer.Plain.Signal   as Sig
import qualified Synthesizer.Plain.Modifier as Modifier
import qualified Synthesizer.Causal.Process as Causal

import qualified Synthesizer.Plain.Filter.Recursive.SecondOrder as SecondOrder

import qualified Synthesizer.Interpolation.Class as Interpol

import qualified Control.Monad.Trans.State as MS
import qualified Control.Applicative.HT as App
import Control.Applicative (Applicative, pure, (<*>))

import qualified Data.Foldable as Fold
import qualified Data.Traversable as Trav

import Foreign.Storable (Storable(..))
import qualified Foreign.Storable.Record as Store

import qualified Algebra.Module                as Module
import qualified Algebra.Transcendental        as Trans
import qualified Algebra.Ring                  as Ring
import qualified Algebra.Additive              as Additive

import NumericPrelude.Numeric
import NumericPrelude.Base


data Parameter a =
        Parameter {forall a. Parameter a -> a
k1, forall a. Parameter a -> a
k2, forall a. Parameter a -> a
ampIn, forall a. Parameter a -> a
ampI1, forall a. Parameter a -> a
ampI2, forall a. Parameter a -> a
ampLimit :: !a}

instance Functor Parameter where
   {-# INLINE fmap #-}
   fmap :: forall a b. (a -> b) -> Parameter a -> Parameter b
fmap a -> b
f Parameter a
p = forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
k1 Parameter a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
k2 Parameter a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampIn Parameter a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampI1 Parameter a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampI2 Parameter a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampLimit Parameter a
p)

instance Applicative Parameter where
   {-# INLINE pure #-}
   pure :: forall a. a -> Parameter a
pure a
x = forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter a
x a
x a
x a
x a
x a
x
   {-# INLINE (<*>) #-}
   Parameter (a -> b)
f <*> :: forall a b. Parameter (a -> b) -> Parameter a -> Parameter b
<*> Parameter a
p = forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      (forall a. Parameter a -> a
k1 Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
k1 Parameter a
p) (forall a. Parameter a -> a
k2 Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
k2 Parameter a
p) (forall a. Parameter a -> a
ampIn Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampIn Parameter a
p) (forall a. Parameter a -> a
ampI1 Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampI1 Parameter a
p) (forall a. Parameter a -> a
ampI2 Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampI2 Parameter a
p) (forall a. Parameter a -> a
ampLimit Parameter (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Parameter a -> a
ampLimit Parameter a
p)

instance Fold.Foldable Parameter where
   {-# INLINE foldMap #-}
   foldMap :: forall m a. Monoid m => (a -> m) -> Parameter a -> m
foldMap = forall (t :: * -> *) m a.
(Traversable t, Monoid m) =>
(a -> m) -> t a -> m
Trav.foldMapDefault

instance Trav.Traversable Parameter where
   {-# INLINE sequenceA #-}
   sequenceA :: forall (f :: * -> *) a.
Applicative f =>
Parameter (f a) -> f (Parameter a)
sequenceA Parameter (f a)
p =
      forall (m :: * -> *) a b c d e f r.
Applicative m =>
(a -> b -> c -> d -> e -> f -> r)
-> m a -> m b -> m c -> m d -> m e -> m f -> m r
App.lift6 forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
         (forall a. Parameter a -> a
k1 Parameter (f a)
p) (forall a. Parameter a -> a
k2 Parameter (f a)
p) (forall a. Parameter a -> a
ampIn Parameter (f a)
p) (forall a. Parameter a -> a
ampI1 Parameter (f a)
p) (forall a. Parameter a -> a
ampI2 Parameter (f a)
p) (forall a. Parameter a -> a
ampLimit Parameter (f a)
p)

instance Interpol.C a v => Interpol.C a (Parameter v) where
   {-# INLINE scaleAndAccumulate #-}
   scaleAndAccumulate :: (a, Parameter v) -> (Parameter v, Parameter v -> Parameter v)
scaleAndAccumulate =
      forall a v (f :: * -> *).
(C a v, Applicative f) =>
(a, f v) -> (f v, f v -> f v)
Interpol.scaleAndAccumulateApplicative
{-
      Interpol.runMac $ App.lift6 Parameter
         (Interpol.element k1)
         (Interpol.element k2)
         (Interpol.element ampIn)
         (Interpol.element ampI1)
         (Interpol.element ampI2)
         (Interpol.element ampLimit)
-}

instance Storable a => Storable (Parameter a) where
   sizeOf :: Parameter a -> Int
sizeOf    = forall r. Dictionary r -> r -> Int
Store.sizeOf forall a. Storable a => Dictionary (Parameter a)
storeParameter
   alignment :: Parameter a -> Int
alignment = forall r. Dictionary r -> r -> Int
Store.alignment forall a. Storable a => Dictionary (Parameter a)
storeParameter
   peek :: Ptr (Parameter a) -> IO (Parameter a)
peek      = forall r. Dictionary r -> Ptr r -> IO r
Store.peek forall a. Storable a => Dictionary (Parameter a)
storeParameter
   poke :: Ptr (Parameter a) -> Parameter a -> IO ()
poke      = forall r. Dictionary r -> Ptr r -> r -> IO ()
Store.poke forall a. Storable a => Dictionary (Parameter a)
storeParameter

storeParameter ::
   Storable a => Store.Dictionary (Parameter a)
storeParameter :: forall a. Storable a => Dictionary (Parameter a)
storeParameter =
   forall r. Access r r -> Dictionary r
Store.run forall a b. (a -> b) -> a -> b
$
   forall (m :: * -> *) a b c d e f r.
Applicative m =>
(a -> b -> c -> d -> e -> f -> r)
-> m a -> m b -> m c -> m d -> m e -> m f -> m r
App.lift6 forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
k1)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
k2)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
ampIn)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
ampI1)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
ampI2)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Parameter a -> a
ampLimit)


data Result a =
        Result {forall a. Result a -> a
highpass, forall a. Result a -> a
bandpass, forall a. Result a -> a
lowpass, forall a. Result a -> a
bandlimit :: !a}

instance Functor Result where
   {-# INLINE fmap #-}
   fmap :: forall a b. (a -> b) -> Result a -> Result b
fmap a -> b
f Result a
p = forall a. a -> a -> a -> a -> Result a
Result
      (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
highpass Result a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
bandpass Result a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
lowpass Result a
p) (a -> b
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
bandlimit Result a
p)

instance Applicative Result where
   {-# INLINE pure #-}
   pure :: forall a. a -> Result a
pure a
x = forall a. a -> a -> a -> a -> Result a
Result a
x a
x a
x a
x
   {-# INLINE (<*>) #-}
   Result (a -> b)
f <*> :: forall a b. Result (a -> b) -> Result a -> Result b
<*> Result a
p = forall a. a -> a -> a -> a -> Result a
Result
      (forall a. Result a -> a
highpass Result (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
highpass Result a
p) (forall a. Result a -> a
bandpass Result (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
bandpass Result a
p) (forall a. Result a -> a
lowpass Result (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
lowpass Result a
p) (forall a. Result a -> a
bandlimit Result (a -> b)
f forall a b. (a -> b) -> a -> b
$ forall a. Result a -> a
bandlimit Result a
p)

instance Fold.Foldable Result where
   {-# INLINE foldMap #-}
   foldMap :: forall m a. Monoid m => (a -> m) -> Result a -> m
foldMap = forall (t :: * -> *) m a.
(Traversable t, Monoid m) =>
(a -> m) -> t a -> m
Trav.foldMapDefault

instance Trav.Traversable Result where
   {-# INLINE sequenceA #-}
   sequenceA :: forall (f :: * -> *) a.
Applicative f =>
Result (f a) -> f (Result a)
sequenceA Result (f a)
p =
      forall (m :: * -> *) a b c d r.
Applicative m =>
(a -> b -> c -> d -> r) -> m a -> m b -> m c -> m d -> m r
App.lift4 forall a. a -> a -> a -> a -> Result a
Result
         (forall a. Result a -> a
highpass Result (f a)
p) (forall a. Result a -> a
bandpass Result (f a)
p) (forall a. Result a -> a
lowpass Result (f a)
p) (forall a. Result a -> a
bandlimit Result (f a)
p)

instance Additive.C v => Additive.C (Result v) where
   {-# INLINE zero #-}
   {-# INLINE (+) #-}
   {-# INLINE (-) #-}
   {-# INLINE negate #-}
   zero :: Result v
zero   = forall (f :: * -> *) a. Applicative f => a -> f a
pure forall a. C a => a
zero
   + :: Result v -> Result v -> Result v
(+)    = forall (m :: * -> *) a b r.
Applicative m =>
(a -> b -> r) -> m a -> m b -> m r
App.lift2 forall a. C a => a -> a -> a
(+)
   (-)    = forall (m :: * -> *) a b r.
Applicative m =>
(a -> b -> r) -> m a -> m b -> m r
App.lift2 (-)
   negate :: Result v -> Result v
negate = forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap forall a. C a => a -> a
negate
{-
   zero = Result zero zero zero zero
   (+) (Result xhp xbp xlp xbl) (Result yhp ybp ylp ybl) =
      Result (xhp + yhp) (xbp + ybp) (xlp + ylp) (xbl + ybl)
   (-) (Result xhp xbp xlp xbl) (Result yhp ybp ylp ybl) =
      Result (xhp - yhp) (xbp - ybp) (xlp - ylp) (xbl - ybl)
   negate (Result xhp xbp xlp xbl) =
      Result (negate xhp) (negate xbp) (negate xlp) (negate xbl)
-}

instance Module.C a v => Module.C a (Result v) where
   {-# INLINE (*>) #-}
   a
s*> :: a -> Result v -> Result v
*>Result v
v = forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (a
sforall a v. C a v => a -> v -> v
*>) Result v
v
{-
   s *> (Result hp bp lp bl) =
      Result (s *> hp) (s *> bp) (s *> lp) (s *> bl)
-}

instance Storable a => Storable (Result a) where
   sizeOf :: Result a -> Int
sizeOf    = forall r. Dictionary r -> r -> Int
Store.sizeOf forall a. Storable a => Dictionary (Result a)
storeResult
   alignment :: Result a -> Int
alignment = forall r. Dictionary r -> r -> Int
Store.alignment forall a. Storable a => Dictionary (Result a)
storeResult
   peek :: Ptr (Result a) -> IO (Result a)
peek      = forall r. Dictionary r -> Ptr r -> IO r
Store.peek forall a. Storable a => Dictionary (Result a)
storeResult
   poke :: Ptr (Result a) -> Result a -> IO ()
poke      = forall r. Dictionary r -> Ptr r -> r -> IO ()
Store.poke forall a. Storable a => Dictionary (Result a)
storeResult

storeResult ::
   Storable a => Store.Dictionary (Result a)
storeResult :: forall a. Storable a => Dictionary (Result a)
storeResult =
   forall r. Access r r -> Dictionary r
Store.run forall a b. (a -> b) -> a -> b
$
   forall (m :: * -> *) a b c d r.
Applicative m =>
(a -> b -> c -> d -> r) -> m a -> m b -> m c -> m d -> m r
App.lift4 forall a. a -> a -> a -> a -> Result a
Result
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Result a -> a
highpass)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Result a -> a
bandpass)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Result a -> a
lowpass)
      (forall a r. Storable a => (r -> a) -> Access r a
Store.element forall a. Result a -> a
bandlimit)



{-|
The computation of the internal parameters is a bit complicated,
but it fulfills the following properties:

* At the resonance frequency the band pass has 180 degree phase shift.
  This is also approximately the frequency
  where the filter has maximum output.
  Even more important, this is the frequency where the band limit filter works.

* At the resonance frequency highpass, lowpass, and bandpass
  amplify by the factor @resonance@.

* The lowpass amplifies the frequency zero by factor 1.

* The highpass amplifies the highest representable (Nyquist) frequency by the factor 1.

* The bandlimit amplifies both frequency zero and Nyquist frequency
  by factor one and cancels the resonance frequency.
-}
{-# INLINE parameter #-}
parameter, parameterAlt, parameterOld :: Trans.C a => Pole a -> Parameter a
parameter :: forall a. C a => Pole a -> Parameter a
parameter (Pole a
resonance a
frequency) =
   let w :: a
w      = forall a. C a => a -> a
sin (forall a. C a => a
piforall a. C a => a -> a -> a
*a
frequency)
       w2 :: a
w2     = a
wforall a. C a => a -> Integer -> a
^Integer
2
       q2 :: a
q2     = a
resonanceforall a. C a => a -> Integer -> a
^Integer
2
       q21w2 :: a
q21w2  = a
4forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
*(a
1forall a. C a => a -> a -> a
-a
w2)
       sqrtQZ :: a
sqrtQZ = a
w forall a. C a => a -> a -> a
* forall a. C a => a -> a
sqrt (a
q21w2 forall a. C a => a -> a -> a
+ a
w2)
       pk1 :: a
pk1    = (a
w2forall a. C a => a -> a -> a
+a
sqrtQZ) forall a. C a => a -> a -> a
/ (a
q2forall a. C a => a -> a -> a
+a
w2forall a. C a => a -> a -> a
+a
sqrtQZ)
       d :: a
d      = (a
q21w2forall a. C a => a -> a -> a
*a
w2 forall a. C a => a -> a -> a
+ a
w2forall a. C a => a -> Integer -> a
^Integer
2 forall a. C a => a -> a -> a
- a
q2)
                  forall a. C a => a -> a -> a
/ (a
q21w2 forall a. C a => a -> a -> a
- a
2forall a. C a => a -> a -> a
*a
q2 forall a. C a => a -> a -> a
- a
w2 forall a. C a => a -> a -> a
+ (a
1forall a. C a => a -> a -> a
-a
4forall a. C a => a -> a -> a
*a
w2)forall a. C a => a -> a -> a
*a
sqrtQZ)
       volHP :: a
volHP  = (a
2forall a. C a => a -> a -> a
-a
pk1)forall a. C a => a -> a -> a
/a
4 forall a. C a => a -> a -> a
- a
d
       volRel :: a
volRel = forall a. C a => a -> a
sqrt ((a
2forall a. C a => a -> a -> a
-a
pk1 forall a. C a => a -> a -> a
+ a
4 forall a. C a => a -> a -> a
* a
d) forall a. C a => a -> a -> a
/ a
volHP)
   in  forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1forall a. C a => a -> a -> a
/a
volRel)  a
volHP
          a
volHP  a
volRel  a
volRel  (forall a. C a => a -> a
recip a
resonance)

parameterAlt :: forall a. C a => Pole a -> Parameter a
parameterAlt (Pole a
resonance a
frequency) =
   let w :: a
w      = forall a. C a => a -> a
sin (forall a. C a => a
piforall a. C a => a -> a -> a
*a
frequency)
       w2 :: a
w2     = a
wforall a. C a => a -> Integer -> a
^Integer
2
       q2 :: a
q2     = a
resonanceforall a. C a => a -> Integer -> a
^Integer
2
       sqrtQZ :: a
sqrtQZ = a
w forall a. C a => a -> a -> a
* forall a. C a => a -> a
sqrt (a
4forall a. C a => a -> a -> a
*a
q2 forall a. C a => a -> a -> a
+ a
w2 forall a. C a => a -> a -> a
- a
4forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
*a
w2)
       pk1 :: a
pk1    = (a
w2forall a. C a => a -> a -> a
+a
sqrtQZ) forall a. C a => a -> a -> a
/ (a
q2forall a. C a => a -> a -> a
+a
w2forall a. C a => a -> a -> a
+a
sqrtQZ)
       zr :: a
zr     = a
1 forall a. C a => a -> a -> a
- a
2 forall a. C a => a -> a -> a
* a
w2
       pk2 :: a
pk2    = a
2forall a. C a => a -> a -> a
-a
pk1 forall a. C a => a -> a -> a
+
                   a
4 forall a. C a => a -> a -> a
* (a
w2forall a. C a => a -> Integer -> a
^Integer
2forall a. C a => a -> a -> a
-a
q2forall a. C a => a -> a -> a
*a
zrforall a. C a => a -> Integer -> a
^Integer
2) forall a. C a => a -> a -> a
/ (a
2forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
*a
zrforall a. C a => a -> a -> a
-a
w2forall a. C a => a -> a -> a
+(a
1forall a. C a => a -> a -> a
-a
4forall a. C a => a -> a -> a
*a
w2)forall a. C a => a -> a -> a
*a
sqrtQZ)
       volHP :: a
volHP  = (a
4forall a. C a => a -> a -> a
-a
2forall a. C a => a -> a -> a
*a
pk1forall a. C a => a -> a -> a
-a
pk2) forall a. C a => a -> a -> a
/ a
4
       volLP :: a
volLP  = a
pk2
       volBP :: a
volBP  = forall a. C a => a -> a
sqrt (a
volHPforall a. C a => a -> a -> a
*a
volLP)
   in  forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1forall a. C a => a -> a -> a
*a
volHPforall a. C a => a -> a -> a
/a
volBP)  (a
pk2forall a. C a => a -> a -> a
*a
volHPforall a. C a => a -> a -> a
/a
volLP)
          a
volHP  (a
volBPforall a. C a => a -> a -> a
/a
volHP)  (a
volLPforall a. C a => a -> a -> a
/a
volBP)  (forall a. C a => a -> a
recip a
resonance)

{-
This computation is more affected by cancelations
for small frequencies, i.e. zr1 = cos eps - 1.
-}
parameterOld :: forall a. C a => Pole a -> Parameter a
parameterOld (Pole a
resonance a
frequency) =
   let zr :: a
zr     = forall a. C a => a -> a
cos (a
2forall a. C a => a -> a -> a
*forall a. C a => a
piforall a. C a => a -> a -> a
*a
frequency)
       zr1 :: a
zr1    = a
zrforall a. C a => a -> a -> a
-a
1
       q2 :: a
q2     = a
resonanceforall a. C a => a -> Integer -> a
^Integer
2
       sqrtQZ :: a
sqrtQZ = forall a. C a => a -> a
sqrt (a
zr1forall a. C a => a -> a -> a
*(-a
8forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
+a
zr1forall a. C a => a -> a -> a
-a
4forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
*a
zr1))
       pk1 :: a
pk1    = (-a
zr1forall a. C a => a -> a -> a
+a
sqrtQZ) forall a. C a => a -> a -> a
/ (a
2forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
-a
zr1forall a. C a => a -> a -> a
+a
sqrtQZ)
       q21zr :: a
q21zr  = a
4forall a. C a => a -> a -> a
*a
q2forall a. C a => a -> a -> a
*a
zr
       a :: a
a      = a
2 forall a. C a => a -> a -> a
* (a
zr1forall a. C a => a -> a -> a
*a
zr1forall a. C a => a -> a -> a
-a
q21zrforall a. C a => a -> a -> a
*a
zr) forall a. C a => a -> a -> a
/ (a
zr1forall a. C a => a -> a -> a
+a
q21zrforall a. C a => a -> a -> a
+(a
1forall a. C a => a -> a -> a
+a
2forall a. C a => a -> a -> a
*a
zr1)forall a. C a => a -> a -> a
*a
sqrtQZ)
       pk2 :: a
pk2    = a
aforall a. C a => a -> a -> a
+a
2forall a. C a => a -> a -> a
-a
pk1
       volHP :: a
volHP  = (a
4forall a. C a => a -> a -> a
-a
2forall a. C a => a -> a -> a
*a
pk1forall a. C a => a -> a -> a
-a
pk2) forall a. C a => a -> a -> a
/ a
4
       volLP :: a
volLP  = a
pk2
       volBP :: a
volBP  = forall a. C a => a -> a
sqrt (a
volHPforall a. C a => a -> a -> a
*a
volLP)
   in  forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1forall a. C a => a -> a -> a
*a
volHPforall a. C a => a -> a -> a
/a
volBP)  (a
pk2forall a. C a => a -> a -> a
*a
volHPforall a. C a => a -> a -> a
/a
volLP)
          a
volHP  (a
volBPforall a. C a => a -> a -> a
/a
volHP)  (a
volLPforall a. C a => a -> a -> a
/a
volBP)  (forall a. C a => a -> a
recip a
resonance)


{-
simplified iteration:

s'  = u + k1*i1 - k2*i2
i1' = i1 - s'
i2' = i2 - i1'
y0 = i2'

s' = u + k1*i1 - k2*i2
y0 = i2 - (i1 - s')

y0 = i2 - (i1 - (u + k1*i1 - k2*i2))

y0 = i2 - i1 + u + k1*i1 - k2*i2

y0 = u + (k1-1)*i1 + (1-k2)*i2

y0 = u + (k1-1)*(y1-y2) + (1-k2)*y1

y0 = u + (k1-k2)*y1 + (1-k1)*y2
-}
{- |
Convert parameters of universal filter to general second order filter parameters.
Filtering with these parameters does not yield exactly the same result
since the initial conditions are different.
-}
parameterToSecondOrderLowpass ::
   (Ring.C a) => Parameter a -> SecondOrder.Parameter a
parameterToSecondOrderLowpass :: forall a. C a => Parameter a -> Parameter a
parameterToSecondOrderLowpass Parameter a
p =
   SecondOrder.Parameter {
      c0 :: a
SecondOrder.c0 = a
1,
      c1 :: a
SecondOrder.c1 = a
0,
      c2 :: a
SecondOrder.c2 = a
0,
      d1 :: a
SecondOrder.d1 = forall a. Parameter a -> a
k1 Parameter a
p forall a. C a => a -> a -> a
- forall a. Parameter a -> a
k2 Parameter a
p,
      d2 :: a
SecondOrder.d2 = a
1 forall a. C a => a -> a -> a
- forall a. Parameter a -> a
k1 Parameter a
p
   }


type State v = (v,v)

{-| Universal filter: Computes high pass, band pass, low pass in one go -}
{-# INLINE step #-}
step :: (Ring.C a, Module.C a v) =>
   Parameter a -> v -> MS.State (State v) (Result v)
step :: forall a v.
(C a, C a v) =>
Parameter a -> v -> State (State v) (Result v)
step Parameter a
p v
u =
   forall (m :: * -> *) s a. Monad m => (s -> (a, s)) -> StateT s m a
MS.state forall a b. (a -> b) -> a -> b
$ \(v
i1,v
i2) ->
      let newsum :: v
newsum = forall a. Parameter a -> a
ampIn Parameter a
p forall a v. C a v => a -> v -> v
*> v
u forall a. C a => a -> a -> a
+ forall a. Parameter a -> a
k1 Parameter a
p forall a v. C a v => a -> v -> v
*> v
i1 forall a. C a => a -> a -> a
- forall a. Parameter a -> a
k2 Parameter a
p forall a v. C a v => a -> v -> v
*> v
i2
          newi1 :: v
newi1  = v
i1 forall a. C a => a -> a -> a
- forall a. Parameter a -> a
ampI1 Parameter a
p forall a v. C a v => a -> v -> v
*> v
newsum
          newi2 :: v
newi2  = v
i2 forall a. C a => a -> a -> a
- forall a. Parameter a -> a
ampI2 Parameter a
p forall a v. C a v => a -> v -> v
*> v
newi1
          out :: Result v
out    = forall a. a -> a -> a -> a -> Result a
Result v
newsum v
newi1 v
newi2 (v
u forall a. C a => a -> a -> a
+ forall a. Parameter a -> a
ampLimit Parameter a
p forall a v. C a v => a -> v -> v
*> v
newi1)
      in  (Result v
out, (v
newi1, v
newi2))

{-# INLINE modifierInit #-}
modifierInit :: (Ring.C a, Module.C a v) =>
   Modifier.Initialized (State v) (v,v) (Parameter a) v (Result v)
modifierInit :: forall a v.
(C a, C a v) =>
Initialized (State v) (State v) (Parameter a) v (Result v)
modifierInit =
   forall s init ctrl a b.
(init -> s)
-> (ctrl -> a -> State s b) -> Initialized s init ctrl a b
Modifier.Initialized forall a. a -> a
id forall a v.
(C a, C a v) =>
Parameter a -> v -> State (State v) (Result v)
step

{-# INLINE modifier #-}
modifier :: (Ring.C a, Module.C a v) =>
   Modifier.Simple (State v) (Parameter a) v (Result v)
modifier :: forall a v.
(C a, C a v) =>
Simple (State v) (Parameter a) v (Result v)
modifier = forall s init ctrl a b.
ModifierInit s init ctrl a b -> init -> Modifier s ctrl a b
Sig.modifierInitialize forall a v.
(C a, C a v) =>
Initialized (State v) (State v) (Parameter a) v (Result v)
modifierInit (forall a. C a => a
zero, forall a. C a => a
zero)

{-# INLINE causal #-}
causal ::
   (Ring.C a, Module.C a v) =>
   Causal.T (Parameter a, v) (Result v)
causal :: forall a v. (C a, C a v) => T (Parameter a, v) (Result v)
causal =
   forall s ctrl a b. Simple s ctrl a b -> T (ctrl, a) b
Causal.fromSimpleModifier forall a v.
(C a, C a v) =>
Simple (State v) (Parameter a) v (Result v)
modifier


{-# INLINE runInit #-}
runInit :: (Ring.C a, Module.C a v) =>
   (v,v) -> Sig.T (Parameter a) -> Sig.T v -> Sig.T (Result v)
runInit :: forall a v.
(C a, C a v) =>
(v, v) -> T (Parameter a) -> T v -> T (Result v)
runInit = forall s init ctrl a b.
ModifierInit s init ctrl a b -> init -> T ctrl -> T a -> T b
Sig.modifyModulatedInit forall a v.
(C a, C a v) =>
Initialized (State v) (State v) (Parameter a) v (Result v)
modifierInit

{-# INLINE run #-}
run :: (Ring.C a, Module.C a v) =>
   Sig.T (Parameter a) -> Sig.T v -> Sig.T (Result v)
run :: forall a v. (C a, C a v) => T (Parameter a) -> T v -> T (Result v)
run = forall a v.
(C a, C a v) =>
(v, v) -> T (Parameter a) -> T v -> T (Result v)
runInit (forall a. C a => a
zero, forall a. C a => a
zero)