use std::f32::consts::PI;
use std::marker::PhantomData;

use num::Complex;
use num::Float;

use crate::map;
use crate::units::DigitalFrequency;
use crate::units::Phase;

/// Numericaly controlled oscillator
///
/// ```
/// let nco: Nco<f32> = DigitalFrequency::from_rad(2 * f32::PI).into();
/// // Or
/// let nco: Nco<f32> = Nco::from(DigitalFrequency::from_rad(2 * f32::PI));
///
/// // Rotates by 90deg per sample
/// // The next function is from the iterator implementation
/// assert_eq!(nco.next(), Complex::new(1., 0.));
/// assert_eq!(nco.next(), Complex::new(0., 1.));
/// assert_eq!(nco.next(), Complex::new(-1., 0.));
/// assert_eq!(nco.next(), Complex::new(0., -1.));
/// assert_eq!(nco.next(), Complex::new(1., 0.));
/// ```
#[derive(Clone, Copy)]
pub struct Nco<T>
{
    // Current phase angle of the oscillator
    phase: usize,

    // "Phase derivative": How much to increase the phase per sample
    d_phase: usize,
    _phantom: PhantomData<T>,
}

impl<T> Nco<T>
{
    /// Creates a new Nco with a specific frequency starting at phase 0
    pub fn new(frequency: DigitalFrequency) -> Self
    {
        Self {
            phase: 0,
            d_phase: frequency.0,
            _phantom: Default::default(),
        }
    }

    /// Gets the current frequency of the oscillator
    pub fn frequency(&self) -> DigitalFrequency
    {
        DigitalFrequency(self.d_phase)
    }

    /// Creates a new Nco with a specific frequency and starting phase
    pub fn with_phase(frequency: DigitalFrequency, phase: Phase) -> Self
    {
        Self {
            phase: phase.0.0,
            d_phase: frequency.0,
            _phantom: Default::default(),
        }
    }

    /// Sets the current phase.
    pub fn set_phase(&mut self, phase: Phase)
    {
        self.phase = phase.0.0;
    }

    /// Sets the current phase
    pub fn set_frequency(&mut self, frequency: DigitalFrequency)
    {
        self.d_phase = frequency.0;
    }

    /// Steps the oscillator by one sample
    pub fn step(&mut self)
    {
        let (new, _) = self.phase.overflowing_add(self.d_phase);
        self.phase = new;
    }
}

impl<T: Float + From<f32>> Nco<T>
{
    /// Gets the current value of the oscillator as a
    /// complex number
    pub fn sample(&self) -> Complex<T>
    {
        let t = map(
            <T as From<f32>>::from(self.phase as f32),
            <T as From<f32>>::from(0.0f32),
            <T as From<f32>>::from(usize::MAX as f32),
            <T as From<f32>>::from(0.0f32),
            <T as From<f32>>::from(2. * PI),
        );
        Complex::new(t.cos(), t.sin())
    }
}

impl<T: Float + From<f32>> Iterator for Nco<T>
{
    type Item = Complex<T>;

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

impl<T> From<DigitalFrequency> for Nco<T>
{
    fn from(value: DigitalFrequency) -> Self
    {
        Nco::new(value)
    }
}
