extern crate goertzel_filter;
extern crate num;
extern crate itertools_num;

use num::Complex;
use itertools_num::linspace;
use std::ops::Sub;
use std::f64::consts::{PI};

static TAU: f64 = 2.0 * PI;

// with great help from http://codereview.stackexchange.com/a/144037/105623
fn first_difference<T>(input: &[T]) -> Vec<T>
  where for <'a> &'a T: Sub<Output = T> {

  let minuend_iter = input.iter();
  let subtrahend_iter = input.iter().skip(1);
  
  minuend_iter.zip(subtrahend_iter).map(|(minuend, subtrahend)| {
    minuend - subtrahend
  }).collect()
}

fn main() {
  // Maximum frequency (so sample at double this)
  let max_freq: f64 = 500.0;
  let interval_length = 0.5;
  
  // Time vector sampled at 1000 times/s (~Nyquist), over 500ms
  let delta: f64 = &interval_length / &max_freq / 2.0;
  let time_500ms: Vec<f64> = linspace(0.0,
                                      interval_length,
                                      (&max_freq / 2.0) as usize)
    .map(|sample| { sample * delta })
    .collect();

  // Number of offsets to use
  let offset_count = 15;
  // How far apart the offset is in sample if they were equally spaced
  let offset_length = (time_500ms.len() / &offset_count) as usize;

  // Create a bunch of unit impulses offset by that many samples
  let offset_impulses: Vec<Vec<f64>> = (0..offset_count)
    .map(|offset_ind| {
      let mut impulse: Vec<f64> = time_500ms.clone();

      impulse[offset_ind as usize * offset_length] = 1.0;
      
      impulse
    })
    .collect();

  let dft_freq = 500.0;

  let impulse_dfts: Vec<Complex<f64>> = offset_impulses.iter().map(|impulse| {
    goertzel_filter::dft(&impulse, dft_freq)
  }).collect();

  let dft_phases: Vec<f64> = impulse_dfts
    .iter()
    .map(|&dft| { ((dft.arg() % TAU) + TAU) % TAU }) // modulo workaround
    .collect();

  for phase in &dft_phases {
    println!("phase: {}", phase);
  }

  let dft_phase_diffs: Vec<f64> = first_difference(&dft_phases)
    .iter().map(|&diff| { (diff + TAU ) % TAU }).collect();

  for diff in dft_phase_diffs { println!("phase diff: {}", &diff); }
}