/*
    The MIT License (MIT)

    Copyright (c) 2020 Matthias P. Braendli

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#![no_main]
#![no_std]

const REF_CLOCK : u32 = 25_000_000;

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,
    timer::{Timer},
};

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) -> (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(())
}

fn set_vfo(freq: u32, siclock: &mut dyn Si5351)
{
    let (div, mult, num, denom) = clock_settings_with_pll_calculation(freq);

    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();
}


#[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 gpioa = dp.GPIOA.split(&mut rcc.apb2);
    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();

    let c1 = gpioa.pa6;
    let c2 = gpioa.pa7;

    let qei = Timer::tim3(dp.TIM3, &clocks, &mut rcc.apb1)
        .qei((c1, c2), &mut afio.mapr);

    // 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 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();

    set_vfo(23_000_000, &mut siclock);

    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();

    let mut last_encoder_count = qei.count();
    loop {
        led.toggle().unwrap();

        lcd.set_cursor_pos(40, &mut delay).unwrap();
        let mut string = arrayvec::ArrayString::<[_; 16]>::new();
        let encoder_count = qei.count();
        if encoder_count != last_encoder_count {
            let freq = 23_000_000 + encoder_count as u32;

            set_vfo(freq, &mut siclock);

            write!(string, "{} ", freq).unwrap();
            lcd.write_str(&string, &mut delay).unwrap();
        }
        last_encoder_count = encoder_count;

        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 { }
}