use core::marker::PhantomData;
use embedded_hal::blocking::i2c;

const I2C_ADDR: u8 = 96;

const CLK_ENABLE_CONTROL: u8 = 3;
//const PLLX_SRC: u8 = 15;
const PLL_RESET: u8 = 177;
const XTAL_LOAD_CAP: u8 = 183;

#[derive(PartialEq, Copy, Clone)]
pub enum PLL {
    A,
    B,
}

const PLL_BASE_ADDR: [u8; 2] = [26, 34];

impl PLL {
    fn base_address(&self) -> u8 {
        return PLL_BASE_ADDR[*self as usize];
    }
}

#[derive(Copy, Clone)]
pub enum Multisynth {
    MS0,
    MS1,
    MS2,
}

const MS_BASE_ADDR: [u8; 3] = [42, 50, 58];
const MS_CTRL_ADDR: [u8; 3] = [16, 17, 18];

impl Multisynth {
    fn base_address(&self) -> u8 {
        return MS_BASE_ADDR[*self as usize];
    }

    fn ctrl_address(&self) -> u8 {
        return MS_CTRL_ADDR[*self as usize];
    }
}

pub struct PllParams {
    pub p1: u32,
    pub p2: u32,
    pub p3: u32,
}

pub struct Si5153<I2C> {
    // Marker that makes sure we always get the same I2C
    i2c: PhantomData<I2C>,
    pll_freqs: [u32; 2],
    outputs: u8,
    ms_srcs: [PLL; 3],
}

impl<I2C, E> Si5153<I2C>
where
    I2C: i2c::Write<Error = E>,
{
    pub fn new(_i2c: &I2C) -> Self {
        Si5153 {
            i2c: PhantomData,
            pll_freqs: [0, 0],
            outputs: 0,
            ms_srcs: [PLL::A, PLL::A, PLL::A],
        }
    }

    pub fn init(&mut self, i2c: &mut I2C, freq_xtal: u32, freq_a: u32, freq_b: u32) {
        self.pll_freqs[PLL::A as usize] = freq_a;
        self.pll_freqs[PLL::B as usize] = freq_b;

        self.outputs = 0xFF;
        self.write_byte_reg(i2c, CLK_ENABLE_CONTROL, self.outputs); // Disable all outputs
        self.write_byte_reg(i2c, XTAL_LOAD_CAP, 0xD2); //crystal load capacitor = 10pF

        self.write_byte_reg(i2c, PLL_RESET, 0xA0); // Reset both PLLs

        for ms in [Multisynth::MS0, Multisynth::MS1, Multisynth::MS2].iter() {
            self.ms_srcs[*ms as usize] = PLL::A;
            self.write_byte_reg(i2c, ms.ctrl_address(), 0x0F); // MSi as Source, PLLA to MSi, 8 mA output
        }

        for pll in [PLL::A, PLL::B].iter() {
            let fdiv = self.pll_freqs[*pll as usize] / freq_xtal;
            let rm = self.pll_freqs[*pll as usize] % freq_xtal;

            //TODO: Find better way to determine c and b
            let c = 0x0FFFFF;
            let a = fdiv;
            let b = ((rm as u64) * (c as u64) / (freq_xtal as u64)) as u32;

            let params = PllParams {
                p1: 128 * a + (128 * b / c) - 512,
                p2: 128 * b - c * (128 * b / c),
                p3: c,
            };

            self.write_params(i2c, pll.base_address(), &params)
        }
    }

    pub fn enable_ms_output(&mut self, i2c: &mut I2C, synth: Multisynth) {
        self.outputs &= !(1 << (synth as u8));
        self.write_byte_reg(i2c, CLK_ENABLE_CONTROL, self.outputs);
    }

    pub fn disable_ms_output(&mut self, i2c: &mut I2C, synth: Multisynth) {
        self.outputs |= 1 << (synth as u8);
        self.write_byte_reg(i2c, CLK_ENABLE_CONTROL, self.outputs);
    }

    pub fn set_ms_source(&mut self, i2c: &mut I2C, synth: Multisynth, pll: PLL) {
        let value: u8 = if pll == PLL::A {
            self.ms_srcs[synth as usize] = PLL::A;
            0x0F // MS as Source, PLLA to MS, 8 mA output
        } else {
            self.ms_srcs[synth as usize] = PLL::B;
            0x2F // MS as Source, PLLB to MS, 8 mA output
        };

        self.write_byte_reg(i2c, synth.ctrl_address(), value);
    }

    pub fn set_ms_freq(&mut self, i2c: &mut I2C, synth: Multisynth, freq: u32) {
        let pll = self.ms_srcs[synth as usize];
        let fdiv = self.pll_freqs[pll as usize] / freq;
        let rm = self.pll_freqs[pll as usize] % freq;

        //TODO: Find better way to determine c and b
        let c: u32 = 0x0FFFFF;
        let a: u32 = fdiv;
        let b: u32 = ((rm as u64) * (c as u64) / (freq as u64)) as u32;

        let params = PllParams {
            p1: 128 * a + (128 * b / c) - 512,
            p2: 128 * b - c * (128 * b / c),
            p3: c,
        };
        self.write_params(i2c, synth.base_address(), &params)
    }

    fn write_byte_reg(&self, i2c: &mut I2C, reg_addr: u8, data: u8) {
        let res = i2c.write(I2C_ADDR, &[reg_addr, data]);
        if res.is_err() {
            panic!("i2c write failed. regAdder: {}", reg_addr)
        }
    }

    fn write_params(&self, i2c: &mut I2C, base: u8, params: &PllParams) {
        let data: [u8; 9] = [
            base,
            ((params.p3 & 0x00FF00) >> 8) as u8,
            (params.p3 & 0x0000FF) as u8,
            ((params.p1 & 0x030000) >> 16) as u8,
            ((params.p1 & 0x00FF00) >> 8) as u8,
            (params.p1 & 0x0000FF) as u8,
            (((params.p3 & 0x0F0000) >> 12) | ((params.p2 & 0x0F0000) >> 16)) as u8,
            ((params.p2 & 0x00FF00) >> 8) as u8,
            (params.p2 & 0x0000FF) as u8,
        ];

        let res = i2c.write(I2C_ADDR, &data);
        if res.is_err() {
            panic!("i2c write failed. regAdder: {}", base)
        }
    }

    pub fn write_synth_params(&self, i2c: &mut I2C, synth: Multisynth, params: &PllParams) {
        self.write_params(i2c, synth.base_address(), params);
    }
}