#![no_std]
#![no_main]

mod model;

use panic_halt as _;

#[rtic::app(device = stm32f1xx_hal::pac)]
mod app {
    use crate::model::PriorityFrame;
    use bxcan::{filter::Mask32, Fifo, Frame, Interrupts, Rx0, StandardId, Tx};
    use heapless::binary_heap::{BinaryHeap, Max};
    use stm32f1xx_hal::{
        can::Can,
        device::TIM2,
        gpio::{Output, Pin},
        pac::{Interrupt, CAN1},
        prelude::*,
        timer::Delay,
    };

    #[shared]
    struct Shared {
        can_tx_queue: BinaryHeap<PriorityFrame, Max, 16>,
    }

    #[local]
    struct Local {
        can_tx: Tx<Can<CAN1>>,
        can_rx: Rx0<Can<CAN1>>,
        led: Pin<'C', 13, Output>,
        delay: Delay<TIM2, 1000000>,
    }

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

        let clocks = rcc
            .cfgr
            .use_hse(8.MHz())
            .sysclk(64.MHz())
            .hclk(64.MHz())
            .pclk1(16.MHz())
            .pclk2(64.MHz())
            .freeze(&mut flash.acr);

        let can = Can::new(ctx.device.CAN1, ctx.device.USB);
        let mut gpioa = ctx.device.GPIOA.split();
        let can_rx_pin = gpioa.pa11.into_floating_input(&mut gpioa.crh);
        let can_tx_pin = gpioa.pa12.into_alternate_push_pull(&mut gpioa.crh);
        let mut afio = ctx.device.AFIO.constrain();
        can.assign_pins((can_tx_pin, can_rx_pin), &mut afio.mapr);

        // APB1 (PCLK1): 16MHz, Bit rate: 1000kBit/s, Sample Point 87.5%
        // Value was calculated with http://www.bittiming.can-wiki.info/
        let mut can = bxcan::Can::builder(can)
            .set_bit_timing(0x001c_0000)
            .leave_disabled();

        can.modify_filters()
            .enable_bank(0, Fifo::Fifo0, Mask32::accept_all());

        can.enable_interrupts(
            Interrupts::TRANSMIT_MAILBOX_EMPTY | Interrupts::FIFO0_MESSAGE_PENDING,
        );

        nb::block!(can.enable_non_blocking()).unwrap();

        let (can_tx, can_rx, _) = can.split();
        let can_tx_queue = BinaryHeap::new();

        let delay = ctx.device.TIM2.delay_us(&clocks);
        let mut gpioc = ctx.device.GPIOC.split();
        let led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);

        (
            Shared { can_tx_queue },
            Local {
                can_tx,
                can_rx,
                led,
                delay,
            },
            init::Monotonics(),
        )
    }

    fn speed2array(speed: i16) -> [u8; 8] {
        let b = speed.to_be_bytes();
        [b[0], b[1], b[0], b[1], b[0], b[1], b[0], b[1]]
    }

    fn enqueue_frame(queue: &mut BinaryHeap<PriorityFrame, Max, 16>, frame: Frame) {
        queue.push(PriorityFrame(frame)).unwrap();
        rtic::pend(Interrupt::USB_HP_CAN_TX);
    }

    #[idle(shared = [can_tx_queue], local = [delay])]
    fn idle(cx: idle::Context) -> ! {
        let mut tx_queue = cx.shared.can_tx_queue;

        for sign in [1, -1].iter().cycle() {
            for speed in (0..=30_000)
                .step_by(250)
                .chain(core::iter::repeat(30_000).take(1000 / 25))
                .chain((0..=30_000).step_by(250).map(|v| 30_000 - v))
                .map(|v| v * sign)
            {
                tx_queue.lock(|mut tx_queue| {
                    enqueue_frame(
                        &mut tx_queue,
                        Frame::new_data(StandardId::new(0x1FF).unwrap(), speed2array(speed)),
                    );
                });

                cx.local.delay.delay(25.millis());
            }
        }

        unreachable!()
    }

    #[task(binds = USB_HP_CAN_TX, local = [can_tx, led], shared = [can_tx_queue])]
    fn can_tx(cx: can_tx::Context) {
        let tx = cx.local.can_tx;
        let mut tx_queue = cx.shared.can_tx_queue;

        tx.clear_interrupt_flags();

        (&mut tx_queue).lock(|tx_queue| {
            while let Some(frame) = tx_queue.peek() {
                match tx.transmit(&frame.0) {
                    Ok(status) => match status.dequeued_frame() {
                        None => {
                            tx_queue.pop();
                            cx.local.led.toggle();
                        }
                        Some(pending_frame) => {
                            tx_queue.pop();
                            enqueue_frame(tx_queue, pending_frame.clone());
                        }
                    },
                    Err(nb::Error::WouldBlock) => break,
                    Err(_) => unreachable!(),
                }
            }
        });
    }

    #[task(binds = USB_LP_CAN_RX0, local = [can_rx], shared = [can_tx_queue])]
    fn can_rx0(cx: can_rx0::Context) {
        loop {
            match cx.local.can_rx.receive() {
                Ok(_frame) => {
                    // ここがデータきたときのあれ。
                }
                Err(nb::Error::WouldBlock) => break,
                Err(nb::Error::Other(_)) => {}
            }
        }
    }
}