use std::f32::consts::PI;

use crate::{complex::Complex32, filtering::fir::FIRFilter, map, nco::Nco, windows};

pub struct IQSampler
{
    local_oscillator: Nco,
    low_pass_i: FIRFilter,
    low_pass_q: FIRFilter,
}

impl IQSampler
{
    pub fn new(center_freq: f32) -> Self {
        // Design a lowpass filter that cuts off at the center freq
        // Estimate FIR length :
        let fir_length = 50;

        // Ideal transfer function :
        let mut transfer_function = vec![Complex32::zero(); fir_length];
        let bin_id = map(center_freq, 0., PI, 0., transfer_function.len() as f32 / 2.).floor() as usize;
        for i in 0..bin_id
        {
            transfer_function[i] = Complex32::new(1., 0.);
            transfer_function[fir_length - 1 - i] = Complex32::new(1., 0.);
        }

        let ir = crate::filtering::impulse_response::design::ir_from_transfer_function(&transfer_function, fir_length, windows::blackmann);
        let mut low_pass_i = FIRFilter::new(&ir);
        let mut low_pass_q = FIRFilter::new(&ir);
        low_pass_i.normalize_sum();
        low_pass_q.normalize_sum();

        IQSampler {
            local_oscillator: Nco::new(center_freq),
            low_pass_i,
            low_pass_q
        }
    }

    pub fn sample(&mut self, input_sample: f32) -> Complex32
    {
        let i_mixed = self.local_oscillator.cexp().re * input_sample;
        let q_mixed = self.local_oscillator.cexp().im * input_sample;
        self.local_oscillator.step();

        // TODO: Could use one filter for both I and Q
        Complex32::new(
            self.low_pass_i.next(Complex32::new(i_mixed, 0.)).re,
            self.low_pass_q.next(Complex32::new(q_mixed, 0.)).re
        ) * 2.

    }
}
