// Numerically controlled oscillator

use crate::complex::{Complex, Complex32};
use std::f32::consts::PI;
use std::ops::{Add, Div, Mul, Sub};

// Utilities
fn map<T>(input: T, in_min: T, in_max: T, out_min: T, out_max: T) -> T
where
    T: Clone + Add<Output = T> + Mul<Output = T> + Sub<Output = T> + Div<Output = T>,
{
    ((input - in_min.clone()) / (in_max - in_min)) * (out_max - out_min.clone()) + out_min
}

fn euclid_mod(a: f32, m: f32) -> f32 {
    let r = a % m;
    if r < 0.0 { r + m } else { r }
}

pub struct Nco {
    // Phase of NCO
    theta: u32,  // 0 <=> 0, 0xFFFFFFFF <=> 2pi
    dtheta: u32, // Dtheta = freq : f = dtheta/dt
}

impl Nco {
    pub fn new(freq: f32) -> Self {
        let mut nco = Nco {
            theta: 0,
            dtheta: 0,
        };
        nco.set_frequency(freq);
        nco
    }

    // Sets freq, freq in radian per sample
    pub fn set_frequency(&mut self, freq: f32) {
        self.dtheta = map(
            euclid_mod(freq, 2. * PI),
            0.,
            2. * PI,
            0.,
            0xFFFFFFFFu32 as f32,
        )
        .floor() as u32;
    }

    // Adjusts freq, freq in radian per sample
    pub fn adjust_frequency(&mut self, freq_off: f32) {
        self.set_frequency(self.get_frequency() + freq_off);
    }

    pub fn adjust_phase(&mut self, phase_off: f32) {
        let offset = map(
            euclid_mod(phase_off, 2. * PI),
            0.,
            2. * PI,
            0.,
            0xFFFFFFFFu32 as f32,
        )
        .floor() as u32;
        self.theta = self.theta.overflowing_add(offset).0;
    }

    pub fn get_frequency(&self) -> f32 {
        map(self.dtheta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI)
    }

    pub fn step(&mut self) {
        let bef = self.theta as i64;
        self.theta = self.theta.overflowing_add(self.dtheta).0;
    }

    pub fn step_n(&mut self, n: u32) {
        self.theta = self
            .theta
            .overflowing_add(self.dtheta.overflowing_mul(n).0)
            .0;
    }

    pub fn sin(&self) -> f32 {
        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).sin()
    }

    pub fn cos(&self) -> f32 {
        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).cos()
    }

    pub fn cexp(&self) -> Complex<f32> {
        //Complex::new(self.cos(), self.sin())
        Complex::cexp(map(
            self.theta as f32,
            0.,
            0xFFFFFFFFu32 as f32,
            0.,
            2. * PI,
        ))
    }
}

impl Iterator for Nco
{
    type Item = Complex32;

    fn next(&mut self) -> Option<Self::Item> {
        let val = self.cexp();
        self.step();
        Some(val)
    }
}
