-----------------------------------------------------------------------------
-- |
-- Module      :  DSP.Estimation.Frequency.PerMax
-- Copyright   :  (c) Matthew Donadio 2003
-- License     :  GPL
--
-- Maintainer  :  m.p.donadio@ieee.org
-- Stability   :  experimental
-- Portability :  portable
--
-- This module implements an algorithm to maximize the peak value of a
-- DFT\/FFT.  It is based off an aticle by Mark Sullivan from Personal
-- Engineering Magazine.
--
-- Maximizes
--
-- @S(w) = 1\/N * sum(k=0,N-1) |x[k] * e^(-jwk)|^2@
--
-- which is equivalent to solving
--
-- @S'(w) = Im{X(w) * ~Y(w)} = 0@
--
-- where
--
-- @X(w) =         sum(k=0,N-1) (x[k] * e^(-jwk))@
-- @Y(w) = X'(w) = sum(k=0,N-1) (k * x[k] * e^(-jwk))@
--
-- This algorithm used the bisection method for finding the zero of a
-- function.  The search area is +- half a bin width.
--
-- Regula falsi requires an additional (x,f(x)) pair which is expensive
-- in this case.  Newton's method could be used but requires S''(w),
-- which takes twice as long to caculate as S'(w).  Brent's method may be
-- best here, but it also requires three (x,f(x)) pairs
--
-----------------------------------------------------------------------------

module DSP.Estimation.Frequency.PerMax (permax) where

import Data.Array
import Data.Complex

-- TODO: could we use sinc interpolation instead of calc_x,calc_y for
-- the off-bin values?

-- TODO: the twiddle factor in calc_x,calc_y can be computed
-- recursively

-- TODO: the twiddle factor in calc_x,calc_y can be shared


-- calc_x x w = sum [ x!k * cis (-w * fromIntegral k) | k <- [0..(n-1)] ]
--      where n = snd (bounds x) + 1

calc_x :: (RealFloat a, Ix i) =>
          Array i (Complex a) -> a -> Complex a
calc_x :: forall a i.
(RealFloat a, Ix i) =>
Array i (Complex a) -> a -> Complex a
calc_x Array i (Complex a)
x a
w = forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum forall a b. (a -> b) -> a -> b
$ forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith forall a. Num a => a -> a -> a
(*) (forall i e. Array i e -> [e]
elems Array i (Complex a)
x) (forall a. (a -> a) -> a -> [a]
iterate (forall a. Floating a => a -> Complex a
cis (-a
w) forall a. Num a => a -> a -> a
*) Complex a
1)

calc_y :: (RealFloat b, Ix i, Integral i) =>
          Array i (Complex b) -> b -> Complex b
calc_y :: forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> Complex b
calc_y Array i (Complex b)
x b
w = forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [ forall a b. (Integral a, Num b) => a -> b
fromIntegral i
k forall a. Num a => a -> a -> a
* Array i (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!i
k forall a. Num a => a -> a -> a
* forall a. Floating a => a -> Complex a
cis (-b
w forall a. Num a => a -> a -> a
* forall a b. (Integral a, Num b) => a -> b
fromIntegral i
k) | i
k <- [i
0..(i
nforall a. Num a => a -> a -> a
-i
1)] ]
    where n :: i
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array i (Complex b)
x) forall a. Num a => a -> a -> a
+ i
1

-- | Discrete frequency periodigram maximizer

permax :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

permax :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
permax Array a (Complex b)
x a
k = forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> b -> b
permax' Array a (Complex b)
x (b
wforall a. Num a => a -> a -> a
-b
d) (b
wforall a. Num a => a -> a -> a
+b
d)
    where w :: b
w = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k forall a. Fractional a => a -> a -> a
/ forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n
          d :: b
d = b
1 forall a. Fractional a => a -> a -> a
/ forall a b. (Integral a, Num b) => a -> b
fromIntegral (a
2forall a. Num a => a -> a -> a
*a
n) -- half a bin width
          n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

permax' :: (RealFloat b, Ix i, Integral i) =>
           Array i (Complex b) -> b -> b -> b
permax' :: forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> b -> b
permax' Array i (Complex b)
x b
w0 b
w1 | b
w1forall a. Num a => a -> a -> a
-b
w0 forall a. Ord a => a -> a -> Bool
< b
eps = b
wmid
                | Bool
otherwise   = if forall a. Num a => a -> a
signum b
t0 forall a. Eq a => a -> a -> Bool
== forall a. Num a => a -> a
signum b
tm
                                then forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> b -> b
permax' Array i (Complex b)
x b
wmid b
w1
                                else forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> b -> b
permax' Array i (Complex b)
x b
w0   b
wmid
    where t0 :: b
t0 = forall a. Complex a -> a
imagPart ((forall a i.
(RealFloat a, Ix i) =>
Array i (Complex a) -> a -> Complex a
calc_x Array i (Complex b)
x b
w0)   forall a. Num a => a -> a -> a
* (forall a. Num a => Complex a -> Complex a
conjugate (forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> Complex b
calc_y Array i (Complex b)
x b
w0)))
          tm :: b
tm = forall a. Complex a -> a
imagPart ((forall a i.
(RealFloat a, Ix i) =>
Array i (Complex a) -> a -> Complex a
calc_x Array i (Complex b)
x b
wmid) forall a. Num a => a -> a -> a
* (forall a. Num a => Complex a -> Complex a
conjugate (forall b i.
(RealFloat b, Ix i, Integral i) =>
Array i (Complex b) -> b -> Complex b
calc_y Array i (Complex b)
x b
wmid)))
--        t1 = imagPart ((calc_x x w1)   * (conjugate (calc_y x w1)))
          wmid :: b
wmid = (b
w0 forall a. Num a => a -> a -> a
+ b
w1) forall a. Fractional a => a -> a -> a
/ b
2 -- bisection method
--          wmid = w1 - t1 * (w1 - w0) / (t1 - t0) -- regula falsi
          eps :: b
eps = b
1.0e-6
--        n = snd (bounds x) + 1