//! Project: CMtec CMDR joystick 25
//! Date:    2023-08-01
//! Author:  Christoffer Martinsson
//! Email:   cm@cmtec.se
//! License: Please refer to LICENSE in root directory
#![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::digital::{InputPin, OutputPin};
use embedded_hal_0_2::adc::OneShot;
use embedded_hal_0_2::timer::CountDown;
use fugit::{ExtU32, RateExtU32};
use libm::powf;
use panic_halt as _;
use rp2040_hal::{
    Sio,
    adc::Adc,
    adc::AdcPin,
    clocks::{Clock, init_clocks_and_plls},
    gpio::{AnyPin, Pins},
    i2c::I2C,
    pac,
    pio::{PIOExt, StateMachineIndex},
    timer::Timer,
    watchdog::Watchdog,
};
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::*;

// The linker will place this boot block at the start of our program image. We
/// need this to help the ROM bootloader get our code up and running.
#[unsafe(link_section = ".boot2")]
#[unsafe(no_mangle)]
#[used]
pub static BOOT2_FIRMWARE: [u8; 256] = rp2040_boot2::BOOT_LOADER_W25Q080;

const XTAL_FREQ_HZ: u32 = 12_000_000u32;

// 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 ADC_MIN: u16 = 0;
pub const ADC_MAX: u16 = 4095;
pub const AXIS_CENTER: u16 = (ADC_MIN + ADC_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,
}

#[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: ADC_MAX,
            min: ADC_MIN,
            center: AXIS_CENTER,
            deadzone: (100, 50, 100),
            expo: true,
            trim: 0,
            hold: AXIS_CENTER,
            hold_pending: false,
        }
    }
}

#[rp2040_hal::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(
        XTAL_FREQ_HZ,
        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.gpio14.reconfigure(), // sda
        pins.gpio15.reconfigure(), // 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 = AdcPin::new(pins.gpio29.into_floating_input()).unwrap();
    let mut adc_pin_left_y = AdcPin::new(pins.gpio28.into_floating_input()).unwrap();
    let mut adc_pin_right_x = AdcPin::new(pins.gpio27.into_floating_input()).unwrap();
    let mut adc_pin_right_y = AdcPin::new(pins.gpio26.into_floating_input()).unwrap();

    // Setting up array with pins connected to button rows
    let button_matrix_row_pins: &mut [&mut dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &mut [
        &mut pins.gpio6.into_pull_up_input(),
        &mut pins.gpio8.into_pull_up_input(),
        &mut pins.gpio4.into_pull_up_input(),
        &mut pins.gpio7.into_pull_up_input(),
        &mut pins.gpio5.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.gpio9.into_push_pull_output(),
        &mut pins.gpio10.into_push_pull_output(),
        &mut pins.gpio11.into_push_pull_output(),
        &mut pins.gpio12.into_push_pull_output(),
        &mut pins.gpio13.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 mut left_extra_button = pins.gpio1.into_pull_up_input();
    let mut right_extra_button = pins.gpio0.into_pull_up_input();

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

    // Set red color to statusled indicating error if not reaching assumed state (USB connect)
    status_led.update(StatusMode::Error);

    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, &clocks);

    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 virtual_ry: i16 = 0;
    let mut virtual_rz: i16 = 0;
    let mut virtual_step: i16 = 512;
    let mut gimbal_mode: u8;

    // 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  |                 |
    // ---------------------------------------------------------------
    // Config Layer (holding CONFIG button)
    // ---------------------------------------------------------------
    // |BOOT L| CAL U|           | CONFIG |              |  -  |  -  |
    // ---------------------------------------------------------------
    // |     |  -  |  -  |  -  |             |  -  |  -  |  -  |     |
    // |                                                             |
    // |                 |C M10|             |  -  |                 |
    // |                 |C M7 |             |  -  |                 |
    // |        -/-                                        -/-       |
    // |                 |  -  |             |  -  |                 |
    // |           |  -  |  -  |  -  | |  -  |C OK |  -  |           |
    // |                 |  -  |             |  -  |                 |
    // ---------------------------------------------------------------
    // USB HID joystick map :
    // ---------------------------------------------------------------
    // | Ry- L| Ry+ U| Rz- U|      | B26 |        | Rz+ U| B1 U| B2 L|
    // ---------------------------------------------------------------
    // |     | B5 |  B14 | B9  |             | B10 | B15 | B6 |      |
    // |                                                             |
    // |                 | B13 |             | B17 |                 |
    // |                 | B16 |             | B12 |                 |
    // |      X1/Y1                                       X2/Y2      |
    // |                 | B18 |             | B22 |                 |
    // |           | B21 | B11 | B19 | | B25 | TH  | B23 |           |
    // |                 | B20 |             | B24 |                 |
    // ---------------------------------------------------------------
    //
    pub const CONFIG_BUTTON: usize = 2;
    pub const BOOT_BUTTON: usize = 0;
    pub const CAL_BUTTON: usize = 1;
    pub const CAL_DONE_BUTTON: usize = 20;
    pub const CAL_M10_GIMBLE_BUTTON: usize = 8;
    pub const CAL_M7_GIMBLE_BUTTON: usize = 9;
    // Special button functions
    // Throttle hold:
    pub const TH_BUTTON: usize = 20;

    // Set up usb button layout
    buttons[0].usb_button = 4;
    buttons[1].usb_button = 3;
    buttons[2].usb_button = 26; // Button used as global config. Do not define secondary USB button!
    buttons[3].usb_button = 2;
    buttons[4].usb_button = 1;
    buttons[5].usb_button = 5;
    buttons[6].usb_button = 14;
    buttons[7].usb_button = 9;
    buttons[8].usb_button = 13;
    buttons[9].usb_button = 16;
    buttons[10].usb_button = 6;
    buttons[11].usb_button = 15;
    buttons[12].usb_button = 10;
    buttons[13].usb_button = 17;
    buttons[14].usb_button = 12;
    buttons[15].usb_button = 11;
    buttons[16].usb_button = 18;
    buttons[17].usb_button = 19;
    buttons[18].usb_button = 20;
    buttons[19].usb_button = 21;
    buttons[20].usb_button = 0;
    buttons[21].usb_button = 22;
    buttons[22].usb_button = 23;
    buttons[23].usb_button = 24;
    buttons[24].usb_button = 25;
    buttons[25].usb_button = 7;
    buttons[26].usb_button = 8;

    // Table for gimbal expo curve lookup insded of doing floating point math for every analog read
    let expo_lut: [u16; ADC_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(rp2040_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))
        .strings(&[StringDescriptors::default()
            .manufacturer("CMtec")
            .product("CMDR Joystick 25")
            .serial_number("0001")])
        .unwrap()
        .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() {
            // Scan button matrix
            button_matrix.scan_matrix(&mut delay);
            // Read ADC values
            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 = ADC_MAX - left_x;
                right_y = ADC_MAX - right_y;
            } else if gimbal_mode == GIMBAL_MODE_M7 {
                // Invert Y1 and X2 axis (M7 gimbals)
                left_y = ADC_MAX - left_y;
                right_x = ADC_MAX - right_x;
            }
            // Process anlog filter
            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();

            // Filter left hat swith buttons
            for i in 16..=19 {
                if (16..=19).filter(|&j| j != i).any(|j| buttons[j].pressed) {
                    buttons[i].pressed = false;
                }
            }
            // Fix button state for center hat press on hat
            if buttons[16..=19].iter().any(|b| b.pressed) {
                buttons[15].pressed = false;
            }
            // Filter right hat swith buttons
            for i in 21..=24 {
                if (21..=24).filter(|&j| j != i).any(|j| buttons[j].pressed) {
                    buttons[i].pressed = false;
                }
            }
            // Fix button state for center hat press on hat
            if buttons[21..=24].iter().any(|b| b.pressed) {
                buttons[20].pressed = false;
            }

            // Secondary way to enter bootloader (pressing all left hands buttons except the hat
            if buttons[BOOT_BUTTON].pressed && buttons[CONFIG_BUTTON].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,
                );
            }

            // Calibration of center position (pressing all right hands buttons except
            // the hat switch)
            if buttons[CAL_BUTTON].pressed && buttons[CONFIG_BUTTON].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[CAL_M10_GIMBLE_BUTTON].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[CAL_M7_GIMBLE_BUTTON].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[CAL_DONE_BUTTON].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;
            }

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

            // 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,
                    ADC_MIN,
                    AXIS_CENTER,
                    ADC_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,
                    ADC_MAX,
                    axis[GIMBAL_AXIS_LEFT_Y].hold,
                    ADC_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;
            }

            // Update Virtual RY
            if buttons[1].pressed && !buttons[0].pressed {
                virtual_ry = virtual_ry.saturating_add(100);
                usb_activity = true;
            } else if buttons[0].pressed && !buttons[1].pressed {
                virtual_ry = virtual_ry.saturating_sub(100);
                usb_activity = true;
            } else if virtual_ry != 0 && !buttons[1].pressed && !buttons[0].pressed {
                // Optional: decay to center
                virtual_ry = 0;
                usb_activity = true;
            }
            // Update Virtual RZ
            if buttons[25].pressed && !buttons[26].pressed {
                virtual_rz = virtual_rz.saturating_add(100);
                usb_activity = true;
            } else if buttons[26].pressed && !buttons[25].pressed {
                virtual_rz = virtual_rz.saturating_sub(100);
                usb_activity = true;
            } else if virtual_rz != 0 && !buttons[25].pressed && !buttons[26].pressed {
                // Optional: decay to center
                virtual_rz = 0;
                usb_activity = true;
            }

            // 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;
            }

            // 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 == TH_BUTTON
                    && 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 == TH_BUTTON
                    && 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,
                &virtual_ry,
                &virtual_rz,
            )) {
                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
///
/// Waiting for USB enumerate = flashing green
/// USB mode (Normal mode) = green
/// Throttle hold mode = orange
/// Calibrating = flashing blue
///
/// # Arguments
/// * `status_led` - Reference to status LED
/// * `usb_active` - Reference to bool that indicates if USB is active
/// * `throttle_hold_enable` - Reference to bool that indicates if left y axis is hold (trimmed)
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
    I: AnyPin<Function = P::PinFunction>,
    P: PIOExt,
    SM: StateMachineIndex,
{
    if *calibration_active {
        status_led.update(StatusMode::ActivityFlash);
    } 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],
    virtual_ry: &i16,
    virtual_rz: &i16,
) -> JoystickReport {
    let x: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_LEFT_X].value);
    let y: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_LEFT_Y].value);
    let z: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_RIGHT_X].value);
    let rx: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value);
    let ry: i16 = *virtual_ry;
    let rz: i16 = *virtual_rz;

    // Update button state for joystick buttons
    let mut buttons: u32 = 0;
    for key in matrix_keys.iter_mut() {
        if key.pressed && key.usb_button != 0 {
            buttons |= 1 << (key.usb_button - 1);
        }
    }

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

    JoystickReport {
        x,
        y,
        z,
        rx,
        ry,
        rz,
        buttons,
    }
}

/// 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; ADC_MAX as usize + 1],
) -> u16 {
    if value <= min {
        return ADC_MIN;
    }
    if value >= max {
        return ADC_MAX;
    }

    let mut calibrated_value = AXIS_CENTER;

    if value > (center + deadzone.1) {
        calibrated_value = remap(
            value,
            center + deadzone.1,
            max - deadzone.2,
            AXIS_CENTER,
            ADC_MAX,
        );
    } else if value < (center - deadzone.1) {
        calibrated_value = remap(
            value,
            min + deadzone.0,
            center - deadzone.1,
            ADC_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; ADC_MAX as usize + 1] {
    let mut lut: [u16; ADC_MAX as usize + 1] = [0; ADC_MAX as usize + 1];
    for i in 0..ADC_MAX + 1 {
        let value_float = i as f32 / ADC_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 * ADC_MAX as f32) as u16, ADC_MIN, ADC_MAX);
    }
    lut
}

/// Convert 12bit unsigned values to 16bit signed
///
/// # Arguments
/// * `val` - 12bit unsigned
fn axis_12bit_to_i16(val: u16) -> i16 {
    assert!(val <= 0x0FFF); // Ensure it's 12-bit

    // Map 0..4095 → -32768..32767 using integer math
    // Formula: ((val * 65535) / 4095) - 32768
    let scaled = ((val as u32 * 65535) / 4095) as i32 - 32768;

    scaled as i16
}
