{-# OPTIONS -fglasgow-exts -fno-implicit-prelude #-}
{- |
Copyright   :  (c) Henning Thielemann 2008
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 at once.

ToDo: band limit filter as sum of input and band pass
-}
module Synthesizer.Plain.Filter.Recursive.Universal where

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

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

import Algebra.Module((*>))

import Control.Monad.State (State(..), )

import qualified Prelude as P
import PreludeBase
import NumericPrelude


data Parameter a =
        Parameter {k1, k2, ampIn, ampI1, ampI2 :: !a}

data Result a =
        Result {highpass, bandpass, lowpass :: !a}

instance Additive.C v => Additive.C (Result v) where
   {-# INLINE zero #-}
   {-# INLINE (+) #-}
   {-# INLINE (-) #-}
   {-# INLINE negate #-}
   zero = Result zero zero zero
   (+) (Result xhp xbp xlp) (Result yhp ybp ylp) = Result (xhp + yhp) (xbp + ybp) (xlp + ylp)
   (-) (Result xhp xbp xlp) (Result yhp ybp ylp) = Result (xhp - yhp) (xbp - ybp) (xlp - ylp)
   negate                   (Result xhp xbp xlp) = Result (negate xhp) (negate xbp) (negate xlp)


instance Module.C a v => Module.C a (Result v) where
   {-# INLINE (*>) #-}
   s *> (Result hp bp lp) = Result (s *> hp) (s *> bp) (s *> lp)


{-| Universal filter: Computes high pass, band pass, low pass in one go -}
{-# INLINE parameter #-}
parameter :: Trans.C a => Pole a -> Parameter a
parameter (Pole resonance frequency) =
    let zr     = cos (2*pi*frequency)
        zr1    = zr-1
        q2     = resonance^2
        sqrtQZ = sqrt (zr1*(-8*q2+zr1-4*q2*zr1))
        pk1    = (-zr1+sqrtQZ) / (2*q2-zr1+sqrtQZ)
        q21zr  = 4*q2*zr
        a      = 2 * (zr1*zr1-q21zr*zr) / (zr1+q21zr+(1+2*zr1)*sqrtQZ)
        pk2    = a+2-pk1
        volHP  = (4-2*pk1-pk2) / 4
        volLP  = pk2
        volBP  = sqrt (volHP*volLP)
    in  Parameter
           (pk1*volHP/volBP)  (pk2*volHP/volLP)
           volHP  (volBP/volHP)  (volLP/volBP)

{-# INLINE step #-}
step :: (Ring.C a, Module.C a v) =>
   Parameter a -> v -> State (v,v) (Result v)
step p u =
   State $ \(i1,i2) ->
      let newsum = ampIn p *> u + k1 p *> i1 - k2 p *> i2
          newi1  = i1 - ampI1 p *> newsum
          newi2  = i2 - ampI2 p *> newi1
          out    = Result newsum newi1 newi2
      in  (out, (newi1, newi2))

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

{-# INLINE modifier #-}
modifier :: (Ring.C a, Module.C a v) =>
   Modifier.Simple (v,v) (Parameter a) v (Result v)
modifier = Sig.modifierInitialize modifierInit (zero, zero)

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

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