#![no_main]
#![no_std]
use defmt_rtt as _; // global logger

use panic_probe as _;
use stm32f1xx_hal as _;

// same panicking *behavior* as `panic-probe` but doesn't print a panic message
// this prevents the panic message being printed *twice* when `defmt::panic` is invoked
#[defmt::panic_handler]
fn panic() -> ! {
    cortex_m::asm::udf()
}

use rtic::app;

mod filters;
mod si5153;

#[app(device = stm32f1xx_hal::pac, peripherals = true, dispatchers = [SPI3])]
mod app {

    use num::Complex;
    use stm32f1xx_hal::{
        adc,
        gpio::{
            self, gpioa, gpiob, gpioc, Alternate, Analog, Floating, Input, OpenDrain, Output,
            PushPull, CRH, CRL,
        },
        i2c,
        i2c::BlockingI2c,
        pac::{ADC1, I2C1, TIM2, TIM4},
        prelude::*,
        serial::{self, Config, Serial},
        stm32,
        timer::{self, Channel, CounterHz, Event, Tim3NoRemap, Tim4NoRemap},
    };

    use systick_monotonic::Systick;

    use arrayvec::ArrayString;
    use num_traits::float::Float;

    use crate::filters;
    use crate::si5153;

    type AppI2C1 = BlockingI2c<
        I2C1,
        (
            gpiob::PB6<Alternate<OpenDrain>>,
            gpiob::PB7<Alternate<OpenDrain>>,
        ),
    >;

    type AudioPwm = timer::PwmHz<
        TIM4,
        Tim4NoRemap,
        timer::Ch<2>,
        gpio::Pin<Alternate<gpio::PushPull>, CRH, 'B', 8>,
    >;

    #[monotonic(binds = SysTick, default = true)]
    type MonoTimer = Systick<1_000>;

    #[shared]
    struct Shared {}

    #[local]
    struct Local {
        pll: si5153::Si5153<AppI2C1>,
        i2c: AppI2C1,
        board_led: gpioc::PC13<Output<PushPull>>,
        rx_en: gpioa::PA7<Output<PushPull>>,
        adc1: adc::Adc<ADC1>,
        mic_in: gpio::Pin<Analog, CRL, 'A', 4>,
        i_in: gpio::Pin<Analog, CRL, 'A', 1>,
        q_in: gpio::Pin<Analog, CRL, 'A', 0>,
        phase: f32,
        i_offset: f32,
        q_offset: f32,
        audio_pwm: AudioPwm,
        timer: CounterHz<TIM2>,
        usb_filter: filters::FirFilter<63>,
    }

    #[init]
    fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) {
        let mut flash = cx.device.FLASH.constrain();
        let rcc = cx.device.RCC.constrain();

        // Freeze the configuration of all the clocks in the system and store the frozen frequencies in
        // `clocks`
        let clocks = rcc
            .cfgr
            .use_hse(8.MHz())
            .sysclk(72.MHz())
            .pclk1(36.MHz())
            .freeze(&mut flash.acr);

        defmt::info!("Clock Setup done");

        let mono = Systick::new(cx.core.SYST, clocks.sysclk().to_Hz());

        let mut afio = cx.device.AFIO.constrain();

        // Acquire the GPIOC peripheral
        let mut gpioa = cx.device.GPIOA.split();
        let mut gpiob = cx.device.GPIOB.split();
        let mut gpioc = cx.device.GPIOC.split();

        let board_led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);

        let scl = gpiob.pb6.into_alternate_open_drain(&mut gpiob.crl);
        let sda = gpiob.pb7.into_alternate_open_drain(&mut gpiob.crl);
        let mut i2c = i2c::BlockingI2c::i2c1(
            cx.device.I2C1,
            (scl, sda),
            &mut afio.mapr,
            i2c::Mode::Standard {
                frequency: 400.kHz(),
            },
            clocks,
            5,
            1,
            5,
            5,
        );

        let mut pll = si5153::Si5153::new(&i2c);
        pll.init(&mut i2c, 25000000, 800000000, 800000000);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS0, si5153::PLL::A);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS1, si5153::PLL::A);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS2, si5153::PLL::B);

        pll.set_ms_freq(&mut i2c, si5153::Multisynth::MS0, 8_000_000);
        pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS0, 0);
        pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS0);

        pll.set_ms_freq(&mut i2c, si5153::Multisynth::MS1, 8_000_000);
        pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS1, 100);
        pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS1);

        let adc1 = adc::Adc::adc1(cx.device.ADC1, clocks);
        let mic_in = gpioa.pa4.into_analog(&mut gpioa.crl);

        let i_in = gpioa.pa1.into_analog(&mut gpioa.crl);
        let q_in = gpioa.pa0.into_analog(&mut gpioa.crl);

        let audio_out = gpiob.pb8.into_alternate_push_pull(&mut gpiob.crh);
        let mut audio_pwm = cx.device.TIM4.pwm_hz::<Tim4NoRemap, _, _>(
            audio_out,
            &mut afio.mapr,
            192.kHz(),
            &clocks,
        );
        audio_pwm.enable(Channel::C3);
        audio_pwm.set_duty(Channel::C3, 0u16);

        let mut rx_en = gpioa.pa7.into_push_pull_output(&mut gpioa.crl);
        rx_en.set_high();

        let mut bias_pin = gpioa.pa6.into_alternate_push_pull(&mut gpioa.crl);
        let mut bias_pwm =
            cx.device
                .TIM3
                .pwm_hz::<Tim3NoRemap, _, _>(bias_pin, &mut afio.mapr, 64.kHz(), &clocks);

        let mut timer = timer::Timer2::new(cx.device.TIM2, &clocks).counter_hz();
        timer.start(6400.Hz()).unwrap();
        // Generate an interrupt when the timer expires
        timer.listen(Event::Update);

        (
            Shared {},
            Local {
                i2c,
                pll,
                board_led,
                rx_en,
                adc1,
                mic_in,
                i_in,
                q_in,
                phase: 0.0,
                i_offset: 2048.0,
                q_offset: 2048.0,
                audio_pwm,
                timer,
                usb_filter: filters::usb_firfilter(),
            },
            init::Monotonics(mono),
        )
    }

    #[task(binds=TIM2, local=[timer, pll, i2c, adc1, mic_in, i_in, q_in, audio_pwm, phase, i_offset, q_offset, board_led, usb_filter])]
    fn transmit(mut ctx: transmit::Context) {
        ctx.local.board_led.toggle();

        let mut adc = ctx.local.adc1;
        let mut i_in = ctx.local.i_in;
        let mut q_in = ctx.local.q_in;

        let i_raw: u16 = adc.read(&mut *q_in).unwrap();
        let q_raw: u16 = adc.read(&mut *i_in).unwrap();

        *ctx.local.i_offset = 0.95 * *ctx.local.i_offset + 0.05 * (i_raw as f32);
        *ctx.local.q_offset = 0.95 * *ctx.local.q_offset + 0.05 * (q_raw as f32);

        let i_sample = (i_raw as f32) - *ctx.local.i_offset;
        let q_sample = (q_raw as f32) - *ctx.local.q_offset;

        let sample = Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
        let filtered = ctx.local.usb_filter.compute(sample) * 2.0;

        let max_duty = if ctx.local.audio_pwm.get_max_duty() != 0 {
            ctx.local.audio_pwm.get_max_duty() as f32
        } else {
            2.0.powi(16)
        };

        let output = filtered.re * max_duty;
        ctx.local.audio_pwm.set_duty(Channel::C3, output as u16);

        ctx.local.timer.clear_interrupt(Event::Update);
    }
}