#![no_main]
#![no_std]

use core::convert::TryInto;

use cortex_m_rt::ExceptionFrame;
use cortex_m_semihosting::hio;
use panic_semihosting as _;

use stm32f1xx_hal::{
    prelude::*,
    pac,
    i2c::{BlockingI2c, Mode},
    delay::Delay,
};

use embedded_hal::digital::v2::{OutputPin, ToggleableOutputPin};
use hd44780_driver::{Cursor, CursorBlink, Display, DisplayMode, HD44780};
use si5351::{Si5351, Si5351Device};

use core::fmt::Write;

fn gcd(x: u32, y: u32) -> u32 {
    let mut x = x;
    let mut y = y;
    while y != 0 {
        let t = y;
        y = x % y;
        x = t;
    }
    x
}

fn clock_settings_for_pll(freq: u32, pll: u32) -> (u16, u32, u32) {
    let a = pll / freq;
    let b = pll - (a * freq);
    let gcd = gcd(b, freq);

    let b = b / gcd;
    let c = freq / gcd;
    (a.try_into().unwrap(), b, c)
}

fn clock_settings_with_pll_calculation(freq: u32, ref_clock: u32) -> (u16, u8, u32, u32) {
    let mut divider : u32 = 900_000_000 / freq; // Calculate the division ratio. 900,000,000 is the maximum internal

    if (divider % 2) == 1 {
        divider -= 1 // Ensure an even integer division ratio
    }

    let pll_freq = divider * freq;
    // mult is an integer that must be in the range 15..90
    let mult = pll_freq / ref_clock;
    let l = pll_freq % ref_clock;

    let denom = 1048575;
    let num = f64::from(l) * f64::from(denom) / f64::from(ref_clock);

    (divider.try_into().unwrap(), mult.try_into().unwrap(), num as u32, denom)
}

fn print(step: usize) -> Result<(), core::fmt::Error> {
    let mut stdout = match hio::hstdout() {
        Ok(fd) => fd,
        Err(()) => return Err(core::fmt::Error),
    };

    let language = "Rust";
    let ranking = 1;

    write!(stdout, "{}: {} on embedded is #{}!\n", step, language, ranking)?;

    Ok(())
}

#[cortex_m_rt::entry]
fn main() -> ! {
    let cp = cortex_m::Peripherals::take().unwrap();
    let dp = pac::Peripherals::take().unwrap();

    let mut flash = dp.FLASH.constrain();
    let mut rcc = dp.RCC.constrain();
    let mut afio = dp.AFIO.constrain(&mut rcc.apb2);
    let clocks = rcc.cfgr.freeze(&mut flash.acr);
    let mut delay = Delay::new(cp.SYST, clocks);

    let mut gpiob = dp.GPIOB.split(&mut rcc.apb2);

    // Configure PB14 as output. (LED)
    let mut led = gpiob.pb14.into_push_pull_output(&mut gpiob.crh);
    led.set_low().unwrap();

    // Configure I2C1 to be used for Si5351 and display
    let scl = gpiob.pb6.into_alternate_open_drain(&mut gpiob.crl);
    let sda = gpiob.pb7.into_alternate_open_drain(&mut gpiob.crl);

    let i2c = BlockingI2c::i2c1(
        dp.I2C1,
        (scl, sda),
        &mut afio.mapr,
        Mode::Standard {
            frequency: 100_000.hz(),
        },
        clocks,
        &mut rcc.apb1,
        /* start_timeout_us */ 1000,
        /* start_retries */ 10,
        /* addr_timeout_us */ 1000,
        /* data_timeout_us */ 1000,
    );

    let i2c_busmanager = shared_bus::CortexMBusManager::new(i2c);

    /*
    // Configure I2C2
    let scl2 = gpiob.pb10.into_alternate_open_drain(&mut gpiob.crh);
    let sda2 = gpiob.pb11.into_alternate_open_drain(&mut gpiob.crh);

    let i2c2 = BlockingI2c::i2c2(
        dp.I2C2,
        (scl2, sda2),
        Mode::Fast {
            frequency: 400_000.hz(),
            duty_cycle: DutyCycle::Ratio2to1,
        },
        clocks,
        &mut rcc.apb1,
        /* start_timeout_us */ 1000,
        /* start_retries */ 10,
        /* addr_timeout_us */ 1000,
        /* data_timeout_us */ 1000,
    );
    */

    const I2C_ADDRESS: u8 = 0b010_0000; // MCP23008, depending on solder bridges
    let mut lcd = match HD44780::new_i2c_mcp23008(i2c_busmanager.acquire(), I2C_ADDRESS, &mut delay) {
        Ok(lcd) => lcd,
        Err(_) => panic!("HD44780 init fail"),
    };

    lcd.reset(&mut delay).unwrap();
    lcd.clear(&mut delay).unwrap();
    lcd.set_display_mode(
        DisplayMode {
            display: Display::On,
            cursor_visibility: Cursor::Invisible,
            cursor_blink: CursorBlink::Off,
        },
        &mut delay).unwrap();
    lcd.set_cursor_pos(0, &mut delay).unwrap();
    lcd.write_str("Hello, world!", &mut delay).unwrap();

    let ref_clock = 25_000_000;
    let mut siclock = Si5351Device::new(i2c_busmanager.acquire(), false, ref_clock);
    siclock.init(si5351::CrystalLoad::_10).unwrap();

    // See freqplan.py for Si5351 frequency plan
    // CLK1 = 116MHz
    let pll_a_mult = 32;
    siclock.setup_pll_int(si5351::PLL::A, 32).unwrap();

    {
        let clk1 = 116_000_000;
        let (a, b, c) = clock_settings_for_pll(clk1, pll_a_mult * ref_clock);
        siclock.setup_multisynth(si5351::Multisynth::MS1, a, b, c, si5351::OutputDivider::Div1).unwrap();
        siclock.select_clock_pll(si5351::ClockOutput::Clk1, si5351::PLL::A);
        siclock.set_clock_enabled(si5351::ClockOutput::Clk1, true);
        siclock.flush_clock_control(si5351::ClockOutput::Clk1).unwrap();
    }

    {
        let clk2 = 4_195_210;
        let (a, b, c) = clock_settings_for_pll(clk2, pll_a_mult * ref_clock);
        siclock.setup_multisynth(si5351::Multisynth::MS2, a, b, c, si5351::OutputDivider::Div1).unwrap();
        siclock.select_clock_pll(si5351::ClockOutput::Clk2, si5351::PLL::A);
        siclock.set_clock_enabled(si5351::ClockOutput::Clk2, true);
        siclock.flush_clock_control(si5351::ClockOutput::Clk2).unwrap();
    }

    siclock.reset_pll(si5351::PLL::A).unwrap();

    {
        let clk0 = 23_000_000;

        let (div, mult, num, denom) = clock_settings_with_pll_calculation(clk0, ref_clock);

        siclock.setup_pll(si5351::PLL::B, mult, num, denom).unwrap();
        siclock.setup_multisynth_int(si5351::Multisynth::MS0, div, si5351::OutputDivider::Div1).unwrap();

        siclock.select_clock_pll(si5351::ClockOutput::Clk0, si5351::PLL::B);
        siclock.set_clock_enabled(si5351::ClockOutput::Clk0, true);
        siclock.flush_clock_control(si5351::ClockOutput::Clk0).unwrap();
    }

    siclock.reset_pll(si5351::PLL::B).unwrap();

    siclock.flush_output_enabled().unwrap();


    lcd.set_cursor_pos(0, &mut delay).unwrap();
    lcd.write_str("Clocks set.     ", &mut delay).unwrap();

    let mut step = 0;
    print(step).unwrap();

    loop {
        led.toggle().unwrap();
        delay.delay_ms(600u32);

        lcd.set_cursor_pos(40, &mut delay).unwrap();
        let mut string = arrayvec::ArrayString::<[_; 16]>::new();
        write!(string, "Step {}     ", step).unwrap();
        lcd.write_str(&string, &mut delay).unwrap();

        step += 1;
    }
}

#[cortex_m_rt::exception]
fn HardFault(ef: &ExceptionFrame) -> ! {
    let periph = unsafe { cortex_m::Peripherals::steal() };
    let hfsr = periph.SCB.hfsr.read();
    let cfsr = periph.SCB.cfsr.read();

    let mut stdout = match hio::hstdout() {
        Ok(fd) => fd,
        Err(()) => panic!("no stdout"),
    };

    let _ = write!(stdout, "Hardfault {:x} {:x} at {:x}\n", hfsr, cfsr, ef.pc);
    cortex_m::asm::bkpt();
    loop { }
}

#[cortex_m_rt::exception]
fn DefaultHandler(irqn: i16) {
    let mut stdout = match hio::hstdout() {
        Ok(fd) => fd,
        Err(()) => panic!("no stdout"),
    };

    let _ = write!(stdout, "Unhandled exception (IRQn = {})", irqn);
    cortex_m::asm::bkpt();
    loop { }
}



    /* code to discover i2c device address
    let mut stdout = hio::hstdout().unwrap();

    loop {
        for addr in 0..127usize {
            let bytes = [0u8; 1];
            let mut buffer = [0u8; 1];
            match i2c.write_read(addr as u8, &bytes, &mut buffer) {
                Ok(()) => {
                    write!(stdout, "{}: {}\n", addr, buffer[0]).unwrap();
                },
                Err(_) => {
                    write!(stdout, "{}: fail\n", addr).unwrap();
                },
            }
        }
    }
    */