use core::result::Result::Ok;

use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
use libboard_zynq::timer::GlobalTimer;
use log::info;

use crate::pl::csr;

#[cfg(feature = "target_kasli_soc")]
const ADDRESS: u8 = 0x67;

mod i2c {
    use super::*;

    #[derive(Clone, Copy)]
    pub enum DCXO {
        Main,
        #[cfg(has_wrpll)]
        Helper,
    }

    fn half_period(timer: &mut GlobalTimer) {
        timer.delay_us(1)
    }

    fn sda_i(dcxo: DCXO) -> bool {
        match dcxo {
            DCXO::Main => unsafe { csr::main_dcxo::sda_in_read() != 0 },
            #[cfg(has_wrpll)]
            DCXO::Helper => unsafe { csr::helper_dcxo::sda_in_read() != 0 },
        }
    }

    fn sda_oe(dcxo: DCXO, oe: bool) {
        let val = if oe { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::main_dcxo::sda_oe_write(val) },
            #[cfg(has_wrpll)]
            DCXO::Helper => unsafe { csr::helper_dcxo::sda_oe_write(val) },
        };
    }

    fn sda_o(dcxo: DCXO, o: bool) {
        let val = if o { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::main_dcxo::sda_out_write(val) },
            #[cfg(has_wrpll)]
            DCXO::Helper => unsafe { csr::helper_dcxo::sda_out_write(val) },
        };
    }

    fn scl_oe(dcxo: DCXO, oe: bool) {
        let val = if oe { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::main_dcxo::scl_oe_write(val) },
            #[cfg(has_wrpll)]
            DCXO::Helper => unsafe { csr::helper_dcxo::scl_oe_write(val) },
        };
    }

    fn scl_o(dcxo: DCXO, o: bool) {
        let val = if o { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::main_dcxo::scl_out_write(val) },
            #[cfg(has_wrpll)]
            DCXO::Helper => unsafe { csr::helper_dcxo::scl_out_write(val) },
        };
    }

    pub fn init(dcxo: DCXO, timer: &mut GlobalTimer) -> Result<(), &'static str> {
        // Set SCL as output, and high level
        scl_o(dcxo, true);
        scl_oe(dcxo, true);
        // Prepare a zero level on SDA so that sda_oe pulls it down
        sda_o(dcxo, false);
        // Release SDA
        sda_oe(dcxo, false);

        // Check the I2C bus is ready
        half_period(timer);
        half_period(timer);
        if !sda_i(dcxo) {
            // Try toggling SCL a few times
            for _bit in 0..8 {
                scl_o(dcxo, false);
                half_period(timer);
                scl_o(dcxo, true);
                half_period(timer);
            }
        }

        if !sda_i(dcxo) {
            return Err("SDA is stuck low and doesn't get unstuck");
        }
        Ok(())
    }

    pub fn start(dcxo: DCXO, timer: &mut GlobalTimer) {
        // Set SCL high then SDA low
        scl_o(dcxo, true);
        half_period(timer);
        sda_oe(dcxo, true);
        half_period(timer);
    }

    pub fn stop(dcxo: DCXO, timer: &mut GlobalTimer) {
        // First, make sure SCL is low, so that the target releases the SDA line
        scl_o(dcxo, false);
        half_period(timer);
        // Set SCL high then SDA high
        sda_oe(dcxo, true);
        scl_o(dcxo, true);
        half_period(timer);
        sda_oe(dcxo, false);
        half_period(timer);
    }

    pub fn write(dcxo: DCXO, data: u8, timer: &mut GlobalTimer) -> bool {
        // MSB first
        for bit in (0..8).rev() {
            // Set SCL low and set our bit on SDA
            scl_o(dcxo, false);
            sda_oe(dcxo, data & (1 << bit) == 0);
            half_period(timer);
            // Set SCL high ; data is shifted on the rising edge of SCL
            scl_o(dcxo, true);
            half_period(timer);
        }
        // Check ack
        // Set SCL low, then release SDA so that the I2C target can respond
        scl_o(dcxo, false);
        half_period(timer);
        sda_oe(dcxo, false);
        // Set SCL high and check for ack
        scl_o(dcxo, true);
        half_period(timer);
        // returns true if acked (I2C target pulled SDA low)
        !sda_i(dcxo)
    }

    pub fn read(dcxo: DCXO, ack: bool, timer: &mut GlobalTimer) -> u8 {
        // Set SCL low first, otherwise setting SDA as input may cause a transition
        // on SDA with SCL high which will be interpreted as START/STOP condition.
        scl_o(dcxo, false);
        half_period(timer); // make sure SCL has settled low
        sda_oe(dcxo, false);

        let mut data: u8 = 0;

        // MSB first
        for bit in (0..8).rev() {
            scl_o(dcxo, false);
            half_period(timer);
            // Set SCL high and shift data
            scl_o(dcxo, true);
            half_period(timer);
            if sda_i(dcxo) {
                data |= 1 << bit
            }
        }
        // Send ack
        // Set SCL low and pull SDA low when acking
        scl_o(dcxo, false);
        if ack {
            sda_oe(dcxo, true)
        }
        half_period(timer);
        // then set SCL high
        scl_o(dcxo, true);
        half_period(timer);

        data
    }
}

fn write(dcxo: i2c::DCXO, reg: u8, val: u8, timer: &mut GlobalTimer) -> Result<(), &'static str> {
    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, ADDRESS << 1, timer) {
        return Err("Si549 failed to ack write address");
    }
    if !i2c::write(dcxo, reg, timer) {
        return Err("Si549 failed to ack register");
    }
    if !i2c::write(dcxo, val, timer) {
        return Err("Si549 failed to ack value");
    }
    i2c::stop(dcxo, timer);
    Ok(())
}

fn read(dcxo: i2c::DCXO, reg: u8, timer: &mut GlobalTimer) -> Result<u8, &'static str> {
    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, ADDRESS << 1, timer) {
        return Err("Si549 failed to ack write address");
    }
    if !i2c::write(dcxo, reg, timer) {
        return Err("Si549 failed to ack register");
    }
    i2c::stop(dcxo, timer);

    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, (ADDRESS << 1) | 1, timer) {
        return Err("Si549 failed to ack read address");
    }
    let val = i2c::read(dcxo, false, timer);
    i2c::stop(dcxo, timer);
    Ok(val)
}

fn setup(dcxo: i2c::DCXO, hsdiv: u16, lsdiv: u8, fbdiv: u64, timer: &mut GlobalTimer) -> Result<(), &'static str> {
    i2c::init(dcxo, timer)?;

    write(dcxo, 255, 0x00, timer)?; // PAGE
    write(dcxo, 69, 0x00, timer)?; // Disable FCAL override.
    write(dcxo, 17, 0x00, timer)?; // Synchronously disable output

    // The Si549 has no ID register, so we check that it responds correctly
    // by writing values to a RAM-like register and reading them back.
    for test_value in 0..255 {
        write(dcxo, 23, test_value, timer)?;
        let readback = read(dcxo, 23, timer)?;
        if readback != test_value {
            return Err("Si549 detection failed");
        }
    }

    write(dcxo, 23, hsdiv as u8, timer)?;
    write(dcxo, 24, (hsdiv >> 8) as u8 | (lsdiv << 4), timer)?;
    write(dcxo, 26, fbdiv as u8, timer)?;
    write(dcxo, 27, (fbdiv >> 8) as u8, timer)?;
    write(dcxo, 28, (fbdiv >> 16) as u8, timer)?;
    write(dcxo, 29, (fbdiv >> 24) as u8, timer)?;
    write(dcxo, 30, (fbdiv >> 32) as u8, timer)?;
    write(dcxo, 31, (fbdiv >> 40) as u8, timer)?;

    write(dcxo, 7, 0x08, timer)?; // Start FCAL
    timer.delay_us(30_000); // Internal FCAL VCO calibration
    write(dcxo, 17, 0x01, timer)?; // Synchronously enable output

    Ok(())
}

pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
    unsafe {
        csr::main_dcxo::bitbang_enable_write(1);
        csr::main_dcxo::i2c_address_write(ADDRESS);
    }

    #[cfg(rtio_frequency = "125.0")]
    let (m_hsdiv, m_lsdiv, m_fbdiv) = (0x058, 0, 0x04815791F25);

    setup(i2c::DCXO::Main, m_hsdiv, m_lsdiv, m_fbdiv, timer)?;

    // Si549 maximum settling time for large frequency change.
    timer.delay_us(40_000);

    unsafe {
        csr::main_dcxo::bitbang_enable_write(0);
    }

    info!("Main Si549 started");
    Ok(())
}

#[cfg(has_wrpll)]
pub mod wrpll {

    use libcortex_a9::mutex::Mutex;

    use super::*;

    const TIMER_WIDTH: u32 = 24;
    const COUNTER_DIV: u32 = 2;

    const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;

    const KP: i32 = 6;
    const KI: i32 = 2;

    static BASE_ADPLL: Mutex<i32> = Mutex::new(0);
    static H_INTEGRATOR: Mutex<i32> = Mutex::new(0);
    static M_INTEGRATOR: Mutex<i32> = Mutex::new(0);

    #[derive(Clone, Copy)]
    pub enum FIQ {
        GTXTag,
        MainTag,
    }

    mod tag_collector {
        use super::*;

        const BEATING_PERIOD: i32 = 0x8000;
        const BEATING_HALFPERIOD: i32 = 0x4000;

        // for main PLL
        static GTX_TAG: Mutex<u32> = Mutex::new(0);
        static GTX_TAG_READY: Mutex<bool> = Mutex::new(false);
        static MAIN_TAG: Mutex<u32> = Mutex::new(0);
        static MAIN_TAG_READY: Mutex<bool> = Mutex::new(false);

        pub fn reset() {
            clear_phase_diff_ready();
            *GTX_TAG.lock() = 0;
            *MAIN_TAG.lock() = 0;
        }

        pub fn clear_phase_diff_ready() {
            *GTX_TAG_READY.lock() = false;
            *MAIN_TAG_READY.lock() = false;
        }

        pub fn collect_tags(interrupt: FIQ) {
            match interrupt {
                FIQ::GTXTag => {
                    *GTX_TAG.lock() = unsafe { csr::wrpll::gtx_tag_read() };
                    *GTX_TAG_READY.lock() = true;
                }
                FIQ::MainTag => {
                    *MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
                    *MAIN_TAG_READY.lock() = true;
                }
            }
        }

        pub fn phase_diff_ready() -> bool {
            *GTX_TAG_READY.lock() && *MAIN_TAG_READY.lock()
        }

        pub fn get_period_error() -> i32 {
            // n * BEATING_PERIOD - GTX_TAG(n) mod BEATING_PERIOD
            let mut period_error = (*GTX_TAG.lock()).overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32;

            // mapping tags from [0, 2π] -> [-π, π]
            if period_error > BEATING_HALFPERIOD {
                period_error -= BEATING_PERIOD
            }
            period_error
        }

        pub fn get_phase_error() -> i32 {
            // MAIN_TAG(n) - GTX_TAG(n) mod BEATING_PERIOD
            let mut phase_error = (*MAIN_TAG.lock())
                .overflowing_sub(*GTX_TAG.lock())
                .0
                .rem_euclid(BEATING_PERIOD as u32) as i32;

            // mapping tags from [0, 2π] -> [-π, π]
            if phase_error > BEATING_HALFPERIOD {
                phase_error -= BEATING_PERIOD
            }
            phase_error
        }
    }

    pub fn helper_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
        unsafe {
            csr::wrpll::helper_reset_write(1);
            csr::helper_dcxo::bitbang_enable_write(1);
            csr::helper_dcxo::i2c_address_write(ADDRESS);
        }

        #[cfg(rtio_frequency = "125.0")]
        let (h_hsdiv, h_lsdiv, h_fbdiv) = (0x058, 0, 0x04814E8F442); // 125Mhz*32767/32768

        setup(i2c::DCXO::Helper, h_hsdiv, h_lsdiv, h_fbdiv, timer)?;

        // Si549 maximum settling time for large frequency change.
        timer.delay_us(40_000);

        unsafe {
            csr::wrpll::helper_reset_write(0);
            csr::helper_dcxo::bitbang_enable_write(0);
        }
        info!("Helper Si549 started");
        Ok(())
    }

    fn set_fiq(en: bool) {
        let val = if en { 1 } else { 0 };
        unsafe {
            csr::wrpll::gtx_tag_ev_enable_write(val);
            csr::wrpll::main_tag_ev_enable_write(val);
        }
    }

    /// set adpll using gateware i2c
    /// Note: disable main/helper i2c bitbang before using this function
    fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> {
        if adpll.abs() > ADPLL_MAX {
            return Err("adpll is too large");
        }

        match dcxo {
            i2c::DCXO::Main => unsafe {
                if csr::main_dcxo::bitbang_enable_read() == 1 {
                    return Err("Main si549 bitbang mode is active when using gateware i2c");
                }

                while csr::main_dcxo::adpll_busy_read() == 1 {}
                csr::main_dcxo::i2c_address_write(ADDRESS);
                csr::main_dcxo::adpll_write(adpll as u32);

                csr::main_dcxo::adpll_stb_write(1);
                csr::main_dcxo::adpll_stb_write(0);

                if csr::main_dcxo::nack_read() == 1 {
                    return Err("Main si549 failed to ack adpll write");
                }
            },
            #[cfg(has_wrpll)]
            i2c::DCXO::Helper => unsafe {
                if csr::helper_dcxo::bitbang_enable_read() == 1 {
                    return Err("Helper si549 bitbang mode is active when using gateware i2c");
                }

                while csr::helper_dcxo::adpll_busy_read() == 1 {}
                csr::helper_dcxo::i2c_address_write(ADDRESS);
                csr::helper_dcxo::adpll_write(adpll as u32);

                csr::helper_dcxo::adpll_stb_write(1);
                csr::helper_dcxo::adpll_stb_write(0);

                if csr::helper_dcxo::nack_read() == 1 {
                    return Err("Helper si549 failed to ack adpll write");
                }
            },
        };

        Ok(())
    }

    /// To get within capture range
    fn set_base_adpll(timer: &mut GlobalTimer) -> Result<(), &'static str> {
        let count2adpll =
            |error: i32| (((error) as f64 * 1e6) / (0.0001164 * (1 << (TIMER_WIDTH - COUNTER_DIV)) as f64)) as i32;

        let (gtx_count, main_count, _helper_count) = get_freq_counts(timer);
        let mut base_adpll_lock = BASE_ADPLL.lock();
        *base_adpll_lock = count2adpll(gtx_count as i32 - main_count as i32);
        set_adpll(i2c::DCXO::Main, *base_adpll_lock)?;
        set_adpll(i2c::DCXO::Helper, *base_adpll_lock)?;

        Ok(())
    }

    fn get_freq_counts(timer: &mut GlobalTimer) -> (u32, u32, u32) {
        unsafe {
            csr::wrpll::frequency_counter_update_en_write(1);
            timer.delay_us(150_000); // 8ns << TIMER_WIDTH
            csr::wrpll::frequency_counter_update_en_write(0);
            let gtx = csr::wrpll::frequency_counter_counter_gtx0_rtio_rx_read();
            let main = csr::wrpll::frequency_counter_counter_sys_read();
            let helper = csr::wrpll::frequency_counter_counter_helper_read();

            (gtx, main, helper)
        }
    }

    fn reset_plls() -> Result<(), &'static str> {
        *H_INTEGRATOR.lock() = 0;
        *M_INTEGRATOR.lock() = 0;
        set_adpll(i2c::DCXO::Main, 0)?;
        set_adpll(i2c::DCXO::Helper, 0)?;
        Ok(())
    }

    fn clear_pending(interrupt: FIQ) {
        match interrupt {
            FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) },
            FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
        };
    }

    fn is_pending(interrupt: FIQ) -> bool {
        match interrupt {
            FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_read() == 1 },
            FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 },
        }
    }

    pub fn interrupt_handler() {
        if is_pending(FIQ::GTXTag) {
            tag_collector::collect_tags(FIQ::GTXTag);
            clear_pending(FIQ::GTXTag);

            helper_pll().expect("failed to run helper DCXO PLL");
        }

        if is_pending(FIQ::MainTag) {
            tag_collector::collect_tags(FIQ::MainTag);
            clear_pending(FIQ::MainTag);
        }

        if tag_collector::phase_diff_ready() {
            main_pll().expect("failed to run main DCXO PLL");
            tag_collector::clear_phase_diff_ready();
        }
    }

    fn helper_pll() -> Result<(), &'static str> {
        let period_err = tag_collector::get_period_error();
        let mut integrator_lock = H_INTEGRATOR.lock();

        *integrator_lock += period_err * KI;
        let mut h_adpll = *BASE_ADPLL.lock() + period_err * KP + *integrator_lock;

        h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
        set_adpll(i2c::DCXO::Helper, h_adpll)?;

        Ok(())
    }

    fn main_pll() -> Result<(), &'static str> {
        let phase_err = tag_collector::get_phase_error();
        let mut integrator_lock = M_INTEGRATOR.lock();

        *integrator_lock += phase_err * KI;
        let mut m_adpll = *BASE_ADPLL.lock() + phase_err * KP + *integrator_lock;

        m_adpll = m_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
        set_adpll(i2c::DCXO::Main, m_adpll)?;

        Ok(())
    }

    pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) {
        set_fiq(false);

        if rc {
            tag_collector::reset();
            reset_plls().expect("failed to reset main and helper PLL");

            info!("warming up GTX CDR...");
            // gtx need a couple seconds for freq counter to read it properly
            timer.delay_us(20_000_000);
            set_base_adpll(timer).expect("failed to set base adpll");

            // clear gateware pending flag
            clear_pending(FIQ::GTXTag);
            clear_pending(FIQ::MainTag);

            // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL
            set_fiq(true);
            info!("WRPLL interrupt enabled");
        }
    }
}