{-# 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 = b -> b -> b -> b -> b -> b -> Parameter b
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
k1 Parameter a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
k2 Parameter a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampIn Parameter a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampI1 Parameter a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampI2 Parameter a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampLimit Parameter a
p)

instance Applicative Parameter where
   {-# INLINE pure #-}
   pure :: forall a. a -> Parameter a
pure a
x = a -> a -> a -> a -> a -> a -> Parameter a
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 = b -> b -> b -> b -> b -> b -> Parameter b
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
k1 Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
k1 Parameter a
p) (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
k2 Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
k2 Parameter a
p) (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
ampIn Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampIn Parameter a
p) (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
ampI1 Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampI1 Parameter a
p) (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
ampI2 Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
forall a. Parameter a -> a
ampI2 Parameter a
p) (Parameter (a -> b) -> a -> b
forall a. Parameter a -> a
ampLimit Parameter (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Parameter a -> a
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 = (a -> m) -> Parameter a -> m
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 =
      (a -> a -> a -> a -> a -> a -> Parameter a)
-> f a -> f a -> f a -> f a -> f a -> f a -> f (Parameter a)
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 a -> a -> a -> a -> a -> a -> Parameter a
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
         (Parameter (f a) -> f a
forall a. Parameter a -> a
k1 Parameter (f a)
p) (Parameter (f a) -> f a
forall a. Parameter a -> a
k2 Parameter (f a)
p) (Parameter (f a) -> f a
forall a. Parameter a -> a
ampIn Parameter (f a)
p) (Parameter (f a) -> f a
forall a. Parameter a -> a
ampI1 Parameter (f a)
p) (Parameter (f a) -> f a
forall a. Parameter a -> a
ampI2 Parameter (f a)
p) (Parameter (f a) -> f a
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 =
      (a, Parameter v) -> (Parameter v, Parameter v -> Parameter v)
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    = Dictionary (Parameter a) -> Parameter a -> Int
forall r. Dictionary r -> r -> Int
Store.sizeOf Dictionary (Parameter a)
forall a. Storable a => Dictionary (Parameter a)
storeParameter
   alignment :: Parameter a -> Int
alignment = Dictionary (Parameter a) -> Parameter a -> Int
forall r. Dictionary r -> r -> Int
Store.alignment Dictionary (Parameter a)
forall a. Storable a => Dictionary (Parameter a)
storeParameter
   peek :: Ptr (Parameter a) -> IO (Parameter a)
peek      = Dictionary (Parameter a) -> Ptr (Parameter a) -> IO (Parameter a)
forall r. Dictionary r -> Ptr r -> IO r
Store.peek Dictionary (Parameter a)
forall a. Storable a => Dictionary (Parameter a)
storeParameter
   poke :: Ptr (Parameter a) -> Parameter a -> IO ()
poke      = Dictionary (Parameter a)
-> Ptr (Parameter a) -> Parameter a -> IO ()
forall r. Dictionary r -> Ptr r -> r -> IO ()
Store.poke Dictionary (Parameter a)
forall a. Storable a => Dictionary (Parameter a)
storeParameter

storeParameter ::
   Storable a => Store.Dictionary (Parameter a)
storeParameter :: forall a. Storable a => Dictionary (Parameter a)
storeParameter =
   Access (Parameter a) (Parameter a) -> Dictionary (Parameter a)
forall r. Access r r -> Dictionary r
Store.run (Access (Parameter a) (Parameter a) -> Dictionary (Parameter a))
-> Access (Parameter a) (Parameter a) -> Dictionary (Parameter a)
forall a b. (a -> b) -> a -> b
$
   (a -> a -> a -> a -> a -> a -> Parameter a)
-> Access (Parameter a) a
-> Access (Parameter a) a
-> Access (Parameter a) a
-> Access (Parameter a) a
-> Access (Parameter a) a
-> Access (Parameter a) a
-> Access (Parameter a) (Parameter a)
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 a -> a -> a -> a -> a -> a -> Parameter a
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
forall a. Parameter a -> a
k1)
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
forall a. Parameter a -> a
k2)
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
forall a. Parameter a -> a
ampIn)
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
forall a. Parameter a -> a
ampI1)
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
forall a. Parameter a -> a
ampI2)
      ((Parameter a -> a) -> Access (Parameter a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Parameter a -> a
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 = b -> b -> b -> b -> Result b
forall a. a -> a -> a -> a -> Result a
Result
      (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
highpass Result a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
bandpass Result a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
lowpass Result a
p) (a -> b
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
bandlimit Result a
p)

instance Applicative Result where
   {-# INLINE pure #-}
   pure :: forall a. a -> Result a
pure a
x = a -> a -> a -> a -> Result a
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 = b -> b -> b -> b -> Result b
forall a. a -> a -> a -> a -> Result a
Result
      (Result (a -> b) -> a -> b
forall a. Result a -> a
highpass Result (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
highpass Result a
p) (Result (a -> b) -> a -> b
forall a. Result a -> a
bandpass Result (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
bandpass Result a
p) (Result (a -> b) -> a -> b
forall a. Result a -> a
lowpass Result (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
forall a. Result a -> a
lowpass Result a
p) (Result (a -> b) -> a -> b
forall a. Result a -> a
bandlimit Result (a -> b)
f (a -> b) -> a -> b
forall a b. (a -> b) -> a -> b
$ Result a -> a
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 = (a -> m) -> Result a -> m
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 =
      (a -> a -> a -> a -> Result a)
-> f a -> f a -> f a -> f a -> f (Result a)
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 a -> a -> a -> a -> Result a
forall a. a -> a -> a -> a -> Result a
Result
         (Result (f a) -> f a
forall a. Result a -> a
highpass Result (f a)
p) (Result (f a) -> f a
forall a. Result a -> a
bandpass Result (f a)
p) (Result (f a) -> f a
forall a. Result a -> a
lowpass Result (f a)
p) (Result (f a) -> f a
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   = v -> Result v
forall a. a -> Result a
forall (f :: * -> *) a. Applicative f => a -> f a
pure v
forall a. C a => a
zero
   + :: Result v -> Result v -> Result v
(+)    = (v -> v -> v) -> Result v -> Result v -> Result v
forall (m :: * -> *) a b r.
Applicative m =>
(a -> b -> r) -> m a -> m b -> m r
App.lift2 v -> v -> v
forall a. C a => a -> a -> a
(+)
   (-)    = (v -> v -> v) -> Result v -> Result v -> Result v
forall (m :: * -> *) a b r.
Applicative m =>
(a -> b -> r) -> m a -> m b -> m r
App.lift2 (-)
   negate :: Result v -> Result v
negate = (v -> v) -> Result v -> Result v
forall a b. (a -> b) -> Result a -> Result b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap v -> v
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 = (v -> v) -> Result v -> Result v
forall a b. (a -> b) -> Result a -> Result b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (a
sa -> v -> v
forall 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    = Dictionary (Result a) -> Result a -> Int
forall r. Dictionary r -> r -> Int
Store.sizeOf Dictionary (Result a)
forall a. Storable a => Dictionary (Result a)
storeResult
   alignment :: Result a -> Int
alignment = Dictionary (Result a) -> Result a -> Int
forall r. Dictionary r -> r -> Int
Store.alignment Dictionary (Result a)
forall a. Storable a => Dictionary (Result a)
storeResult
   peek :: Ptr (Result a) -> IO (Result a)
peek      = Dictionary (Result a) -> Ptr (Result a) -> IO (Result a)
forall r. Dictionary r -> Ptr r -> IO r
Store.peek Dictionary (Result a)
forall a. Storable a => Dictionary (Result a)
storeResult
   poke :: Ptr (Result a) -> Result a -> IO ()
poke      = Dictionary (Result a) -> Ptr (Result a) -> Result a -> IO ()
forall r. Dictionary r -> Ptr r -> r -> IO ()
Store.poke Dictionary (Result a)
forall a. Storable a => Dictionary (Result a)
storeResult

storeResult ::
   Storable a => Store.Dictionary (Result a)
storeResult :: forall a. Storable a => Dictionary (Result a)
storeResult =
   Access (Result a) (Result a) -> Dictionary (Result a)
forall r. Access r r -> Dictionary r
Store.run (Access (Result a) (Result a) -> Dictionary (Result a))
-> Access (Result a) (Result a) -> Dictionary (Result a)
forall a b. (a -> b) -> a -> b
$
   (a -> a -> a -> a -> Result a)
-> Access (Result a) a
-> Access (Result a) a
-> Access (Result a) a
-> Access (Result a) a
-> Access (Result a) (Result a)
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 a -> a -> a -> a -> Result a
forall a. a -> a -> a -> a -> Result a
Result
      ((Result a -> a) -> Access (Result a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Result a -> a
forall a. Result a -> a
highpass)
      ((Result a -> a) -> Access (Result a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Result a -> a
forall a. Result a -> a
bandpass)
      ((Result a -> a) -> Access (Result a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Result a -> a
forall a. Result a -> a
lowpass)
      ((Result a -> a) -> Access (Result a) a
forall a r. Storable a => (r -> a) -> Access r a
Store.element Result a -> a
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      = a -> a
forall a. C a => a -> a
sin (a
forall a. C a => a
pia -> a -> a
forall a. C a => a -> a -> a
*a
frequency)
       w2 :: a
w2     = a
wa -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2
       q2 :: a
q2     = a
resonancea -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2
       q21w2 :: a
q21w2  = a
4a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
*(a
1a -> a -> a
forall a. C a => a -> a -> a
-a
w2)
       sqrtQZ :: a
sqrtQZ = a
w a -> a -> a
forall a. C a => a -> a -> a
* a -> a
forall a. C a => a -> a
sqrt (a
q21w2 a -> a -> a
forall a. C a => a -> a -> a
+ a
w2)
       pk1 :: a
pk1    = (a
w2a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ) a -> a -> a
forall a. C a => a -> a -> a
/ (a
q2a -> a -> a
forall a. C a => a -> a -> a
+a
w2a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ)
       d :: a
d      = (a
q21w2a -> a -> a
forall a. C a => a -> a -> a
*a
w2 a -> a -> a
forall a. C a => a -> a -> a
+ a
w2a -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2 a -> a -> a
forall a. C a => a -> a -> a
- a
q2)
                  a -> a -> a
forall a. C a => a -> a -> a
/ (a
q21w2 a -> a -> a
forall a. C a => a -> a -> a
- a
2a -> a -> a
forall a. C a => a -> a -> a
*a
q2 a -> a -> a
forall a. C a => a -> a -> a
- a
w2 a -> a -> a
forall a. C a => a -> a -> a
+ (a
1a -> a -> a
forall a. C a => a -> a -> a
-a
4a -> a -> a
forall a. C a => a -> a -> a
*a
w2)a -> a -> a
forall a. C a => a -> a -> a
*a
sqrtQZ)
       volHP :: a
volHP  = (a
2a -> a -> a
forall a. C a => a -> a -> a
-a
pk1)a -> a -> a
forall a. C a => a -> a -> a
/a
4 a -> a -> a
forall a. C a => a -> a -> a
- a
d
       volRel :: a
volRel = a -> a
forall a. C a => a -> a
sqrt ((a
2a -> a -> a
forall a. C a => a -> a -> a
-a
pk1 a -> a -> a
forall a. C a => a -> a -> a
+ a
4 a -> a -> a
forall a. C a => a -> a -> a
* a
d) a -> a -> a
forall a. C a => a -> a -> a
/ a
volHP)
   in  a -> a -> a -> a -> a -> a -> Parameter a
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1a -> a -> a
forall a. C a => a -> a -> a
/a
volRel)  a
volHP
          a
volHP  a
volRel  a
volRel  (a -> a
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      = a -> a
forall a. C a => a -> a
sin (a
forall a. C a => a
pia -> a -> a
forall a. C a => a -> a -> a
*a
frequency)
       w2 :: a
w2     = a
wa -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2
       q2 :: a
q2     = a
resonancea -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2
       sqrtQZ :: a
sqrtQZ = a
w a -> a -> a
forall a. C a => a -> a -> a
* a -> a
forall a. C a => a -> a
sqrt (a
4a -> a -> a
forall a. C a => a -> a -> a
*a
q2 a -> a -> a
forall a. C a => a -> a -> a
+ a
w2 a -> a -> a
forall a. C a => a -> a -> a
- a
4a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
*a
w2)
       pk1 :: a
pk1    = (a
w2a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ) a -> a -> a
forall a. C a => a -> a -> a
/ (a
q2a -> a -> a
forall a. C a => a -> a -> a
+a
w2a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ)
       zr :: a
zr     = a
1 a -> a -> a
forall a. C a => a -> a -> a
- a
2 a -> a -> a
forall a. C a => a -> a -> a
* a
w2
       pk2 :: a
pk2    = a
2a -> a -> a
forall a. C a => a -> a -> a
-a
pk1 a -> a -> a
forall a. C a => a -> a -> a
+
                   a
4 a -> a -> a
forall a. C a => a -> a -> a
* (a
w2a -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2a -> a -> a
forall a. C a => a -> a -> a
-a
q2a -> a -> a
forall a. C a => a -> a -> a
*a
zra -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2) a -> a -> a
forall a. C a => a -> a -> a
/ (a
2a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
*a
zra -> a -> a
forall a. C a => a -> a -> a
-a
w2a -> a -> a
forall a. C a => a -> a -> a
+(a
1a -> a -> a
forall a. C a => a -> a -> a
-a
4a -> a -> a
forall a. C a => a -> a -> a
*a
w2)a -> a -> a
forall a. C a => a -> a -> a
*a
sqrtQZ)
       volHP :: a
volHP  = (a
4a -> a -> a
forall a. C a => a -> a -> a
-a
2a -> a -> a
forall a. C a => a -> a -> a
*a
pk1a -> a -> a
forall a. C a => a -> a -> a
-a
pk2) a -> a -> a
forall a. C a => a -> a -> a
/ a
4
       volLP :: a
volLP  = a
pk2
       volBP :: a
volBP  = a -> a
forall a. C a => a -> a
sqrt (a
volHPa -> a -> a
forall a. C a => a -> a -> a
*a
volLP)
   in  a -> a -> a -> a -> a -> a -> Parameter a
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1a -> a -> a
forall a. C a => a -> a -> a
*a
volHPa -> a -> a
forall a. C a => a -> a -> a
/a
volBP)  (a
pk2a -> a -> a
forall a. C a => a -> a -> a
*a
volHPa -> a -> a
forall a. C a => a -> a -> a
/a
volLP)
          a
volHP  (a
volBPa -> a -> a
forall a. C a => a -> a -> a
/a
volHP)  (a
volLPa -> a -> a
forall a. C a => a -> a -> a
/a
volBP)  (a -> a
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     = a -> a
forall a. C a => a -> a
cos (a
2a -> a -> a
forall a. C a => a -> a -> a
*a
forall a. C a => a
pia -> a -> a
forall a. C a => a -> a -> a
*a
frequency)
       zr1 :: a
zr1    = a
zra -> a -> a
forall a. C a => a -> a -> a
-a
1
       q2 :: a
q2     = a
resonancea -> Integer -> a
forall a. C a => a -> Integer -> a
^Integer
2
       sqrtQZ :: a
sqrtQZ = a -> a
forall a. C a => a -> a
sqrt (a
zr1a -> a -> a
forall a. C a => a -> a -> a
*(-a
8a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
+a
zr1a -> a -> a
forall a. C a => a -> a -> a
-a
4a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
*a
zr1))
       pk1 :: a
pk1    = (-a
zr1a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ) a -> a -> a
forall a. C a => a -> a -> a
/ (a
2a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
-a
zr1a -> a -> a
forall a. C a => a -> a -> a
+a
sqrtQZ)
       q21zr :: a
q21zr  = a
4a -> a -> a
forall a. C a => a -> a -> a
*a
q2a -> a -> a
forall a. C a => a -> a -> a
*a
zr
       a :: a
a      = a
2 a -> a -> a
forall a. C a => a -> a -> a
* (a
zr1a -> a -> a
forall a. C a => a -> a -> a
*a
zr1a -> a -> a
forall a. C a => a -> a -> a
-a
q21zra -> a -> a
forall a. C a => a -> a -> a
*a
zr) a -> a -> a
forall a. C a => a -> a -> a
/ (a
zr1a -> a -> a
forall a. C a => a -> a -> a
+a
q21zra -> a -> a
forall a. C a => a -> a -> a
+(a
1a -> a -> a
forall a. C a => a -> a -> a
+a
2a -> a -> a
forall a. C a => a -> a -> a
*a
zr1)a -> a -> a
forall a. C a => a -> a -> a
*a
sqrtQZ)
       pk2 :: a
pk2    = a
aa -> a -> a
forall a. C a => a -> a -> a
+a
2a -> a -> a
forall a. C a => a -> a -> a
-a
pk1
       volHP :: a
volHP  = (a
4a -> a -> a
forall a. C a => a -> a -> a
-a
2a -> a -> a
forall a. C a => a -> a -> a
*a
pk1a -> a -> a
forall a. C a => a -> a -> a
-a
pk2) a -> a -> a
forall a. C a => a -> a -> a
/ a
4
       volLP :: a
volLP  = a
pk2
       volBP :: a
volBP  = a -> a
forall a. C a => a -> a
sqrt (a
volHPa -> a -> a
forall a. C a => a -> a -> a
*a
volLP)
   in  a -> a -> a -> a -> a -> a -> Parameter a
forall a. a -> a -> a -> a -> a -> a -> Parameter a
Parameter
          (a
pk1a -> a -> a
forall a. C a => a -> a -> a
*a
volHPa -> a -> a
forall a. C a => a -> a -> a
/a
volBP)  (a
pk2a -> a -> a
forall a. C a => a -> a -> a
*a
volHPa -> a -> a
forall a. C a => a -> a -> a
/a
volLP)
          a
volHP  (a
volBPa -> a -> a
forall a. C a => a -> a -> a
/a
volHP)  (a
volLPa -> a -> a
forall a. C a => a -> a -> a
/a
volBP)  (a -> a
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 = Parameter a -> a
forall a. Parameter a -> a
k1 Parameter a
p a -> a -> a
forall a. C a => a -> a -> a
- Parameter a -> a
forall a. Parameter a -> a
k2 Parameter a
p,
      d2 :: a
SecondOrder.d2 = a
1 a -> a -> a
forall a. C a => a -> a -> a
- Parameter 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 =
   (State v -> (Result v, State v))
-> StateT (State v) Identity (Result v)
forall (m :: * -> *) s a. Monad m => (s -> (a, s)) -> StateT s m a
MS.state ((State v -> (Result v, State v))
 -> StateT (State v) Identity (Result v))
-> (State v -> (Result v, State v))
-> StateT (State v) Identity (Result v)
forall a b. (a -> b) -> a -> b
$ \(v
i1,v
i2) ->
      let newsum :: v
newsum = Parameter a -> a
forall a. Parameter a -> a
ampIn Parameter a
p a -> v -> v
forall a v. C a v => a -> v -> v
*> v
u v -> v -> v
forall a. C a => a -> a -> a
+ Parameter a -> a
forall a. Parameter a -> a
k1 Parameter a
p a -> v -> v
forall a v. C a v => a -> v -> v
*> v
i1 v -> v -> v
forall a. C a => a -> a -> a
- Parameter a -> a
forall a. Parameter a -> a
k2 Parameter a
p a -> v -> v
forall a v. C a v => a -> v -> v
*> v
i2
          newi1 :: v
newi1  = v
i1 v -> v -> v
forall a. C a => a -> a -> a
- Parameter a -> a
forall a. Parameter a -> a
ampI1 Parameter a
p a -> v -> v
forall a v. C a v => a -> v -> v
*> v
newsum
          newi2 :: v
newi2  = v
i2 v -> v -> v
forall a. C a => a -> a -> a
- Parameter a -> a
forall a. Parameter a -> a
ampI2 Parameter a
p a -> v -> v
forall a v. C a v => a -> v -> v
*> v
newi1
          out :: Result v
out    = v -> v -> v -> v -> Result v
forall a. a -> a -> a -> a -> Result a
Result v
newsum v
newi1 v
newi2 (v
u v -> v -> v
forall a. C a => a -> a -> a
+ Parameter a -> a
forall a. Parameter a -> a
ampLimit Parameter a
p a -> v -> v
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 =
   ((v, v) -> (v, v))
-> (Parameter a -> v -> State (v, v) (Result v))
-> Initialized (v, v) (v, v) (Parameter a) v (Result v)
forall s init ctrl a b.
(init -> s)
-> (ctrl -> a -> State s b) -> Initialized s init ctrl a b
Modifier.Initialized (v, v) -> (v, v)
forall a. a -> a
id Parameter a -> v -> State (v, v) (Result v)
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 = ModifierInit (State v) (State v) (Parameter a) v (Result v)
-> State v -> Modifier (State v) (Parameter a) v (Result v)
forall s init ctrl a b.
ModifierInit s init ctrl a b -> init -> Modifier s ctrl a b
Sig.modifierInitialize ModifierInit (State v) (State v) (Parameter a) v (Result v)
forall a v.
(C a, C a v) =>
Initialized (State v) (State v) (Parameter a) v (Result v)
modifierInit (v
forall a. C a => a
zero, v
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 =
   Simple (State v) (Parameter a) v (Result v)
-> T (Parameter a, v) (Result v)
forall s ctrl a b. Simple s ctrl a b -> T (ctrl, a) b
Causal.fromSimpleModifier Simple (State v) (Parameter a) v (Result v)
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 = ModifierInit (State v) (State v) (Parameter a) v (Result v)
-> State v -> T (Parameter a) -> T v -> T (Result v)
forall s init ctrl a b.
ModifierInit s init ctrl a b -> init -> T ctrl -> T a -> T b
Sig.modifyModulatedInit ModifierInit (State v) (State v) (Parameter a) v (Result v)
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 = (v, v) -> T (Parameter a) -> T v -> T (Result v)
forall a v.
(C a, C a v) =>
(v, v) -> T (Parameter a) -> T v -> T (Result v)
runInit (v
forall a. C a => a
zero, v
forall a. C a => a
zero)