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

//! **NOTE:** To debug this firmware you need your STM32 board connected
//! to an ST-LINK debugger and [probe-run](1).  A `.cargo/config` file is
//! included and configured with `runner = "probe-run --chip STM32F401CC"`.
//!
//! This Riskey keyboard firmware is based on Keyberon.  It reads the state
//! of all channels on multiple 16-channel analog multiplexers (up to 10
//! for a total of 160 analog channels).
//!
//! You'll also need to change the pins to whatever pins you're using with
//! your own board.  The default configuration uses:
//!
//! * `PB0`: Connected to `Z` (analog output on the multilpexer)
//! * `PB5`: Connected to `EN` (enable pin)
//! * `PB12, PB13, PB14, PB15`: Connected to `S0, S1, S2, S3` (channel select pins)
//!
//! **NOTE::** You can just run `EN` to ground to keep your analog multiplexers always enabled.
//!
//! [1]: https://github.com/knurling-rs/probe-run
//!

// NOTE: This is how you perform a software reset:
//       cortex_m::peripheral::SCB::sys_reset();

extern crate analog_multiplexer;
extern crate panic_halt;

use core::sync;

// Generic embedded stuff
use cortex_m;
use cortex_m_rt::pre_init; // For putting the MCU in DFU mode
use embedded_hal::digital::v2::OutputPin;
use pwm::C1;
use rtic::app;
// use rtic::cyccnt::{Instant, U32Ext as _};
use rtt_target::{rprintln, rtt_init_print};
use stm32::SPI1;
use stm32f4xx_hal::pwm;
use stm32f4xx_hal::adc::config::Clock;
use stm32f4xx_hal::adc::{config::AdcConfig, config::SampleTime, Adc};
use stm32f4xx_hal::gpio::gpioa::{PA0, PA1, PA15, PA2, PA3, PA4, PA5, PA6, PA7, PA8};
use stm32f4xx_hal::gpio::gpiob::{PB10, PB12, PB13, PB14, PB15, PB3, PB4, PB5};
use stm32f4xx_hal::gpio::gpioc::PC13;
use stm32f4xx_hal::gpio::{self, Alternate, Analog, Output, PushPull, AF5};
use stm32f4xx_hal::otg_fs::{UsbBus, UsbBusType, USB};
use stm32f4xx_hal::prelude::*;
use stm32f4xx_hal::spi::{NoMiso, NoSck, Spi};
use stm32f4xx_hal::stm32::ADC1;
use stm32f4xx_hal::{stm32, timer};
// use stm32::TIM11;
// use stm32f4xx_hal::interrupt::TIM4;
use usb_device::bus::UsbBusAllocator;
use usb_device::class::UsbClass;
use usb_device::device::UsbDevice;
use usb_device::prelude::*;
use usbd_serial::SerialPort;

// Keyboard-specific stuff
use analog_multiplexer::{DummyPin, Multiplexer};
use keyberon::key_code::KbHidReport;
use keyberon::key_code::KeyCode;
use keyberon::layout::{Event, Layout};
mod layers;
mod multiplexers;
// mod buzzer;

// WS2812/smart-leds stuff
use crate::ws2812::Ws2812;
use smart_leds::{brightness, colors, SmartLedsWrite, RGB8};
use ws2812_spi as ws2812;
// use ws2812_timer_delay as ws2812;

type UsbKeyClass = keyberon::Class<'static, UsbBusType, Leds>;
type UsbKeyDevice = UsbDevice<'static, UsbBusType>;
type UsbSerialDevice = SerialPort<'static, UsbBus<USB>>;

// Configurable options
// TODO: Make these configurable on-the-fly via the serial port:
const ACTUATION_THRESHOLD: u16 = 100; // mV change where a key actuates
const RELEASE_THRESHOLD: u16 = 75; // Lower than actuation so we don't bounce
const LED_BRIGHTNESS: u8 = 64; // 0-255
const NUM_LEDS: usize = 150 + 1; // 70 on the keeb but 50 external! (not sure why I need +1 but I do)
/* Brightness notes:
 * TODO: Make the brightness adjustable on-the-fly
 * These are Rainbow values:
 * At 128 the keeb uses ~916mAu
 * At 110 it's ~854mA
 * At 100 it's ~814mA
 * At 64 it's ~670mA
 * At 32 it's ~560-580mA
 * At 16 it's ~527mA
 * At 8 it's ~516mA
 * At 0 it's 507-541mA (typically ~530mA)
 * WHITE values:
 * 16: ~623mA
 * 32: ~780mA
 * 64: ~1170mA (that escalated fast!)
 * With both USB plugs powered: ~1440mA from the PC USB!
 * Black Pill without anything else: 32mA
 * Riskeypad: 167mA
 * Looks like each 49e hall effect sensor uses about ~5.87mA
*/
const NUM_MULTIPLEXERS: usize = 5; // Total number of analog multiplexers

const IGNORE_BELOW: u16 = 1200; // IMPORTANT: Unused analog pins need to be connected to GND!
                                // Pins on an analog multiplexer that aren't connected to anything
                                // (aka "floating pins") can cause all sorts of trouble.

const DEBUG_REFRESH_INTERVAL: u32 = 150;
// NOTE: The hall effect sensor value tends to hover around 2400 when a key
// is at rest and drops down to about 1000-1800 when a key is fully depressed
// (depending on the strength of the magnet and how close it gets).

// Add a custom device descriptor/message
// Generic keyboard from
// https://github.com/obdev/v-usb/blob/master/usbdrv/USB-IDs-for-free.txt
const VID: u16 = 0x16c0;
const PID: u16 = 0x27db;

// Handy type aliases to avoid a lot of long lines/typing later...
type AnalogPins = (
    PA0<Analog>,
    PA1<Analog>,
    PA2<Analog>,
    PA3<Analog>,
    PA4<Analog>,
    PA5<Analog>,
    PA6<Analog>,
    PA7<Analog>,
);
// Define which pins go to where on your analog multiplexer's select lines
type S0 = PB12<Output<PushPull>>; // These just make things easier to read/reason about
type S1 = PB13<Output<PushPull>>; // aka "very expressive"
type S2 = PB14<Output<PushPull>>;
type S3 = PB15<Output<PushPull>>;
type EN = DummyPin; // NOTE: We assume the enable pin goes to GND at all times

// Buzzer pin
type BUZZER = PA8<Output<PushPull>>;
// RGB LED Data pin
// type RGBLED_PIN = PC13<Output<PushPull>>;
type RGBLED_PIN = PB5<Output<PushPull>>;

// TODO: Add a proper DummyPin struct somewhere we can use with EN
// NOTE: embedded_hal really needs a DummyPin feature for things like unused driver pins!
type SelectPins = (S0, S1, S2, S3, EN);
type Multiplex = Multiplexer<SelectPins>;

// This is a numpad with the Black Pill at the top so we'll use the built-in LED for Numlock:
pub struct Leds {
    num_lock: PA15<gpio::Output<gpio::PushPull>>, // NOTE: PA15 is just a temp unused pin!
}
impl keyberon::keyboard::Leds for Leds {
    fn num_lock(&mut self, status: bool) {
        if status {
            self.num_lock.set_low().unwrap()
        } else {
            self.num_lock.set_high().unwrap()
        }
    }
}

#[app(device = stm32f4xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
const APP: () = {
    struct Resources {
        usb_key_dev: UsbKeyDevice,
        usb_key_class: UsbKeyClass,
        serial: UsbSerialDevice,
        multiplexer: Multiplex,
        timer: timer::Timer<stm32::TIM3>,
        led_timer: timer::Timer<stm32::TIM2>,
        adc: Adc<ADC1>,
        clocks: stm32f4xx_hal::rcc::Clocks,
        analog_pins: AnalogPins,
        layout: Layout<()>,
        // Keep track of the *initial* (resting) value of each sensor
        channel_states: [multiplexers::ChannelStates; 8],
        debug_msg_counter: u32,
        // ws: Ws2812<stm32f4xx_hal::timer::Timer<stm32f4xx_hal::stm32::TIM2>, RGBLED_PIN>,
        ws: Ws2812<
            Spi<
                SPI1,
                (
                    NoSck,
                    NoMiso,
                    PB5<Alternate<AF5>>,
                ),
            >,
        >,
        led_data: [RGB8; NUM_LEDS],
        led_state: u16,
        // buzzer: pwm::PwmChannels<stm32::TIM1, C1>,
    }

    // This idle loop fixes a bug that prevents flashing without the reset pin connected to the ST-LINK:
    #[idle]
    fn idle(_: idle::Context) -> ! {
        loop {
            sync::atomic::spin_loop_hint();
        }
    }

    // #[init(schedule = [do_leds])]
    #[init]
    fn init(mut c: init::Context) -> init::LateResources {
        // Static stuff
        static mut EP_MEMORY: [u32; 1024] = [0; 1024];
        static mut USB_BUS: Option<UsbBusAllocator<UsbBusType>> = None;

        // Initialize (enable) the monotonic timer (CYCCNT)
        // c.core.DCB.enable_trace();
        c.core.DWT.enable_cycle_counter();

        // Boilerplate for setting up the board
        let rcc = c.device.RCC.constrain();
        let clocks = rcc
            .cfgr
            .use_hse(25.mhz())
            .sysclk(84.mhz()) // Should be 84 (trying underclocking)
            .require_pll48clk()
            .freeze();

        // Setup a 1000Hz timer for RTIC/USB FS polling
        let mut timer = timer::Timer::tim3(c.device.TIM3, 1.khz(), clocks);
        timer.listen(timer::Event::TimeOut);

        // Setup another timer for the RGB LEDs
        let mut led_timer = timer::Timer::tim2(c.device.TIM2, 250.hz(), clocks);
        led_timer.listen(timer::Event::TimeOut);
        let led_state: u16 = 0;

        // So we can use GPIOs...
        let gpioa = c.device.GPIOA.split();
        let gpiob = c.device.GPIOB.split();

        // Power detection input
        let ext_power = gpiob.pb10.into_floating_input();

        // Buzzer
        // let mut buzzer_pin = gpioa.pa8.into_push_pull_output();
        // let buzzer_pin = gpioa.pa8.into_alternate_af1();
        // let _ = buzzer_pin.set_low(); // Have to start it low
        // let channels = (
        //     gpioa.pa8.into_alternate_af1(),
        //     gpioa.pa9.into_alternate_af1(),
        // );

        // let pwm = pwm::tim1(c.device.TIM1, channels, clocks, 150.hz());
        // let (mut buzzer, _ch2) = pwm;
        // // let max_duty = buzzer.get_max_duty();
        // let duty = 1_000_000.0 / 20.0;
        // buzzer.set_duty(duty as u16 / 50);
        // // buzzer.enable();
        // buzzer.disable(); // Turn it off by default
        // let buzzer = buzzer::Buzzer::new(buzzer_pin)
        // let buzzer_timer = timer::Timer::tim1(c.device.TIM1, 1000.khz(), clocks);

        // Setup WS2812B-B SPI output
        // let sck = gpiob.pb3.into_alternate_af5();
        // let miso = gpiob.pb4.into_alternate_af5();
        let mosi = gpiob.pb5.into_alternate_af5();
        // Configure SPI with 3Mhz rate
        let spi = Spi::spi1(
            c.device.SPI1,
            (NoSck,NoMiso,mosi),
            ws2812::MODE,
            3_000_000.hz(),
            clocks,
        );
        let mut ws = Ws2812::new(spi);
        // let mut ws = Ws2812::new(led_timer, ws_data_pin);
        let led_data = [RGB8::default(); NUM_LEDS]; // Off by default
        // let mut led_data = [colors::WHITE; NUM_LEDS]; // Make em all white by default
        // for i in 0..NUM_LEDS { // Make em rainbow colored
        //     // led_data[i] = wheel((((i * 256) as u16 / NUM_LEDS as u16 + j as u16) & 255) as u8);
        //     led_data[i] = wheel((((i * 256) as u16 / NUM_LEDS as u16) & 255) as u8);
        // }
        // Write to the LEDs one time (for now)
        ws.write(brightness(led_data.iter().cloned(), LED_BRIGHTNESS))
            .unwrap();
        // delay.delay_ms(5u8);
        // ws.write(brightness(led_data.iter().cloned(), LED_BRIGHTNESS)).unwrap();
        // TODO: Change this to indicate caps lock in the right place:
        // Setup our on-board LED to indicate Numlock
        // NOTE: Disabled the Keyberon LED control feature until I figure out how to get it working with ws2812
        let mut led = gpioa.pa15.into_push_pull_output();
        led.set_low().unwrap();
        let leds = Leds { num_lock: led };

        // Analog setup...
        // Pclk2_div_2 gives us an ADC running at 42Mhz (because sysclk is at 84Mhz)
        let adc_config = AdcConfig::default().clock(Clock::Pclk2_div_2);
        // NOTE: With a 42Mhz ADC clock and a SampleTime of Cycles_480 we'd be able
        // to read all 16 channels on all six analog multiplexers (96 pins) in about
        // 1.25ms which is just *barely* too slow for a 1000Hz USB polling rate.
        // For this reason we take the next cycle down at Cycles_144 which results in
        // a read-all-channels time of about 0.35657ms. So that's the best accuracy
        // we can get without going over our 1ms budget.
        let mut adc = Adc::adc1(c.device.ADC1, true, adc_config);
        let pa0 = gpioa.pa0.into_analog();
        let pa1 = gpioa.pa1.into_analog();
        let pa2 = gpioa.pa2.into_analog();
        let pa3 = gpioa.pa3.into_analog();
        let pa4 = gpioa.pa4.into_analog();
        let pa5 = gpioa.pa5.into_analog();
        let pa6 = gpioa.pa6.into_analog();
        let pa7 = gpioa.pa7.into_analog();
        let analog_pins = (pa0, pa1, pa2, pa3, pa4, pa5, pa6, pa7);
        // Setup PB12-PB15 for accessing S0-S3 on the 74HC4067 multiplexers
        let s0 = gpiob.pb12.into_push_pull_output();
        let s1 = gpiob.pb13.into_push_pull_output();
        let s2 = gpiob.pb14.into_push_pull_output();
        let s3 = gpiob.pb15.into_push_pull_output();
        let en = DummyPin; // Just run it to GND to keep always-enabled
        let select_pins = (s0, s1, s2, s3, en);

        // USB Stuff
        let usb = USB {
            // FULL SPEEEEEEEEEEEED! (aka FS)
            usb_global: c.device.OTG_FS_GLOBAL,
            usb_device: c.device.OTG_FS_DEVICE,
            usb_pwrclk: c.device.OTG_FS_PWRCLK,
            pin_dm: gpioa.pa11.into_alternate_af10(),
            pin_dp: gpioa.pa12.into_alternate_af10(),
        };
        *USB_BUS = Some(UsbBusType::new(usb, EP_MEMORY));
        let usb_bus = USB_BUS.as_ref().unwrap();

        // Setup a USB serial port for controlling the keyboard from a PC
        let serial = SerialPort::new(&usb_bus);
        // Setup the rest of the USB stuff
        let usb_key_class = keyberon::new_class(usb_bus, leds);
        let usb_key_dev = UsbDeviceBuilder::new(usb_bus, UsbVidPid(VID, PID))
            .manufacturer("Riskable")
            .product("Riskey Keyboard")
            // Include a capabilities string (future stuff)
            .serial_number(concat!(env!("CARGO_PKG_VERSION"), " 🖮v1.0:BEEF"))
            .max_power(500) // Pull out as much as we can (for now)!
            .build();

        // Only the choosiest choose these select cuts of selects
        let mut multiplexer = Multiplexer::new(select_pins);
        // Keep track of channel states/values
        let mut ch_states0: multiplexers::ChannelStates = Default::default();
        let mut ch_states1: multiplexers::ChannelStates = Default::default();
        let mut ch_states2: multiplexers::ChannelStates = Default::default();
        let mut ch_states3: multiplexers::ChannelStates = Default::default();
        let mut ch_states4: multiplexers::ChannelStates = Default::default();
        let mut ch_states5: multiplexers::ChannelStates = Default::default();
        let mut ch_states6: multiplexers::ChannelStates = Default::default();
        let mut ch_states7: multiplexers::ChannelStates = Default::default();
        // Read in the initial millivolt values for all analog channels so we have
        // a default/resting state to evaluate against.
        for chan in 0..16 {
            // This sets the channel on all multiplexers simultaneously
            // (since they're all connected to the same S0,S1,S2,S3 pins).
            multiplexer.set_channel(chan);
            // Read the analog value of each channel/key and store it in our ChannelStates struct...
            for multi in 0..NUM_MULTIPLEXERS {
                // This was the best I could do for iteration rather than repetition :(
                let sample = match multi {
                    0 => adc.convert(&analog_pins.0, SampleTime::Cycles_480),
                    1 => adc.convert(&analog_pins.1, SampleTime::Cycles_480),
                    2 => adc.convert(&analog_pins.2, SampleTime::Cycles_480),
                    3 => adc.convert(&analog_pins.3, SampleTime::Cycles_480),
                    4 => adc.convert(&analog_pins.4, SampleTime::Cycles_480),
                    5 => adc.convert(&analog_pins.5, SampleTime::Cycles_480),
                    6 => adc.convert(&analog_pins.5, SampleTime::Cycles_480),
                    7 => adc.convert(&analog_pins.5, SampleTime::Cycles_480),
                    _ => 0, // We only support eight multiplexers right now
                };
                let millivolts = adc.sample_to_millivolts(sample);
                match multi {
                    0 => {
                        ch_states0.update_default_by_index(chan as usize, millivolts);
                    }
                    1 => {
                        ch_states1.update_default_by_index(chan as usize, millivolts);
                    }
                    2 => {
                        ch_states2.update_default_by_index(chan as usize, millivolts);
                    }
                    3 => {
                        ch_states3.update_default_by_index(chan as usize, millivolts);
                    }
                    4 => {
                        ch_states4.update_default_by_index(chan as usize, millivolts);
                    }
                    5 => {
                        ch_states5.update_default_by_index(chan as usize, millivolts);
                    }
                    6 => {
                        ch_states6.update_default_by_index(chan as usize, millivolts);
                    }
                    7 => {
                        ch_states7.update_default_by_index(chan as usize, millivolts);
                    }
                    _ => {} // We only support six multiplexers right now
                };
            }
        }
        let channel_states = [
            ch_states0, ch_states1, ch_states2, ch_states3, ch_states4, ch_states5, ch_states6,
            ch_states7,
        ];
        let debug_msg_counter = 0;

        // Setup some rtt-target debugging:
        rtt_init_print!(); // NOTE: DO NOT MOVE THIS HIGHER
        rprintln!("init()");
        // Check the state of the external power connector
        match ext_power.is_high() {
            Ok(result) => rprintln!("ext_power.is_high(): {}", result),
            _ => rprintln!("Something else"),
        }
        init::LateResources {
            usb_key_dev,
            usb_key_class,
            serial,
            multiplexer,
            timer,
            led_timer,
            adc,
            clocks,
            analog_pins,
            layout: Layout::new(layers::LAYERS),
            channel_states,
            debug_msg_counter,
            ws,
            led_data,
            led_state,
            // buzzer,
        }
    }

    #[task(binds = OTG_FS, priority = 3, resources = [usb_key_dev, usb_key_class, serial])]
    fn usb_tx(mut c: usb_tx::Context) {
        usb_poll(
            &mut c.resources.usb_key_dev,
            &mut c.resources.usb_key_class,
            &mut c.resources.serial,
        );
    }

    #[task(binds = OTG_FS_WKUP, priority = 3, resources = [usb_key_dev, usb_key_class, serial])]
    fn usb_rx(mut c: usb_rx::Context) {
        usb_poll(
            &mut c.resources.usb_key_dev,
            &mut c.resources.usb_key_class,
            &mut c.resources.serial,
        );
    }

    // #[task(priority = 1, resources = [ws, led_data])]
    #[task(binds = TIM2, priority = 1, resources = [led_timer, ws, led_data, led_state])]
    // #[task(priority = 1, resources = [ws, led_data], schedule = [do_leds])]
    fn do_leds(c: do_leds::Context) {
        // rprintln!("do_leds()");
        c.resources.led_timer.clear_interrupt(timer::Event::TimeOut);
        let ws = c.resources.ws;
        let led_data = c.resources.led_data;
        let led_state = c.resources.led_state;
        // Do the rainbow run thing (for now)
        for i in 0..NUM_LEDS {
            led_data[i] = wheel(((i * 256) as u16 / NUM_LEDS as u16 + *led_state as u16 & 255) as u8);
        }
        // Increment the LED state value
        *led_state = led_state.saturating_add(1);
        if *led_state as u16 > 65534 {
            *led_state = 0;
        }
        // let test = c.resources.led_state;
        cortex_m::interrupt::disable();
        // rprintln!("do_leds() write()");
        match ws.write(brightness(led_data.iter().cloned(), LED_BRIGHTNESS)) {
            Ok(_) => {},
            // Ok(result) => rprintln!("do_leds() result: {:?}", result),
            Err(err) => rprintln!("Err writing to LEDs: {:?}", err)
        }
        // Arg: Why is this necessary?
        unsafe {
            cortex_m::interrupt::enable();
        }
    }

    // TODO: Make the tick() function much more simple like keyberon-f4:
    // #[task(binds = TIM3, priority = 1, resources = [usb_class, matrix, debouncer, layout, timer])]
    // fn tick(mut c: tick::Context) {
    //     c.resources.timer.clear_interrupt(timer::Event::TimeOut);
    //     for event in c
    //         .resources
    //         .debouncer
    //         .events(c.resources.matrix.get().unwrap())
    //     {
    //         c.resources.layout.event(event);
    //     }
    //     match c.resources.layout.tick() {
    //         keyberon::layout::CustomEvent::Release(()) => cortex_m::peripheral::SCB::sys_reset(),
    //         _ => (),
    //     }
    //     send_report(c.resources.layout.keycodes(), &mut c.resources.usb_class);
    // }

    #[task(binds = TIM3, priority = 2, resources = [
        debug_msg_counter, usb_key_class, layout, multiplexer,
        timer, adc, clocks, analog_pins, channel_states, serial])]
    fn tick(mut c: tick::Context) {
        c.resources.timer.clear_interrupt(timer::Event::TimeOut);
        // Shortcuts:
        let adc = c.resources.adc;
        let multiplexer = c.resources.multiplexer;
        let analog_pins = c.resources.analog_pins;
        let layout = c.resources.layout;
        let ch_states = c.resources.channel_states;
        let debug_msg_counter = c.resources.debug_msg_counter;
        // let buzzer = c.resources.buzzer;
        // let buzzer_timer = c.resources.buzzer_timer;
        // FYI: I tested with all the different SampleTime options and they all provided consistent
        // results.  I chose Cycles_84 because it seemed like a safe middle ground.
        let sample_time = SampleTime::Cycles_84;

        // Loop over one channel at a time while reading said channel from all multiplexers at once
        for chan in 0..16 {
            // This sets the channel on all multiplexers simultaneously
            // (since they're all connected to the same S0,S1,S2,S3 pins).
            multiplexer.set_channel(chan);
            // Read the analog value of each channel/key and trigger a Keyberon event
            // if it's over the ACTUATION_THRESHOLD
            for multi in 0..NUM_MULTIPLEXERS + 1 {
                // This was the best I could do for iteration rather than repetition :(
                let sample = match multi {
                    0 => adc.convert(&analog_pins.0, sample_time),
                    1 => adc.convert(&analog_pins.1, sample_time),
                    2 => adc.convert(&analog_pins.2, sample_time),
                    3 => adc.convert(&analog_pins.3, sample_time),
                    4 => adc.convert(&analog_pins.4, sample_time),
                    5 => adc.convert(&analog_pins.5, sample_time),
                    6 => adc.convert(&analog_pins.6, sample_time),
                    7 => adc.convert(&analog_pins.7, sample_time),
                    _ => 0, // We only support eight multiplexers right now
                };
                let millivolts = adc.sample_to_millivolts(sample);
                match multi {
                    0 => {
                        ch_states[0].update_value_by_index(chan as usize, millivolts);
                    }
                    1 => {
                        ch_states[1].update_value_by_index(chan as usize, millivolts);
                    }
                    2 => {
                        ch_states[2].update_value_by_index(chan as usize, millivolts);
                    }
                    3 => {
                        ch_states[3].update_value_by_index(chan as usize, millivolts);
                    }
                    4 => {
                        ch_states[4].update_value_by_index(chan as usize, millivolts);
                    }
                    5 => {
                        ch_states[5].update_value_by_index(chan as usize, millivolts);
                    }
                    6 => {
                        ch_states[6].update_value_by_index(chan as usize, millivolts);
                    }
                    7 => {
                        ch_states[7].update_value_by_index(chan as usize, millivolts);
                    }
                    _ => {} // We only support eight multiplexers right now
                            // I'm sure you can figure out how to add more if you want =)
                };
                let _ = check_keypress(multi as usize, chan as usize, millivolts, ch_states, layout);
            }
        }
        // Do the buzzer thing
        // let mut keydown = false;
        // for multi in 0..NUM_MULTIPLEXERS + 1 {
        //     if ch_states[multi].keydown() {
        //         keydown = true;
        //     }
        // }
        // if keydown {
        //     // rprintln!("Playing buzzer!");
        //     buzzer.disable();
        //     buzzer.enable();
        // } else {
        //     // rprintln!("Disabling buzzer");
        //     buzzer.disable();
        // }
        // Only post these debug messages as fast as DEBUG_REFRESH_INTERVAL because if we don't
        // they'll come in every 1ms and can you say, "insane screen flicker"? Cuz I can!
        if *debug_msg_counter > DEBUG_REFRESH_INTERVAL {
            // rprintln!("ChannelValues size: {:?}", core::mem::size_of::<multiplexers::ChannelValues>());
            rprintln!("\x1B[2J\x1B[0H"); // Clear the screen and move the cursor to 0,0
            rprintln!("{}", ch_states[0]); // Debug AM0
            rprintln!("{}", ch_states[1]); // Debug AM1
            rprintln!("{}", ch_states[2]); // Debug AM2
            rprintln!("{}", ch_states[3]); // Debug AM3
            rprintln!("{}", ch_states[4]); // Debug AM4
            // serial.lock(|s| match s.write(&[1,2,3]) {
            //     Ok(len) if len > 0 => {
            //         {}
            //     },
            //     _ => {}
            // });
            // match serial.write(&ch_states[0]) {
            //     Ok(len) if len > 0 => {
            //         write_offset += len;
            //     }
            //     _ => {}
            // }
            *debug_msg_counter = 0;
        } else {
            *debug_msg_counter = debug_msg_counter.saturating_add(1);
        }
        // if layout.states.len() > 0 {
        //     rprintln!("main.rs layout: {}", layout);
        // }
        // if layout.sequenced.len() > 0 {
        //     rprintln!("main.rs sequenced: {:?}", layout.sequenced);
        // }
        // let keycodes = layout.tick();
        // for k in keycodes.into_iter() {
        //     rprintln!("keycode: {:?}", k);
        // }
        // send_report(layout.tick(), &mut c.resources.usb_key_class);
        match layout.tick() {
            keyberon::layout::CustomEvent::Release(()) => cortex_m::peripheral::SCB::sys_reset(),
            _ => (),
        }
        send_report(layout.keycodes(), &mut c.resources.usb_key_class);
    }

    extern "C" {
        // fn TIM2();
        // fn TIM4();
        fn EXTI0();
    // fn EXTI1();
    }
};

///! Updates the status of all the Keyberon stuffin *layout* and returns true if a NEW keypress was detected
fn check_keypress(
    multilpexer: usize,
    chan: usize,
    millivolts: u16,
    ch_states: &mut [multiplexers::ChannelStates],
    layout: &mut Layout<()>,
) -> bool {
    let ch_state = ch_states[multilpexer].by_index(chan);
    if ch_state.value > IGNORE_BELOW {
        let voltage_difference = if millivolts < ch_state.default {
            ch_state.default - millivolts
        } else {
            0
        };
        // rprintln!("voltage_difference: {:?}", voltage_difference);
        if voltage_difference > ACTUATION_THRESHOLD {
            if !ch_state.pressed {
                ch_states[multilpexer].press(chan);
                // rprintln!(
                //     "Triggering Press event for ADC pin: PA{}, channel {} ({})",
                //     multilpexer,
                //     chan,
                //     voltage_difference
                // );
                let _ = layout.event(Event::Press(multilpexer as u8, chan as u8));
                return true
            }
        } else if voltage_difference < RELEASE_THRESHOLD {
            if ch_state.pressed {
                ch_states[multilpexer].release(chan);
                // rprintln!(
                //     "Triggering Release event for ADC pin: PA{}, channel {} ({})",
                //     multilpexer,
                //     chan,
                //     voltage_difference
                // );
                let _ = layout.event(Event::Release(multilpexer as u8, chan as u8));
                return false
            }
        }
    }
    false
}

fn send_report(
    iter: impl Iterator<Item = KeyCode>,
    usb_key_class: &mut resources::usb_key_class<'_>,
) {
    use rtic::Mutex;
    let report: KbHidReport = iter.collect();
    if usb_key_class.lock(|k| k.device_mut().set_keyboard_report(report.clone())) {
        while let Ok(0) = usb_key_class.lock(|k| k.write(report.as_bytes())) {}
    }
}

fn usb_poll(usb_dev: &mut UsbKeyDevice, keyboard: &mut UsbKeyClass, serial: &mut UsbSerialDevice) {
    if usb_dev.poll(&mut [keyboard, serial]) {
        keyboard.poll();
        serial.poll();
        // This was taken from the USB serial port example code:
        let mut buf = [0u8; 64];

        match serial.read(&mut buf) {
            Ok(count) if count > 0 => {
                // Convert send char to upper case
                for c in buf[0..count].iter_mut() {
                    if 0x61 <= *c && *c <= 0x7a {
                        *c &= !0x20;
                        rprintln!("{}", c);
                    }
                }
                // Echo back
                let mut write_offset = 0;
                while write_offset < count {
                    match serial.write(&buf[write_offset..count]) {
                        Ok(len) if len > 0 => {
                            write_offset += len;
                        }
                        _ => {}
                    }
                }
            }
            _ => {}
        }
    }
}

///! Plays the given tone on the given buzzer pin
fn tone(tone: u16, mut buzzer: BUZZER) {
    
}

/// Input a value 0 to 255 to get a color value
/// The colours are a transition r - g - b - back to r.
fn wheel(mut wheel_pos: u8) -> RGB8 {
    wheel_pos = 255 - wheel_pos;
    if wheel_pos < 85 {
        return (255 - wheel_pos * 3, 0, wheel_pos * 3).into();
    }
    if wheel_pos < 170 {
        wheel_pos -= 85;
        return (0, wheel_pos * 3, 255 - wheel_pos * 3).into();
    }
    wheel_pos -= 170;
    (wheel_pos * 3, 255 - wheel_pos * 3, 0).into()
}

// TODO: Get this working...
// This lets us put the MCU in DFU bootloader mode so you can flash without an ST-LINK:
// NOTE: This is much more reliable when you enable `inline-asm` in cortex-m
// #[pre_init]
// unsafe fn before_main() {
//     extern "C" {
//         static mut _panic_dump_start: u8;
//     }

//     use cortex_m::register::msp;

//     let start_ptr = &mut _panic_dump_start as *mut u8;

//     // Panic-persist sets a flag to the start of the dump region
//     // when a panic occurs
//     if 0x0FACADE0 == core::ptr::read_unaligned(start_ptr.cast::<usize>()) {
//         // Clear the flag
//         start_ptr.cast::<usize>().write_unaligned(0x00000000);

//         // The DFU bootloader's reset vector and initial stack pointer
//         const SYSMEM_MSP: u32 = 0x1fff0000;
//         const SYSMEM_RESET: u32 = 0x1fff0004;

//         let dfu_msp = core::ptr::read(SYSMEM_MSP as *const u32);
//         let putter: *const fn() = SYSMEM_RESET as *const fn();

//         msp::write(dfu_msp);
//         (*putter)();
//     }
// }
/*
jamesmunns
what happens is:

* a panic happens
* panic-persist writes the panic message to uninit RAM, and sets a magic word flag
* reboot happens
* on pre-init, if that flag is set, immediately change the SP and jump to the bootloader address

For some reason, that didn't always work on the F405 (or at least it was much more reliable on the stm32f072), which might be because this would be better to do in a naked assembly function.
but yeah, adamgreig is right, there's an app note that gives you the values you need for the stack pointer and jump-to location
You might also consider resetting the vector table and stack pointer back to the "right" location if you don't jump to bootloader. I'm not sure how "clean" the bootloader leaves the operating environment
(but then definitely do that in ASM, not Rust)

ALTERNATIVE NOTE from: adamgreig (@adamgreig:matrix.org):

this sequence of "write MSP, jump to function" is common enough that it's now part of cortex-m, but not yet released, so one day it will be as easy as cortex_m::asm::bootload(0 as *const u32)

you'll still need to either write VTOR to the chip-specific address, or set a chip-specific RAM remap, but the operation "read SP and RV from vector table, write SP to MSP, jump to RV" will be done for you
(write VTOR on cortex-m3 and above, set a RAM remap on cortex-m0 which don't have VTOR)
*/
