#![deny(unsafe_code)]
#![no_std]
#![no_main]

extern crate panic_semihosting;

use cortex_m_rt::entry;
use embedded_hal::digital::v2::OutputPin;
use rtt_target::{rprintln, rtt_init_print};
use stm32f1xx_hal::{delay::Delay, pac, pac::TIM1, pac::TIM2, prelude::*, rcc::Enable, rcc::Reset};

#[entry]
fn main() -> ! {
    rtt_init_print!();

    // Get access to the core peripherals from the cortex-m crate
    let cp = cortex_m::Peripherals::take().unwrap();
    // Get access to the device specific peripherals from the peripheral access crate
    let dp = pac::Peripherals::take().unwrap();

    // Take ownership over the raw flash and rcc devices and convert them into the corresponding
    // HAL structs
    let mut flash = dp.FLASH.constrain();
    let mut rcc = dp.RCC.constrain();

    let clocks = rcc
        .cfgr
        .use_hse(8.mhz())
        .sysclk(48.mhz())
        .pclk1(24.mhz())
        .freeze(&mut flash.acr);

    // Freeze the configuration of all the clocks in the system and store the frozen frequencies in
    // `clocks`
    //let clocks = rcc.cfgr.freeze(&mut flash.acr);

    // Acquire the GPIOC peripheral
    let mut gpioc = dp.GPIOC.split(&mut rcc.apb2);

    // Configure gpio C pin 13 as a push-pull output. The `crh` register is passed to the function
    // in order to configure the port. For pins 0-7, crl should be passed instead.
    let mut led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);

    // Setup timers
    let tim1 = dp.TIM1;

    TIM1::enable(&mut rcc.apb2);
    TIM1::reset(&mut rcc.apb2);

    // Enable external clocking
    tim1.smcr.write(|w| {
        w.etf().no_filter(); // No filter for to 10Mhz clock
        w.etps().div1(); // No divider
        w.etp().not_inverted(); // on rising edege at ETR pin
        w.ece().enabled() // mode 2 (use ETR pin)
    });

    tim1.ccmr1_input().write(|w| {
        w.cc1s().ti1();
        w.ic1f().no_filter()
        //w.ic1psc().bits(0)
    });

    tim1.ccer.write(|w| {
        w.cc1p().set_bit();
        w.cc1e().set_bit()
    });

    tim1.cr2.write(|w| {
        w.mms().update() // Trigger output on update/overflow
    });

    // Counting up to 10^7 should need 24 bits
    // Clock tim2 by tim1s overflow to make a 32bit timer

    let tim2 = dp.TIM2;

    TIM2::enable(&mut rcc.apb1);
    TIM2::reset(&mut rcc.apb1);

    tim2.smcr.write(|w| {
        w.ts().itr0(); // Trigger from internal trigger 0
        w.sms().ext_clock_mode() // Use trigger as clock
    });

    tim2.ccmr1_input().write(|w| {
        w.cc1s().ti1();
        w.ic1f().no_filter()
        //w.ic1psc().bits(0)
    });

    tim2.ccer.write(|w| {
        w.cc1p().set_bit();
        w.cc1e().set_bit()
    });

    tim1.cr1.write(|w| w.cen().enabled());
    tim2.cr1.write(|w| w.cen().enabled());

    let mut delay = Delay::new(cp.SYST, clocks);

    let mut last_ic = 0u32;
    let mut avg = 10f64;

    // Skip the first measurement, it will be garbage
    while !tim1.sr.read().cc1if().bit_is_set() || !tim2.sr.read().cc1if().bit_is_set() {
        delay.delay_ms(10u16);
    }
    let ic1 = tim1.ccr1.read().bits();
    let ic2 = tim2.ccr1.read().bits();

    last_ic = ic2 << 16 | ic1;

    loop {
        while !tim1.sr.read().cc1if().bit_is_set() || !tim2.sr.read().cc1if().bit_is_set() {
            delay.delay_ms(10u16);
        }

        let ic1 = tim1.ccr1.read().bits();
        let ic2 = tim2.ccr1.read().bits();

        let sum_ic = ic2 << 16 | ic1;

        let diff_ic = if sum_ic > last_ic {
            sum_ic - last_ic
        } else {
            u32::MAX - last_ic + sum_ic
        };

        last_ic = sum_ic;

        let freq = (diff_ic as f64) / 1_000_000f64;
        avg = avg * 0.99 + freq * 0.01;

        rprintln!("Counters:");
        rprintln!("ic1: {}", ic1);
        rprintln!("ic2: {}", ic2);
        rprintln!("sum_ic: {}", sum_ic);
        rprintln!("diff_ic: {}", diff_ic);
        rprintln!("freq: {} MHz", freq);
        rprintln!("avg: {} MHz", avg);
    }
}