//! Project: CMtec CMDR joystick 25
//! Date:    2023-08-01
//! Author:  Christoffer Martinsson
//! Email:   cm@cmtec.se
//! License: Please refer to LICENSE in root directory
//
// HW Button index map:
// ---------------------------------------------------------------
// |  0 L|  1 U| 25 U |        |  2  |        | 26 U |  4 U|  3 L|
// ---------------------------------------------------------------
// |     |  5  |  6  |  7  |             | 12  | 11  | 10  |     |
// |                                                             |
// |                 |  8  |             | 13  |                 |
// |                 |  9  |             | 14  |                 |
// |      X1/Y1                                       X2/Y2      |
// |                 | 16  |             | 21  |                 |
// |           | 19  | 15  | 17  | | 24  | 20  | 22  |           |
// |                 | 18  |             | 23  |                 |
// ---------------------------------------------------------------
//
// USB HID joystick map :
// ---------------------------------------------------------------
// | Fn L| B2 U| B26 U|        | B3  |        | B28 U| B5 U| B4 L|
// ---------------------------------------------------------------
// |     | TH | B6  | B8/16|            |B13/17| B12 | B11 |     |
// |                                                             |
// |                 | B9  |             | B14 |                 |
// |                 | B10 |             | B15 |                 |
// |      X1/Y1                                       X2/Y2      |
// |                 | H1U |             | H2U |                 |
// |           | H1L | B18 | H1R | | H2L | B19 | H2R |           |
// |                 | H1D |             | H2D |                 |
// ---------------------------------------------------------------
//
// USB HID joystick map (Fn):
// ---------------------------------------------------------------
// | Fn L| B21 U| B27 U|       | B3  |       | B28 U| B5 U| B22 L|
// ---------------------------------------------------------------
// |      | TH | B7 | B8/16|            |B13/17| B24 | B23 |     |
// |                                                             |
// |                 | B29 |             | B31 |                 |
// |                 | B30 |             | B32 |                 |
// |      X1/Y1                                       X2/Y2      |
// |                 | H3U |             | H4U |                 |
// |           | H3L | B20 | H3R | | H4L | B25 | H4R |           |
// |                 | H3D |             | H4D |                 |
// ---------------------------------------------------------------
//
// Config Layer (holding CONFIG button)
// ---------------------------------------------------------------
// |BOOT L| CAL U|           | CONFIG |              |  -  |  -  |
// ---------------------------------------------------------------
// |     |  -  |  -  |  -  |             |  -  |  -  |  -  |     |
// |                                                             |
// |                 |  -  |             |  -  |                 |
// |                 |  -  |             |  -  |                 |
// |        -/-                                        -/-       |
// |                 |  -  |             |  -  |                 |
// |           |  -  |  -  |  -  | |  -  |  -  |  -  |           |
// |                 |  -  |             |  -  |                 |
// ---------------------------------------------------------------

#![no_std]
#![no_main]

mod button_matrix;
mod status_led;
mod usb_joystick_device;

use button_matrix::ButtonMatrix;
use core::convert::Infallible;
use cortex_m::delay::Delay;
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
use eeprom24x::{Eeprom24x, SlaveAddr};
use embedded_hal::adc::OneShot;
use embedded_hal::digital::v2::*;
use embedded_hal::timer::CountDown;
use fugit::{ExtU32, RateExtU32};
use libm::powf;
use panic_halt as _;
use rp2040_hal::{
    adc::Adc,
    gpio::{Function, FunctionConfig, PinId, ValidPinMode},
    i2c::I2C,
    pio::StateMachineIndex,
};
use status_led::{StatusMode, Ws2812StatusLed};
use usb_device::class_prelude::*;
use usb_device::prelude::*;
use usb_joystick_device::{JoystickConfig, JoystickReport};
use usbd_human_interface_device::prelude::*;
use waveshare_rp2040_zero::entry;
use waveshare_rp2040_zero::{
    hal::{
        clocks::{init_clocks_and_plls, Clock},
        pac,
        pio::PIOExt,
        timer::Timer,
        watchdog::Watchdog,
        Sio,
    },
    Pins, XOSC_CRYSTAL_FREQ,
};

// Public constants
pub const BUTTON_ROWS: usize = 5;
pub const BUTTON_COLS: usize = 5;
pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS;

pub const AXIS_MIN: u16 = 0;
pub const AXIS_MAX: u16 = 4095;
pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2;

pub const NBR_OF_GIMBAL_AXIS: usize = 4;
pub const GIMBAL_AXIS_LEFT_X: usize = 0;
pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
pub const GIMBAL_AXIS_RIGHT_Y: usize = 3;
pub const GIMBAL_MODE_M10: u8 = 0;
pub const GIMBAL_MODE_M7: u8 = 1;

// Analog smoothing settings.
pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

pub const DEBOUNCE: u8 = 10;

pub const RELEASE_RIMEOUT: u16 = 30; // => 300ms

// Public types
#[derive(Copy, Clone, Default)]
pub struct Button {
    pub pressed: bool,
    pub previous_pressed: bool,
    pub usb_changed: bool,
    pub usb_changed_to_pressed: bool,
    pub usb_button: usize,
    pub usb_button_sec_enable: bool,
    pub usb_button_sec: usize,
    pub usb_button_sec_trigger_index: usize,
    pub usb_button_sec_pressed: bool,
    pub usb_button_toggle_enable: bool,
    pub usb_release_timeout: u16,
}

#[derive(Copy, Clone)]
pub struct GimbalAxis {
    pub value: u16,
    pub previous_value: u16,
    pub idle_value: u16,
    pub max: u16,
    pub min: u16,
    pub center: u16,
    pub deadzone: (u16, u16, u16),
    pub expo: bool,
    pub trim: i16,
    pub hold: u16,
    pub hold_pending: bool,
}

impl Default for GimbalAxis {
    fn default() -> Self {
        GimbalAxis {
            value: AXIS_CENTER,
            previous_value: AXIS_CENTER,
            idle_value: AXIS_CENTER,
            max: AXIS_MAX,
            min: AXIS_MIN,
            center: AXIS_CENTER,
            deadzone: (100, 50, 100),
            expo: true,
            trim: 0,
            hold: AXIS_CENTER,
            hold_pending: false,
        }
    }
}

#[entry]
fn main() -> ! {
    // Grab our singleton objects
    let mut pac = pac::Peripherals::take().unwrap();

    // Set up the watchdog driver - needed by the clock setup code
    let mut watchdog = Watchdog::new(pac.WATCHDOG);

    // Configure clocks and PLLs
    let clocks = init_clocks_and_plls(
        XOSC_CRYSTAL_FREQ,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    let core = pac::CorePeripherals::take().unwrap();

    // The single-cycle I/O block controls our GPIO pins
    let sio = Sio::new(pac.SIO);

    // Set the pins to their default state
    let pins = Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let i2c = I2C::i2c1(
        pac.I2C1,
        pins.gp14.into_mode(), // sda
        pins.gp15.into_mode(), // scl
        400.kHz(),
        &mut pac.RESETS,
        125_000_000.Hz(),
    );

    let i2c_address = SlaveAddr::Alternative(false, false, false);
    let mut eeprom = Eeprom24x::new_24x32(i2c, i2c_address);

    // Enable adc
    let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);

    // Configure ADC input pins
    // Have not figured out hov to store the adc pins in an array yet
    // TODO: Find a way to store adc pins in an array
    let mut adc_pin_left_x = pins.gp29.into_floating_input();
    let mut adc_pin_left_y = pins.gp28.into_floating_input();
    let mut adc_pin_right_x = pins.gp27.into_floating_input();
    let mut adc_pin_right_y = pins.gp26.into_floating_input();

    // Setting up array with pins connected to button rows
    let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[
        &pins.gp6.into_pull_up_input(),
        &pins.gp8.into_pull_up_input(),
        &pins.gp4.into_pull_up_input(),
        &pins.gp7.into_pull_up_input(),
        &pins.gp5.into_pull_up_input(),
    ];

    // Setting up array with pins connected to button columns
    let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [
        &mut pins.gp9.into_push_pull_output(),
        &mut pins.gp10.into_push_pull_output(),
        &mut pins.gp11.into_push_pull_output(),
        &mut pins.gp12.into_push_pull_output(),
        &mut pins.gp13.into_push_pull_output(),
    ];

    // Create button matrix object that scans all buttons
    let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
        ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE);

    // Initialize button matrix
    button_matrix.init_pins();

    // Setup extra buttons (connected to TX/RX pins)
    let left_extra_button = pins.gp1.into_pull_up_input();
    let right_extra_button = pins.gp0.into_pull_up_input();

    // Create status LED
    let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS);
    let mut status_led = Ws2812StatusLed::new(
        pins.neopixel.into_mode(),
        &mut pio,
        sm0,
        clocks.peripheral_clock.freq(),
    );

    let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());

    // Scan matrix to get initial state and check if bootloader should be entered
    // This is done by holding button 0 pressed while power on the unit
    for _ in 0..10 {
        button_matrix.scan_matrix(&mut delay);
    }
    if button_matrix.buttons_pressed()[0] {
        status_led.update(StatusMode::Bootloader);
        let gpio_activity_pin_mask: u32 = 0;
        let disable_interface_mask: u32 = 0;
        rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
    }

    // Create timers
    let timer = Timer::new(pac.TIMER, &mut pac.RESETS);

    let mut status_led_count_down = timer.count_down();
    status_led_count_down.start(250.millis());

    let mut scan_count_down = timer.count_down();
    scan_count_down.start(200u32.micros());

    let mut data_process_count_down = timer.count_down();
    data_process_count_down.start(1200u32.micros());

    let mut usb_update_count_down = timer.count_down();
    usb_update_count_down.start(10.millis());

    let mut usb_activity: bool = false;
    let mut usb_active: bool = false;
    let mut calibration_active: bool = false;
    let mut throttle_hold_enable: bool = false;

    let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
    let mut buttons: [Button; NUMBER_OF_BUTTONS + 2] = [Button::default(); NUMBER_OF_BUTTONS + 2];
    let mut gimbal_mode: u8;

    // Set up usb button layout
    buttons[0].usb_button = 0;
    buttons[1].usb_button = 2;
    buttons[1].usb_button_sec_enable = true;
    buttons[1].usb_button_sec = 21;
    buttons[1].usb_button_sec_trigger_index = 0;
    buttons[2].usb_button = 3;
    buttons[3].usb_button = 4;
    buttons[3].usb_button_sec_enable = true;
    buttons[3].usb_button_sec = 22;
    buttons[3].usb_button_sec_trigger_index = 0;
    buttons[4].usb_button = 5;
    buttons[5].usb_button = 0;
    buttons[6].usb_button = 7;
    buttons[7].usb_button = 8;
    buttons[7].usb_button_sec = 16;
    buttons[7].usb_button_toggle_enable = true;
    buttons[8].usb_button = 9;
    buttons[9].usb_button = 10;
    buttons[10].usb_button = 11;
    buttons[10].usb_button_sec_enable = true;
    buttons[10].usb_button_sec = 23;
    buttons[10].usb_button_sec_trigger_index = 0;
    buttons[11].usb_button = 12;
    buttons[11].usb_button_sec_enable = true;
    buttons[11].usb_button_sec = 24;
    buttons[11].usb_button_sec_trigger_index = 0;
    buttons[12].usb_button = 13;
    buttons[12].usb_button_sec = 17;
    buttons[12].usb_button_toggle_enable = true;
    buttons[13].usb_button = 14;
    buttons[14].usb_button = 15;
    buttons[15].usb_button = 18;
    buttons[15].usb_button_sec_enable = true;
    buttons[15].usb_button_sec = 20;
    buttons[15].usb_button_sec_trigger_index = 0;
    buttons[20].usb_button = 19;
    buttons[20].usb_button_sec_enable = true;
    buttons[20].usb_button_sec = 25;
    buttons[20].usb_button_sec_trigger_index = 0;
    buttons[25].usb_button = 26;
    buttons[25].usb_button_sec_enable = true;
    buttons[25].usb_button_sec = 27;
    buttons[25].usb_button_sec_trigger_index = 0;
    buttons[26].usb_button = 28;

    // Table for gimbal expo curve lookup insded of doing floating point math for every analog read
    let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3);

    // Create dynamic smoother array for gimbal axis
    let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
        DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
        DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
        DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
        DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
    ];

    // Configure USB
    let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new(
        pac.USBCTRL_REGS,
        pac.USBCTRL_DPRAM,
        clocks.usb_clock,
        true,
        &mut pac.RESETS,
    ));

    let mut usb_hid_joystick = UsbHidClassBuilder::new()
        .add_device(JoystickConfig::default())
        .build(&usb_bus);

    let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002))
        .manufacturer("CMtec")
        .product("CMDR Joystick 25")
        .serial_number("0001")
        .build();

    // Read calibration data from eeprom
    for (index, item) in axis.iter_mut().enumerate() {
        item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
        item.min <<= 8;
        item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
        item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
        item.max <<= 8;
        item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
        item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16;
        item.center <<= 8;
        item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
    }
    gimbal_mode = eeprom.read_byte(25).unwrap();

    loop {
        // Take care of USB HID poll requests
        if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
            usb_active = true;
        }

        if scan_count_down.wait().is_ok() {
            button_matrix.scan_matrix(&mut delay);

            let mut left_x: u16 = adc.read(&mut adc_pin_left_x).unwrap();
            let mut left_y: u16 = adc.read(&mut adc_pin_left_y).unwrap();
            let mut right_x: u16 = adc.read(&mut adc_pin_right_x).unwrap();
            let mut right_y: u16 = adc.read(&mut adc_pin_right_y).unwrap();

            if gimbal_mode == GIMBAL_MODE_M10 {
                // Invert X1 and Y2 axis (M10 gimbals)
                left_x = AXIS_MAX - left_x;
                right_y = AXIS_MAX - right_y;
            } else if gimbal_mode == GIMBAL_MODE_M7 {
                // Invert Y1 and X2 axis (M7 gimbals)
                left_y = AXIS_MAX - left_y;
                right_x = AXIS_MAX - right_x;
            }

            smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32);
            smoother[GIMBAL_AXIS_LEFT_Y].tick(left_y as i32);
            smoother[GIMBAL_AXIS_RIGHT_X].tick(right_x as i32);
            smoother[GIMBAL_AXIS_RIGHT_Y].tick(right_y as i32);
        }

        if status_led_count_down.wait().is_ok() {
            update_status_led(
                &mut status_led,
                &usb_active,
                &calibration_active,
                &throttle_hold_enable,
            );
        }

        if data_process_count_down.wait().is_ok() {
            // Update pressed keys status
            for (index, key) in button_matrix.buttons_pressed().iter().enumerate() {
                buttons[index].pressed = *key;
            }

            // Updated extra buttons
            buttons[25].pressed = left_extra_button.is_low().unwrap();
            buttons[26].pressed = right_extra_button.is_low().unwrap();

            // Secondary way to enter bootloader (pressing all left hands buttons except the hat
            if buttons[0].pressed && buttons[2].pressed {
                status_led.update(StatusMode::Bootloader);
                let gpio_activity_pin_mask: u32 = 0;
                let disable_interface_mask: u32 = 0;
                rp2040_hal::rom_data::reset_to_usb_boot(
                    gpio_activity_pin_mask,
                    disable_interface_mask,
                );
            }

            // // ON/OFF switch for Throttle hold mode
            throttle_hold_enable = axis[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER;

            // Calibration of center position (pressing all right hands buttons except
            // the hat switch)
            if buttons[1].pressed && buttons[2].pressed {
                for (index, item) in axis.iter_mut().enumerate() {
                    item.center = smoother[index].value() as u16;
                    item.min = item.center;
                    item.max = item.center;
                }
                calibration_active = true;
            }

            // Calibration of min and max position
            if calibration_active {
                for (index, item) in axis.iter_mut().enumerate() {
                    if (smoother[index].value() as u16) < item.min {
                        item.min = smoother[index].value() as u16;
                    } else if (smoother[index].value() as u16) > item.max {
                        item.max = smoother[index].value() as u16;
                    }
                }
            }

            if calibration_active && buttons[8].pressed {
                gimbal_mode = GIMBAL_MODE_M10;
                for (index, item) in axis.iter_mut().enumerate() {
                    item.center = smoother[index].value() as u16;
                    item.min = item.center;
                    item.max = item.center;
                }
            } else if calibration_active && buttons[9].pressed {
                gimbal_mode = GIMBAL_MODE_M7;
                for (index, item) in axis.iter_mut().enumerate() {
                    item.center = smoother[index].value() as u16;
                    item.min = item.center;
                    item.max = item.center;
                }
            }
            // Save calibration data to eeprom (pressing right hat switch)
            else if calibration_active && buttons[20].pressed {
                let mut eeprom_data: [u8; 25] = [0; 25];
                for (index, item) in axis.iter_mut().enumerate() {
                    eeprom_data[index * 6] = item.min as u8;
                    eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8;
                    eeprom_data[(index * 6) + 2] = item.max as u8;
                    eeprom_data[(index * 6) + 3] = (item.max >> 8) as u8;
                    eeprom_data[(index * 6) + 4] = item.center as u8;
                    eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8;
                }
                eeprom_data[24] = gimbal_mode;
                let _ = eeprom.write_page(0x01, &eeprom_data);
                calibration_active = false;
            }

            // Process axis values
            for (index, item) in axis.iter_mut().enumerate() {
                item.value = calculate_axis_value(
                    smoother[index].value() as u16,
                    item.min,
                    item.max,
                    item.center,
                    item.deadzone,
                    item.expo,
                    &expo_lut,
                );
            }

            // Process throttle hold value
            let unprocessed_value = axis[GIMBAL_AXIS_LEFT_Y].value;
            if throttle_hold_enable
                && axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER
                && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending
            {
                axis[GIMBAL_AXIS_LEFT_Y].value = remap(
                    axis[GIMBAL_AXIS_LEFT_Y].value,
                    AXIS_MIN,
                    AXIS_CENTER,
                    AXIS_MIN,
                    axis[GIMBAL_AXIS_LEFT_Y].hold,
                );
            } else if throttle_hold_enable
                && axis[GIMBAL_AXIS_LEFT_Y].value > AXIS_CENTER
                && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending
            {
                axis[GIMBAL_AXIS_LEFT_Y].value = remap(
                    axis[GIMBAL_AXIS_LEFT_Y].value,
                    AXIS_CENTER,
                    AXIS_MAX,
                    axis[GIMBAL_AXIS_LEFT_Y].hold,
                    AXIS_MAX,
                );
            } else if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value == AXIS_CENTER {
                axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold;
                axis[GIMBAL_AXIS_LEFT_Y].hold_pending = false;
            } else if throttle_hold_enable {
                axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold;
            }

            // Generate led activity when gimbal is moved from idle position
            for item in axis.iter_mut() {
                if item.value != item.previous_value {
                    usb_activity = true;
                }
                item.previous_value = item.value;
            }

            // Make sure usb will be updated during timeout countdown
            for key in buttons.iter() {
                if key.usb_release_timeout != 0 {
                    usb_activity = true;
                }
            }

            // Generate led activity when a button is pressed
            for (index, key) in buttons.iter_mut().enumerate() {
                if key.pressed != key.previous_pressed {
                    key.usb_changed = true;
                    key.usb_changed_to_pressed = key.pressed;
                    usb_activity = true;
                }
                // Set throttle_hold_value
                if key.pressed != key.previous_pressed
                    && key.pressed
                    && index == 5
                    && unprocessed_value != AXIS_CENTER
                {
                    axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value;
                    axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true;
                } else if key.pressed != key.previous_pressed
                    && key.pressed
                    && index == 5
                    && unprocessed_value == AXIS_CENTER
                {
                    axis[GIMBAL_AXIS_LEFT_Y].hold = AXIS_CENTER;
                    axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true;
                }
                key.previous_pressed = key.pressed;
            }

            // Reset channel locks when calibration is active
            if calibration_active {
                for axis in axis.iter_mut() {
                    axis.hold = 0;
                }
            }
        }

        // Dont send USB HID joystick report if there is no activity
        // This is to avoid preventing the computer from going to sleep
        if usb_update_count_down.wait().is_ok() && usb_activity {
            match usb_hid_joystick.device().write_report(&get_joystick_report(
                &mut buttons,
                &mut axis,
                &throttle_hold_enable,
            )) {
                Err(UsbHidError::WouldBlock) => {}
                Ok(_) => {}
                Err(e) => {
                    status_led.update(StatusMode::Error);
                    core::panic!("Failed to write joystick report: {:?}", e)
                }
            };
            usb_activity = false;
        }
    }
}

/// Update status LED colour based on function layer and capslock
///
/// USB mode = green
/// Activity = flashing blue
/// Error = steady red (ERROR)
///
/// # Arguments
/// * `status_led` - Reference to status LED
/// * `activity` - Reference to bool that indicates if there is activity
/// * `usb_active` - Reference to bool that indicates if USB is active
/// * `axis_idle` - Reference to bool that indicates if all axis are in idle position
/// * `safety_check` - Reference to bool that indicates if safety check has passed
fn update_status_led<P, SM, I>(
    status_led: &mut Ws2812StatusLed<P, SM, I>,
    usb_active: &bool,
    calibration_active: &bool,
    throttle_hold_enable: &bool,
) where
    P: PIOExt + FunctionConfig,
    I: PinId,
    Function<P>: ValidPinMode<I>,
    SM: StateMachineIndex,
{
    if *calibration_active {
        status_led.update(StatusMode::Warning);
    } else if !*usb_active {
        status_led.update(StatusMode::NormalFlash);
    } else if *usb_active && *throttle_hold_enable {
        status_led.update(StatusMode::Other);
    } else if *usb_active && !*throttle_hold_enable {
        status_led.update(StatusMode::Normal);
    }
}

/// Generate keyboard report based on pressed keys and Fn mode (0, 1, 2 or 3)
/// layout::MAP contains the keycodes for each key in each Fn mode
///
/// # Arguments
///
/// * `matrix_keys` - Array of pressed keys
/// * `axis` - Array of joystick axis values
fn get_joystick_report(
    matrix_keys: &mut [Button; NUMBER_OF_BUTTONS + 2],
    axis: &mut [GimbalAxis; 4],
    throttle_hold_enable: &bool,
) -> JoystickReport {
    let x: u16 = axis[GIMBAL_AXIS_LEFT_X].value;
    let y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_LEFT_Y].value;
    let z: u16 = axis[GIMBAL_AXIS_RIGHT_X].value;
    let rx: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value;
    let ry: u16 = AXIS_CENTER;
    let rz: u16 = AXIS_CENTER;
    let mut hat1: u8 = 0xf;
    let mut hat2: u8 = 0xf;
    let mut hat3: u8 = 0xf;
    let mut hat4: u8 = 0xf;

    // Store hat bits
    let mut hat_left: u8 = 0;
    let mut hat_right: u8 = 0;
    for (index, key) in matrix_keys.iter_mut().enumerate() {
        if (15..=19).contains(&index) && key.pressed {
            hat_left |= 1 << (index - 15);
        }
        if (20..=24).contains(&index) && key.pressed {
            hat_right |= 1 << (index - 20);
        }
    }

    // Convert hat switch data to HID code
    let (hat_l, hat_button_l) = format_hat_value(hat_left);
    let (hat_r, hat_button_r) = format_hat_value(hat_right);

    // Handle sec_button (Fn) for left hat switch
    let mut sec_button_pressed: bool = false;
    for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
        if matrix_keys[15].usb_button_sec_enable
            && matrix_keys[15].usb_button_sec_trigger_index == sec_index
            && sec_key.pressed
        {
            sec_button_pressed = true;
            break;
        }
    }
    if matrix_keys[15].usb_changed {
        matrix_keys[15].usb_button_sec_pressed = sec_button_pressed;
    }
    if matrix_keys[15].usb_button_sec != 0 && matrix_keys[15].usb_button_sec_pressed {
        hat3 = hat_l;
    } else {
        hat1 = hat_l;
    }

    // Handle sec_button (Fn) for right hat switch
    let mut sec_button_pressed: bool = false;
    for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
        if matrix_keys[20].usb_button_sec_enable
            && matrix_keys[20].usb_button_sec_trigger_index == sec_index
            && sec_key.pressed
        {
            sec_button_pressed = true;
            break;
        }
    }
    if matrix_keys[20].usb_changed {
        matrix_keys[20].usb_button_sec_pressed = sec_button_pressed;
    }
    if matrix_keys[20].usb_button_sec != 0 && matrix_keys[20].usb_button_sec_pressed {
        hat4 = hat_r;
    } else {
        hat2 = hat_r;
    }

    // Fix button state for center hat press on hat
    matrix_keys[15].pressed = hat_button_l != 0;
    matrix_keys[20].pressed = hat_button_r != 0;

    // Update button array with Sec button trigger status
    // Using indexing instead of iterating to be able to iterate inside loop
    for index in 0..NUMBER_OF_BUTTONS + 2 {
        let mut sec_button_pressed: bool = false;
        for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
            if matrix_keys[index].usb_button_sec_enable
                && matrix_keys[index].usb_button_sec_trigger_index == sec_index
                && sec_key.pressed
            {
                sec_button_pressed = true;
                break;
            }
        }
        // Only update button when "root key" is pressed (ie. Do not change sec button as soon as Fn
        // key is pressed
        if matrix_keys[index].usb_changed {
            matrix_keys[index].usb_button_sec_pressed = sec_button_pressed;
        }
    }

    // Update button state for joystick buttons
    let mut buttons: u32 = 0;
    for key in matrix_keys.iter_mut() {
        // Toggle mode button
        if key.usb_changed && key.usb_button_toggle_enable {
            key.usb_release_timeout = RELEASE_RIMEOUT;
        }
        if key.pressed
            && key.usb_button != 0
            && key.usb_button_toggle_enable
            && (key.usb_release_timeout > 1 || *throttle_hold_enable)
        {
            buttons |= 1 << (key.usb_button - 1);
        } else if !key.pressed
            && key.usb_button_sec != 0
            && key.usb_button_toggle_enable
            && key.usb_release_timeout > 1
            && !throttle_hold_enable
        {
            buttons |= 1 << (key.usb_button_sec - 1);
        // Sec button mode
        } else if key.pressed && key.usb_button_sec != 0 && key.usb_button_sec_pressed {
            buttons |= 1 << (key.usb_button_sec - 1);
        } else if key.pressed && key.usb_button != 0 && !key.usb_button_toggle_enable {
            buttons |= 1 << (key.usb_button - 1);
        }
    }

    // Auto release button when in toggle mode
    for key in matrix_keys.iter_mut() {
        if key.usb_release_timeout > 0 {
            key.usb_release_timeout -= 1;
        }
    }

    // Reset changed flags
    for key in matrix_keys.iter_mut() {
        key.usb_changed = false;
    }

    JoystickReport {
        x,
        y,
        z,
        rx,
        ry,
        rz,
        hat1,
        hat2,
        hat3,
        hat4,
        buttons,
    }
}

/// Format hat value from 5 switches to USB HID coded value and button state
///
/// # Arguments
/// * `input` - Hat value coded as
///    bit 2-5: direction (U L R D)
///    bit 1: button state
///    0 = not pressed
///    1 = pressed
fn format_hat_value(input: u8) -> (u8, u8) {
    const HAT_CENTER: u8 = 0xf;
    const HAT_UP: u8 = 0;
    const HAT_UP_RIGHT: u8 = 1;
    const HAT_RIGHT: u8 = 2;
    const HAT_DOWN_RIGHT: u8 = 3;
    const HAT_DOWN: u8 = 4;
    const HAT_DOWN_LEFT: u8 = 5;
    const HAT_LEFT: u8 = 6;
    const HAT_UP_LEFT: u8 = 7;

    let direction: u8 = match input & 0xFE {
        2 => HAT_UP,
        4 => HAT_RIGHT,
        6 => HAT_UP_RIGHT,
        8 => HAT_DOWN,
        12 => HAT_DOWN_RIGHT,
        16 => HAT_LEFT,
        24 => HAT_DOWN_LEFT,
        18 => HAT_UP_LEFT,
        _ => HAT_CENTER,
    };

    // Alpine hat switch button filter
    let mut button_state: u8 = 0;
    if input & 0x01 == 0x01 && direction == HAT_CENTER {
        button_state = 1;
    }

    (direction, button_state)
}

/// Calculate value for joystick axis
///
/// # Arguments
/// * `value` - Value to calibrate
/// * `min` - Lower bound of the value's current range
/// * `max` - Upper bound of the value's current range
/// * `center` - Center of the value's current range
/// * `deadzone` - Deadzone of the value's current range (min, center, max)
/// * `expo` - Exponential curve factor enabled
/// * `expo_lut` - Exponential curve lookup table
fn calculate_axis_value(
    value: u16,
    min: u16,
    max: u16,
    center: u16,
    deadzone: (u16, u16, u16),
    expo: bool,
    expo_lut: &[u16; AXIS_MAX as usize + 1],
) -> u16 {
    if value <= min {
        return AXIS_MIN;
    }
    if value >= max {
        return AXIS_MAX;
    }

    let mut calibrated_value = AXIS_CENTER;

    if value > (center + deadzone.1) {
        calibrated_value = remap(
            value,
            center + deadzone.1,
            max - deadzone.2,
            AXIS_CENTER,
            AXIS_MAX,
        );
    } else if value < (center - deadzone.1) {
        calibrated_value = remap(
            value,
            min + deadzone.0,
            center - deadzone.1,
            AXIS_MIN,
            AXIS_CENTER,
        );
    }

    if expo && calibrated_value != AXIS_CENTER {
        calibrated_value = expo_lut[calibrated_value as usize];
    }

    calibrated_value
}

/// Remapping values from one range to another
///
/// # Arguments
/// * `value` - Value to remap
/// * `in_min` - Lower bound of the value's current range
/// * `in_max` - Upper bound of the value's current range
/// * `out_min` - Lower bound of the value's target range
/// * `out_max` - Upper bound of the value's target range
fn remap(value: u16, in_min: u16, in_max: u16, out_min: u16, out_max: u16) -> u16 {
    constrain(
        (value as i64 - in_min as i64) * (out_max as i64 - out_min as i64)
            / (in_max as i64 - in_min as i64)
            + out_min as i64,
        out_min as i64,
        out_max as i64,
    ) as u16
}

/// Constrain a value to a given range
///
/// # Arguments
/// * `value` - Value to constrain
/// * `out_min` - Lower bound of the value's target range
/// * `out_max` - Upper bound of the value's target range
fn constrain<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
    if value < out_min {
        out_min
    } else if value > out_max {
        out_max
    } else {
        value
    }
}

/// Generate exponential lookup table for 12bit values
///
/// # Arguments
/// * `expo` - Exponential curve factor (range 0.0 - 1.0)
fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] {
    let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1];
    for i in 0..AXIS_MAX + 1 {
        let value_float = i as f32 / AXIS_MAX as f32;
        // Calculate expo using 9th order polynomial function with 0.5 as center point
        let value_exp: f32 =
            expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float;
        lut[i as usize] = constrain((value_exp * AXIS_MAX as f32) as u16, AXIS_MIN, AXIS_MAX);
    }
    lut
}
