// Square wave generator
use sound::AudioComponent;
use sound::OUTPUT_SAMPLE_RATE;

#[derive(Debug, Default)]
struct SamplePair(u8);

impl SamplePair {
    fn first(&self) -> u8 {
        self.0 >> 4
    }

    fn second(&self) -> u8 {
        self.0 & 0x0F
    }
}

impl From<u8> for SamplePair {
    fn from(val: u8) -> SamplePair {
        SamplePair(val)
    }
}

#[derive(Debug, Default)]
pub struct WaveGenerator {
    frequency: f32,
    time: f32,
    gb_reg_freq: u16,

    pub enabled: bool,
    samples: [SamplePair; 16],
}

impl WaveGenerator {
    fn update_frequency(&mut self) {
        self.frequency = ((2048 - self.gb_reg_freq) * 2) as _;
        self.time = 0f32;
        println!("Freq = {}", self.frequency);
    }

    pub fn set_lower_freq(&mut self, freq: u8) {
        self.gb_reg_freq &= 0xFF00;
        self.gb_reg_freq |= freq as u16;
        self.update_frequency();
    }

    pub fn set_higher_freq(&mut self, freq: u8) {
        self.gb_reg_freq &= 0x00FF;
        self.gb_reg_freq |= ((freq & 7) as u16) << 8;
        self.update_frequency();
    }

    pub fn reset(&mut self)  {
        self.time = 0f32;
    }

    pub fn sample(&self) -> u8 {
        if self.enabled && self.frequency > 0f32 {
            let temp = self.time * self.frequency;
            let position = temp.trunc() as usize % 32;

            let sample = &self.samples[position / 2];
            match position % 2 {
                0 => sample.first(),
                _ => sample.second(),
            }
        } else {
            0
        }
    }

    pub fn set_sample_pair(&mut self, nr: usize, val: u8) {
        self.samples[nr] = val.into();
    }
}

impl AudioComponent for WaveGenerator {
    fn clock(&mut self) {
        self.time += 1f32 / (OUTPUT_SAMPLE_RATE as f32);
    }
}