1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

extern crate num;

use num::Complex;
use std::f64::consts::{PI, E};

/// Computes the 'intermediate' filter sequence used in the Goertzel filter.
fn intermediate_filter(input: &Vec<f64>, angular_freq: &f64) -> Vec<f64> {
  let freq_term = 2.0 * angular_freq.cos();

  // {\displaystyle s[n]=x[n]+2\cos(\omega _{0})s[n-1]-s[n-2].}
  let mut intermediate_vec: Vec<f64> =
    Vec::with_capacity(input.len());

  // initialize intermediate vector with first two states
  intermediate_vec.push(input[0]);
  intermediate_vec.push(input[1] + freq_term * input[0]);

  for (ind, val) in input.iter().enumerate().skip(2) {
    let filter_term = val + freq_term * intermediate_vec[ind - 1] -
      intermediate_vec[ind - 2];
    intermediate_vec.push(filter_term);
  }

  intermediate_vec
}

/// Applies the two Goertzel filter steps to an input sequence.
pub fn filter_naive(input: &Vec<f64>,
                    linear_freq: f64) -> Vec<Complex<f64>> {
  let angular_freq = linear_freq / 2.0 / PI;
  
  let intermediate_vec = intermediate_filter(&input, &angular_freq);

  // {\displaystyle y[n]=s[n]-e^{-j\omega _{0}}s[n-1].}
  let mut output_vec: Vec<Complex<f64>> =
    Vec::with_capacity(input.len());

  // initialize output vector with first state
  output_vec.push(Complex::from(intermediate_vec[0]));
  
  // backwards from the usual signature, this is actually e^(jw).
  let exponential_term = (- Complex::i() * angular_freq).expf(E);
  
  for (ind, val) in intermediate_vec.iter().enumerate().skip(1) {
    output_vec.push(val - exponential_term * intermediate_vec[ind - 1]);
  }

  output_vec
}

fn bin_freq(linear_freq: f64, signal_length: usize) -> (usize, f64) {
  let signal_len = signal_length as f64;

  let angular_freq = linear_freq / 2.0 / PI;
  
  let approx_bin_num = angular_freq * &signal_len / 2.0 / PI;
  let bin_num: usize = (approx_bin_num) as usize;
  let bin_freq = bin_num as f64 / &signal_len * 2.0 * PI;
    
  if approx_bin_num != bin_num as f64 {
    println!("Warning: aliasing frequency to {} to fit in bin...",
             &bin_freq * 2.0 * PI);
  }

  (bin_num, bin_freq)
}

/// Computes the DFT term of a signal for a single given frequency.
pub fn dft(input: &Vec<f64>, linear_freq: f64) -> Complex<f64> {
  // Constrain frequency to 2 pi k / N
  let bin_freq = bin_freq(linear_freq, input.len()).1;
  
  let mut intermediate_vec = intermediate_filter(&input, &bin_freq);
  intermediate_vec.pop();
  
  // backwards from the usual signature, this is actually e^(jw).
  let exponential_term = (Complex::i() * bin_freq).expf(E);
  
  exponential_term * Complex::from(intermediate_vec.pop().unwrap()) -
    Complex::from(intermediate_vec.pop().unwrap())
}

/// Computes the power at a single DFT frequency.
pub fn dft_power(input: &Vec<f64>, linear_freq: f64) -> f64 {
  // Constrain frequency to 2 pi k / N
  let bin_freq = bin_freq(linear_freq, input.len()).1;

  // sequence s[n], where we throw away s[N]
  let mut intermediate_vec = intermediate_filter(&input, &bin_freq);
  intermediate_vec.pop();
  
  // s[n - 1]
  let state_1 = intermediate_vec.pop().unwrap();
  // s[n - 2]
  let state_2 = intermediate_vec.pop().unwrap();
  
  state_1.powi(2) + state_2.powi(2) - 2.0 * bin_freq.cos() * state_1 * state_2
}