//! # CMtec CMDR Joystick 25 - Main Firmware
//!
//! **Project:** CMtec CMDR joystick 25
//! **Date:** 2023-08-01
//! **Author:** Christoffer Martinsson
//! **Email:** cm@cmtec.se
//! **License:** Please refer to LICENSE in root directory
//!
//! ## Overview
//!
//! This is the main firmware entry point for the CMtec CMDR Joystick 25, a professional-grade
//! USB HID joystick controller built on the Raspberry Pi RP2040 microcontroller. The firmware
//! provides advanced features including:
//!
//! - **4-axis gimbal control** (Left X/Y, Right X/Y) with M10/M7 hardware support
//! - **Virtual axis control** (RY/RZ) via front panel buttons with direction compensation
//! - **Advanced calibration system** with real-time min/max tracking and EEPROM persistence
//! - **Exponential response curves** for enhanced control feel and precision
//! - **Throttle hold functionality** with configurable hold values
//! - **Professional button matrix** (5x5 grid + 2 extra buttons)
//! - **USB HID compliance** with 7-axis, 32-button, 8-direction HAT switch support
//! - **Status LED system** with multiple operational modes
//!
//! ## Architecture
//!
//! The firmware follows a modular design with clear separation of concerns:
//!
//! - **Hardware abstraction** (`hardware.rs`) - Pin definitions and hardware constants
//! - **Axis management** (`axis.rs`) - Gimbal and virtual axis processing with filtering
//! - **Button processing** (`buttons.rs`) - Matrix scanning, debouncing, and special combinations
//! - **Calibration system** (`calibration.rs`) - Real-time calibration and gimbal mode selection
//! - **USB reporting** (`usb_report.rs`) - HID report generation and axis conversion
//! - **Data persistence** (`storage.rs`) - EEPROM calibration data management
//! - **Status indication** (`status.rs`) - LED control and system state visualization
//! - **Signal processing** (`expo.rs`) - Exponential curve lookup tables
//!
//! ## Operational Modes
//!
//! - **Normal Operation** - Standard joystick functionality with all features active
//! - **Calibration Mode** - Real-time axis calibration with min/max tracking
//! - **Throttle Hold** - Maintains throttle position for hands-free operation
//! - **Virtual Throttle** - Button-controlled axis remapping for alternative control schemes
//! - **Bootloader Entry** - Safe firmware update mode via USB mass storage
//!
//! ## Hardware Support
//!
//! - **M10 Gimbal** - Standard configuration with proper axis mapping
//! - **M7 Gimbal** - Hardware variant with axis inversion compensation
//! - **USB HID** - Full-speed USB 2.0 with professional descriptor compliance
//! - **EEPROM Storage** - Persistent calibration data with error handling
//! - **WS2812 Status LED** - Advanced status indication with multiple modes
#![no_std]
#![no_main]

mod axis;
mod button_matrix;
mod buttons;
mod calibration;
mod expo;
mod hardware;
mod mapping;
mod status;
mod storage;
mod usb_joystick_device;
mod usb_report;

use axis::AxisManager;
use button_matrix::ButtonMatrix;
use buttons::{ButtonManager, SpecialAction};
use calibration::CalibrationManager;
use core::convert::Infallible;
use core::panic::PanicInfo;
use cortex_m::delay::Delay;
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;
use hardware::timers;
use mapping::*;
use rp2040_hal::{
    adc::Adc,
    adc::AdcPin,
    clocks::{init_clocks_and_plls, Clock},
    gpio::Pins,
    i2c::I2C,
    pac,
    pio::PIOExt,
    timer::Timer,
    watchdog::Watchdog,
    Sio,
};
use status::{StatusLed, StatusMode, SystemState};
use usb_device::class_prelude::*;
use usb_device::prelude::*;
use usb_joystick_device::JoystickConfig;
use usb_report::get_joystick_report;
use usbd_human_interface_device::prelude::*;

#[panic_handler]
fn panic(_info: &PanicInfo) -> ! {
    loop {}
}

/// Boot loader configuration for RP2040 ROM.
///
/// The linker places this boot block at the start of our program image to help the ROM
/// bootloader initialize our code. This specific boot loader supports W25Q080 flash memory.
#[link_section = ".boot2"]
#[no_mangle]
#[used]
pub static BOOT2_FIRMWARE: [u8; 256] = rp2040_boot2::BOOT_LOADER_W25Q080;

use expo::ExpoLUT;

/// Hardware configuration imports from the hardware abstraction layer.
use hardware::{ADC_MAX, ADC_MIN};
use hardware::{BUTTON_COLS, BUTTON_ROWS, NUMBER_OF_BUTTONS};

/// Additional hardware constants for button debouncing.
use hardware::DEBOUNCE;

#[cfg(not(test))]
#[rp2040_hal::entry]
fn main() -> ! {
    // # Hardware Initialization Phase
    //
    // Initialize all RP2040 peripheral singleton objects and configure the hardware
    // subsystems for joystick operation.

    // Acquire exclusive access to RP2040 peripherals
    let mut pac = pac::Peripherals::take().unwrap();

    // Initialize watchdog timer (required for clock configuration)
    let mut watchdog = Watchdog::new(pac.WATCHDOG);

    // Configure system clocks and phase-locked loops for stable operation
    let clocks = init_clocks_and_plls(
        hardware::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();

    // Initialize SIO (Single-cycle I/O) for high-performance GPIO operations
    let sio = Sio::new(pac.SIO);

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

    let i2c = I2C::i2c1(
        pac.I2C1,
        get_pin!(pins, i2c_sda).reconfigure(), // sda
        get_pin!(pins, i2c_scl).reconfigure(), // scl
        hardware::i2c::i2c_frequency(),
        &mut pac.RESETS,
        hardware::i2c::system_clock(),
    );

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

    // # ADC Configuration
    //
    // Configure the 12-bit ADC for reading analog values from the gimbal potentiometers.
    // The ADC provides 0-4095 raw values that are later processed through filtering,
    // calibration, and exponential curve lookup.

    // Initialize 12-bit ADC with 4 channels for gimbal axes
    let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);

    // Configure ADC input pins for 4-axis gimbal (Left X/Y, Right X/Y)
    let mut adc_pin_left_x = AdcPin::new(get_pin!(pins, adc_left_x).into_floating_input()).unwrap();
    let mut adc_pin_left_y = AdcPin::new(get_pin!(pins, adc_left_y).into_floating_input()).unwrap();
    let mut adc_pin_right_x =
        AdcPin::new(get_pin!(pins, adc_right_x).into_floating_input()).unwrap();
    let mut adc_pin_right_y =
        AdcPin::new(get_pin!(pins, adc_right_y).into_floating_input()).unwrap();

    // # Button Matrix Configuration\n    //\n    // Configure the 5x5 button matrix using row/column scanning technique.\n    // Rows are configured as pull-up inputs, columns as push-pull outputs.\n    // This allows scanning 25 buttons with only 10 GPIO pins.\n\n    // Configure button matrix row pins (inputs with pull-up resistors)
    let button_matrix_row_pins: &mut [&mut dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &mut [
        &mut get_pin!(pins, button_row_0).into_pull_up_input(),
        &mut get_pin!(pins, button_row_1).into_pull_up_input(),
        &mut get_pin!(pins, button_row_2).into_pull_up_input(),
        &mut get_pin!(pins, button_row_3).into_pull_up_input(),
        &mut get_pin!(pins, button_row_4).into_pull_up_input(),
    ];

    // Configure button matrix column pins (push-pull outputs for scanning)
    let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [
        &mut get_pin!(pins, button_col_0).into_push_pull_output(),
        &mut get_pin!(pins, button_col_1).into_push_pull_output(),
        &mut get_pin!(pins, button_col_2).into_push_pull_output(),
        &mut get_pin!(pins, button_col_3).into_push_pull_output(),
        &mut get_pin!(pins, button_col_4).into_push_pull_output(),
    ];

    // Initialize button matrix scanner with debouncing
    let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
        ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE);

    // Configure matrix pins for scanning operation
    button_matrix.init_pins();

    // Configure additional buttons outside the matrix (total: 27 buttons)
    let mut left_extra_button = get_pin!(pins, left_extra_button).into_pull_up_input();
    let mut right_extra_button = get_pin!(pins, right_extra_button).into_pull_up_input();

    // # Status LED Initialization
    //
    // Configure WS2812 RGB LED using PIO state machine for status indication.
    // The LED provides visual feedback for system state, calibration mode,
    // errors, and operational status.

    // Initialize WS2812 status LED using PIO state machine
    let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS);
    let mut status_led = StatusLed::new(
        get_pin!(pins, status_led).into_function(),
        &mut pio,
        sm0,
        clocks.peripheral_clock.freq(),
    );

    // Initial LED state (red) indicates system initialization
    status_led.update(StatusMode::Error);

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

    // # Bootloader Entry Check
    //
    // Perform initial button scan to check for bootloader entry condition.
    // Holding the front-left-lower button during power-on enters USB mass storage
    // bootloader mode for firmware updates.

    // Scan button matrix multiple times to ensure stable debounced readings
    for _ in 0..10 {
        // Multiple scans ensure debounce algorithm captures stable button states
        button_matrix.scan_matrix(&mut delay);
    }
    if button_matrix.buttons_pressed()[BUTTON_FRONT_LEFT_LOWER] {
        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);
    }

    // # Timer Configuration
    //
    // Configure multiple countdown timers for different system operations:
    // - Status LED updates (visual feedback)
    // - Button matrix scanning (input processing)
    // - Data processing (axis and button logic)
    // - USB report transmission (HID communication)
    // - Millisecond timing (general purpose)

    // Initialize hardware timer peripheral
    let timer = Timer::new(pac.TIMER, &mut pac.RESETS, &clocks);

    let mut status_led_count_down = timer.count_down();
    status_led_count_down.start(timers::STATUS_LED_INTERVAL_MS.millis());

    // Removed unused millisecond countdown timer

    let mut scan_count_down = timer.count_down();
    scan_count_down.start(timers::SCAN_INTERVAL_US.micros());

    let mut data_process_count_down = timer.count_down();
    data_process_count_down.start(timers::DATA_PROCESS_INTERVAL_US.micros());

    let mut usb_update_count_down = timer.count_down();
    usb_update_count_down.start(timers::USB_UPDATE_INTERVAL_MS.millis());

    let mut usb_activity: bool = false;
    let mut usb_active: bool = false;
    let mut vt_enable: bool = false;

    let mut axis_manager = AxisManager::new();
    let mut button_manager = ButtonManager::new();
    let mut calibration_manager = CalibrationManager::new();

    // # Signal Processing Initialization
    //
    // Initialize exponential curve lookup tables and digital smoothing filters.
    // The expo curves provide non-linear response characteristics for enhanced
    // control feel, while smoothing filters reduce ADC noise and jitter.

    // Create exponential curve lookup tables (avoids floating-point math in real-time)
    let expo_lut = ExpoLUT::new(0.3);
    let expo_lut_virtual = ExpoLUT::new(0.6);

    // Initialize digital smoothing filters for each gimbal axis
    let mut smoother = AxisManager::create_smoothers();

    // # USB HID Configuration
    //
    // Configure USB Human Interface Device (HID) for joystick functionality.
    // The device presents as a standard USB joystick with:
    // - 7 analog axes (X, Y, Z, RX, RY, RZ, Slider)
    // - 32 digital buttons
    // - 8-direction HAT switch
    // - Full-speed USB 2.0 compliance

    // Initialize USB bus allocator for RP2040
    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(hardware::USB_VID, hardware::USB_PID))
            .strings(&[StringDescriptors::default()
                .manufacturer(hardware::usb::MANUFACTURER)
                .product(hardware::usb::PRODUCT)
                .serial_number(hardware::usb::SERIAL_NUMBER)])
            .unwrap()
            .build();

    // # Calibration Data Initialization
    //
    // Load previously saved calibration data from EEPROM storage.
    // Each axis has individual min/max/center values for accurate scaling.
    // Gimbal mode (M10/M7) is also restored from storage.

    // Load calibration data from EEPROM using CalibrationManager
    let mut read_fn = |addr: u32| eeprom.read_byte(addr).map_err(|_| ());
    CalibrationManager::load_axis_calibration(&mut axis_manager.axes, &mut read_fn);
    let gimbal_mode = CalibrationManager::load_gimbal_mode(&mut read_fn);
    axis_manager.set_gimbal_mode(gimbal_mode);
    calibration_manager.set_gimbal_mode(gimbal_mode);

    loop {
        // # Main Control Loop
        //
        // The main control loop handles multiple time-sliced operations:
        // 1. USB HID polling and device state management
        // 2. High-frequency button matrix scanning and ADC reading
        // 3. Medium-frequency data processing and axis calculations
        // 4. Low-frequency USB report transmission and status updates

        // Handle USB device polling and maintain connection state
        if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
            usb_active = true;
        }

        if scan_count_down.wait().is_ok() {
            // ## High-Frequency Input Sampling (1kHz)
            //
            // Sample all inputs at high frequency for responsive control:
            // - Button matrix scanning with debouncing
            // - ADC reading from all 4 gimbal axes
            // - Digital filtering for noise reduction

            // Scan 5x5 button matrix for input changes
            button_matrix.scan_matrix(&mut delay);

            // Read raw 12-bit ADC values from all 4 gimbal potentiometers
            let mut raw_values = [
                adc.read(&mut adc_pin_left_x).unwrap(),
                adc.read(&mut adc_pin_left_y).unwrap(),
                adc.read(&mut adc_pin_right_x).unwrap(),
                adc.read(&mut adc_pin_right_y).unwrap(),
            ];

            // Apply hardware-specific axis compensation (M10/M7 differences)
            axis_manager.apply_gimbal_compensation(&mut raw_values);

            // Apply digital smoothing filters to reduce ADC noise and jitter
            axis_manager.update_smoothers(&mut smoother, &raw_values);

            // Note: Filtered values are processed in the data processing phase
            // through calculate_axis_value() with expo curves and calibration
        }

        if status_led_count_down.wait().is_ok() {
            // ## Status LED Updates (100Hz)
            //
            // Update status LED to reflect current system state:
            // - Green: Normal operation with USB connection
            // - Blue: Calibration mode active
            // - Yellow: Throttle hold or Virtual Throttle enabled
            // - Red: Error state or disconnected
            // - Purple: Bootloader mode

            let system_state = SystemState {
                usb_active,
                calibration_active: calibration_manager.is_active(),
                throttle_hold_enable: axis_manager.throttle_hold_enable,
                vt_enable,
            };
            status_led.update_from_system_state(
                system_state,
                (timer.get_counter().ticks() / 1000) as u32,
            );
        }

        if data_process_count_down.wait().is_ok() {
            // ## Medium-Frequency Data Processing (100Hz)
            //
            // Process all input data and handle complex logic:
            // - Button state management and special combinations
            // - Axis processing with expo curves and calibration
            // - Calibration system updates and mode selection
            // - Virtual axis control and throttle hold processing

            // Update button states from matrix scan and extra buttons
            button_manager.update_from_matrix(&mut button_matrix);
            button_manager.update_extra_buttons(&mut left_extra_button, &mut right_extra_button);
            button_manager.filter_hat_switches();

            // Process special button combinations for system control
            let action = button_manager.check_special_combinations(
                axis_manager.get_value_before_hold(),
                calibration_manager.is_active(),
            );
            match action {
                SpecialAction::Bootloader => {
                    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,
                    );
                }
                SpecialAction::StartCalibration => {
                    for (index, item) in axis_manager.axes.iter_mut().enumerate() {
                        item.center = smoother[index].value() as u16;
                        item.min = item.center;
                        item.max = item.center;
                    }
                    axis_manager.clear_throttle_hold(); // Clear throttle hold when cancelling calibration
                    calibration_manager.start_calibration();
                }
                SpecialAction::CancelCalibration => {
                    calibration_manager.stop_calibration();
                }
                SpecialAction::ThrottleHold(hold_value) => {
                    axis_manager.set_throttle_hold(hold_value);
                }
                SpecialAction::VirtualThrottleToggle => {
                    vt_enable = !vt_enable;
                }
                SpecialAction::CalibrationSetModeM10 => {
                    // Set gimbal mode to M10 and reset calibration
                    if calibration_manager.set_gimbal_mode_m10(&mut axis_manager.axes, &smoother) {
                        axis_manager.set_gimbal_mode(calibration_manager.get_gimbal_mode());
                        axis_manager.clear_throttle_hold(); // Clear holds after mode change
                    }
                }
                SpecialAction::CalibrationSetModeM7 => {
                    // Set gimbal mode to M7 and reset calibration
                    if calibration_manager.set_gimbal_mode_m7(&mut axis_manager.axes, &smoother) {
                        axis_manager.set_gimbal_mode(calibration_manager.get_gimbal_mode());
                        axis_manager.clear_throttle_hold(); // Clear holds after mode change
                    }
                }
                SpecialAction::CalibrationSave => {
                    // Save calibration data and end calibration mode
                    calibration_manager
                        .save_calibration(&axis_manager.axes, &mut |page: u32, data: &[u8]| {
                            eeprom.write_page(page, data).map_err(|_| ())
                        });
                }
                SpecialAction::None => {}
            }

            // Always update calibration for dynamic min/max tracking when active
            calibration_manager.update_dynamic_calibration(&mut axis_manager.axes, &smoother);

            // ### Axis Processing Pipeline
            //
            // Complete axis processing chain:
            // 1. Apply exponential curves for enhanced feel
            // 2. Handle throttle hold functionality
            // 3. Update virtual axes (RY/RZ) from button input
            // 4. Track axis movement for USB activity detection

            // Process gimbal axes through calibration, expo curves, and scaling
            if axis_manager.process_axis_values(&smoother, &expo_lut) {
                usb_activity = true;
            }

            // Update virtual axes based on front button states
            if axis_manager.update_virtual_axes(button_manager.buttons(), vt_enable) {
                usb_activity = true;
            }

            // Process button logic (press types, timing, USB mapping)
            if button_manager.process_button_logic_with_timer(&timer) {
                usb_activity = true;
            }
        }

        // ## USB HID Report Transmission (20Hz)
        //
        // Transmit USB HID reports only when there is input activity.
        // This power-management approach prevents the computer from staying
        // awake unnecessarily while maintaining responsive control.
        //
        // The report includes:
        // - All 7 analog axes with proper scaling
        // - 32-button bitmask with USB mapping
        // - 8-direction HAT switch state
        // - Virtual throttle mode handling

        // Only transmit USB reports when input activity is detected
        if usb_update_count_down.wait().is_ok() && usb_activity {
            let virtual_ry_value = axis_manager.get_virtual_ry_value(&expo_lut_virtual);
            let virtual_rz_value = axis_manager.get_virtual_rz_value(&expo_lut_virtual);

            match usb_hid_joystick.device().write_report(&get_joystick_report(
                button_manager.buttons_mut(),
                &mut axis_manager.axes,
                virtual_ry_value,
                virtual_rz_value,
                &vt_enable,
            )) {
                Err(UsbHidError::WouldBlock) => {}
                Ok(_) => {}
                Err(e) => {
                    status_led.update(StatusMode::Error);
                    core::panic!("Failed to write joystick report: {:?}", e)
                }
            };
            usb_activity = false;
        }
    }
}
