//! Axis processing for CMDR Joystick 25
//!
//! Responsibilities
//! - Apply gimbal mode compensation (M10/M7) to raw ADC readings
//! - Feed smoothed samples into per‑axis filters
//! - Calibrate + apply deadzones + optional expo via lookup table
//! - Implement throttle‑hold remapping on the throttle axis
//! - Maintain virtual axes updated by button presses
//! - Detect activity to throttle USB traffic
//!
//! Data flow
//! raw ADC → gimbal compensation → smoothing → `GimbalAxis::process_value`
//! → throttle hold → activity detection → USB report conversion

use crate::buttons::{Button, TOTAL_BUTTONS};
use crate::expo::{constrain, ExpoLUT};
use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER, NBR_OF_GIMBAL_AXIS};
use crate::mapping::{
    BUTTON_FRONT_LEFT_EXTRA, BUTTON_FRONT_LEFT_LOWER, BUTTON_FRONT_LEFT_UPPER,
    BUTTON_FRONT_RIGHT_EXTRA,
};
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};

// ==================== AXIS CONSTANTS ====================

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;

/// Digital smoothing parameters for `DynamicSmootherEcoI32` filters.
/// Reduce ADC noise and jitter for stable axis values.
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;

// ==================== AXIS STRUCTS ====================

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

impl Default for GimbalAxis {
    fn default() -> Self {
        GimbalAxis {
            value: AXIS_CENTER,
            value_before_hold: AXIS_CENTER,
            previous_value: AXIS_CENTER,
            max: ADC_MAX,
            min: ADC_MIN,
            center: AXIS_CENTER,
            deadzone: (100, 50, 100),
            expo: true,
            hold: AXIS_CENTER,
            hold_pending: false,
        }
    }
}

impl GimbalAxis {
    /// Create a new GimbalAxis with default settings.
    pub fn new() -> Self {
        Self::default()
    }

    /// Create a new GimbalAxis with calibration data.
    pub fn with_calibration(min: u16, max: u16, center: u16) -> Self {
        let mut axis = Self::new();
        axis.set_calibration(min, max, center);
        axis
    }

    /// Apply calibration data to the axis.
    pub fn set_calibration(&mut self, min: u16, max: u16, center: u16) {
        self.min = min;
        self.max = max;
        self.center = center;
    }

    /// Process filtered ADC value through calibration, deadzone and optional expo.
    pub fn process_value(&mut self, filtered_value: i32, expo_lut: &ExpoLUT) {
        // Convert filtered value to u16 range
        let raw_value = constrain(filtered_value, ADC_MIN as i32, ADC_MAX as i32) as u16;

        // Apply calibration and expo processing
        self.value = calculate_axis_value(
            raw_value,
            self.min,
            self.max,
            self.center,
            self.deadzone,
            self.expo,
            expo_lut,
        );
        self.value_before_hold = self.value;
    }

    /// Set/arm a hold value for the axis (used by throttle hold).
    pub fn set_hold(&mut self, value: u16) {
        self.hold = value;
        self.hold_pending = true;
    }

    /// Check if the axis value changed since the last probe (activity flag).
    pub fn check_activity(&mut self) -> bool {
        let activity = self.value != self.previous_value;
        self.previous_value = self.value;
        activity
    }

    /// Throttle‑hold remapping for throttle axis (center → hold; below/above remapped).
    pub fn process_throttle_hold(&mut self, throttle_hold_enable: bool) {
        if throttle_hold_enable && self.value < AXIS_CENTER && !self.hold_pending {
            self.value = remap(self.value, ADC_MIN, AXIS_CENTER, ADC_MIN, self.hold);
        } else if throttle_hold_enable && self.value > AXIS_CENTER && !self.hold_pending {
            self.value = remap(self.value, AXIS_CENTER, ADC_MAX, self.hold, ADC_MAX);
        } else if throttle_hold_enable && self.value == AXIS_CENTER {
            self.value = self.hold;
            self.hold_pending = false;
        } else if throttle_hold_enable {
            self.value = self.hold;
        }
    }
}

#[derive(Copy, Clone)]
pub struct VirtualAxis {
    pub value: u16,
    step: u16,
}

impl Default for VirtualAxis {
    fn default() -> Self {
        VirtualAxis {
            value: AXIS_CENTER,
            step: 5,
        }
    }
}

impl VirtualAxis {
    pub fn new(step: u16) -> Self {
        VirtualAxis {
            value: AXIS_CENTER,
            step,
        }
    }

    /// Update virtual axis based on button inputs.
    /// Returns true if USB activity should be signaled.
    pub fn update(&mut self, up_pressed: bool, down_pressed: bool, _vt_enable: bool) -> bool {
        let mut activity = false;

        // Compensate value when changing direction
        if up_pressed && !down_pressed && self.value < AXIS_CENTER {
            self.value = AXIS_CENTER + (AXIS_CENTER - self.value) / 2;
        } else if down_pressed && !up_pressed && self.value > AXIS_CENTER {
            self.value = AXIS_CENTER - (self.value - AXIS_CENTER) / 2;
        }

        // Move virtual axis
        if up_pressed && !down_pressed && self.value < ADC_MAX - self.step {
            self.value += self.step;
            activity = true;
        } else if down_pressed && !up_pressed && self.value > ADC_MIN + self.step {
            self.value -= self.step;
            activity = true;
        } else if (self.value != AXIS_CENTER && !up_pressed && !down_pressed)
            || (up_pressed && down_pressed)
        {
            // Return to center when no buttons are pressed or both are pressed
            let before = self.value;
            if self.value > AXIS_CENTER {
                self.value = self.value.saturating_sub(self.step);
            } else if self.value < AXIS_CENTER {
                self.value = self.value.saturating_add(self.step);
            }
            activity |= self.value != before;
        }

        activity
    }
}

// ==================== AXIS MANAGER ====================

pub struct AxisManager {
    pub axes: [GimbalAxis; NBR_OF_GIMBAL_AXIS],
    pub virtual_ry: VirtualAxis,
    pub virtual_rz: VirtualAxis,
    pub gimbal_mode: u8,
    pub throttle_hold_enable: bool,
}

impl Default for AxisManager {
    fn default() -> Self {
        Self::new()
    }
}

impl AxisManager {
    pub fn new() -> Self {
        Self {
            axes: [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS],
            virtual_ry: VirtualAxis::new(5),
            virtual_rz: VirtualAxis::new(5),
            gimbal_mode: GIMBAL_MODE_M10,
            throttle_hold_enable: false,
        }
    }

    pub fn set_gimbal_mode(&mut self, mode: u8) {
        self.gimbal_mode = mode;
    }

    /// Apply gimbal mode compensation to raw ADC values.
    pub fn apply_gimbal_compensation(&self, raw_values: &mut [u16; 4]) {
        if self.gimbal_mode == GIMBAL_MODE_M10 {
            // Invert X1 and Y2 axis (M10 gimbals)
            raw_values[GIMBAL_AXIS_LEFT_X] = ADC_MAX - raw_values[GIMBAL_AXIS_LEFT_X];
            raw_values[GIMBAL_AXIS_RIGHT_Y] = ADC_MAX - raw_values[GIMBAL_AXIS_RIGHT_Y];
        } else if self.gimbal_mode == GIMBAL_MODE_M7 {
            // Invert Y1 and X2 axis (M7 gimbals)
            raw_values[GIMBAL_AXIS_LEFT_Y] = ADC_MAX - raw_values[GIMBAL_AXIS_LEFT_Y];
            raw_values[GIMBAL_AXIS_RIGHT_X] = ADC_MAX - raw_values[GIMBAL_AXIS_RIGHT_X];
        }
    }

    /// Tick smoothing filters with latest raw samples.
    pub fn update_smoothers(
        &self,
        smoother: &mut [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
        raw_values: &[u16; 4],
    ) {
        smoother[GIMBAL_AXIS_LEFT_X].tick(raw_values[GIMBAL_AXIS_LEFT_X] as i32);
        smoother[GIMBAL_AXIS_LEFT_Y].tick(raw_values[GIMBAL_AXIS_LEFT_Y] as i32);
        smoother[GIMBAL_AXIS_RIGHT_X].tick(raw_values[GIMBAL_AXIS_RIGHT_X] as i32);
        smoother[GIMBAL_AXIS_RIGHT_Y].tick(raw_values[GIMBAL_AXIS_RIGHT_Y] as i32);
    }

    /// Update all axes from smoothers (calibration, deadzone, expo) and apply holds.
    pub fn process_axis_values(
        &mut self,
        smoother: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
        expo_lut: &ExpoLUT,
    ) -> bool {
        for (index, axis) in self.axes.iter_mut().enumerate() {
            axis.process_value(smoother[index].value(), expo_lut);
        }
        self.process_throttle_hold();
        self.check_activity()
    }

    /// Apply throttle‑hold to the left‑Y axis and derive enable flag.
    pub fn process_throttle_hold(&mut self) {
        self.throttle_hold_enable = self.axes[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER;
        self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold(self.throttle_hold_enable);
    }

    /// Clear throttle‑hold state for left‑Y axis (reset to initial state).
    pub fn clear_throttle_hold(&mut self) {
        self.axes[GIMBAL_AXIS_LEFT_Y].hold = AXIS_CENTER;
        self.axes[GIMBAL_AXIS_LEFT_Y].hold_pending = false;
    }

    /// Update virtual axes based on button inputs.
    /// Returns true if USB activity should be signaled.
    pub fn update_virtual_axes(
        &mut self,
        buttons: &[Button; TOTAL_BUTTONS],
        vt_enable: bool,
    ) -> bool {
        let mut activity = false;

        // Update Virtual RY
        let ry_activity = self.virtual_ry.update(
            buttons[BUTTON_FRONT_LEFT_UPPER].pressed,
            buttons[BUTTON_FRONT_LEFT_LOWER].pressed,
            vt_enable,
        );
        if ry_activity {
            activity = true;
        }

        // Update Virtual RZ
        let rz_activity = self.virtual_rz.update(
            buttons[BUTTON_FRONT_RIGHT_EXTRA].pressed,
            buttons[BUTTON_FRONT_LEFT_EXTRA].pressed,
            vt_enable,
        );
        if rz_activity {
            activity = true;
        }

        activity
    }

    /// Check for axis activity (movement from previous value).
    pub fn check_activity(&mut self) -> bool {
        let mut activity = false;

        for axis in self.axes.iter_mut() {
            if axis.check_activity() {
                activity = true;
            }
        }

        activity
    }

    /// Set throttle‑hold value for left‑Y axis (original behavior).
    pub fn set_throttle_hold(&mut self, hold_value: u16) {
        self.axes[GIMBAL_AXIS_LEFT_Y].set_hold(hold_value);
    }

    /// Get pre‑hold value for special button combinations (original logic).
    pub fn get_value_before_hold(&self) -> u16 {
        // Original code captured axis.value BEFORE throttle hold was applied
        self.axes[GIMBAL_AXIS_LEFT_Y].value_before_hold
    }

    /// Get virtual RY axis value for joystick report.
    pub fn get_virtual_ry_value(&self, expo_lut: &ExpoLUT) -> u16 {
        calculate_axis_value(
            self.virtual_ry.value,
            ADC_MIN,
            ADC_MAX,
            AXIS_CENTER,
            (0, 0, 0),
            true,
            expo_lut,
        )
    }

    /// Get virtual RZ axis value for joystick report.
    pub fn get_virtual_rz_value(&self, expo_lut: &ExpoLUT) -> u16 {
        calculate_axis_value(
            self.virtual_rz.value,
            ADC_MIN,
            ADC_MAX,
            AXIS_CENTER,
            (0, 0, 0),
            true,
            expo_lut,
        )
    }

    /// Initialize digital smoothing filters for each gimbal axis.
    /// Creates an array of `DynamicSmootherEcoI32` filters configured with shared DSP parameters.
    pub fn create_smoothers() -> [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),
        ]
    }
}

// ==================== AXIS PROCESSING FUNCTIONS ====================

/// Remap a value from one range to another using integer math and clamping.
///
/// # 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
pub 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
}

/// Calculate calibrated axis value with deadzone and optional expo.
///
/// Process raw input through [min, center, max] calibration, apply deadzones,
/// then apply expo via LUT if enabled for smooth response.
///
/// # Arguments
/// * `value` - Raw axis value to process
/// * `min` - Lower bound of the axis calibrated range
/// * `max` - Upper bound of the axis calibrated range
/// * `center` - Center position of the axis calibrated range
/// * `deadzone` - Deadzone settings (min, center, max) for axis
/// * `expo` - Whether exponential curve is enabled for this axis
/// * `expo_lut` - Exponential curve lookup table for smooth response
///
/// # Returns
/// Processed axis value after calibration, deadzone, and optional expo
pub fn calculate_axis_value(
    value: u16,
    min: u16,
    max: u16,
    center: u16,
    deadzone: (u16, u16, u16),
    expo: bool,
    expo_lut: &ExpoLUT,
) -> u16 {
    use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER};

    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.apply(calibrated_value);
    }

    calibrated_value
}

// ==================== TESTS ====================

#[cfg(all(test, feature = "std"))]
mod tests {
    use super::*;

    #[test]
    fn test_gimbal_axis_default() {
        let axis = GimbalAxis::default();
        assert_eq!(axis.value, AXIS_CENTER);
        assert_eq!(axis.min, ADC_MIN);
        assert_eq!(axis.max, ADC_MAX);
        assert_eq!(axis.center, AXIS_CENTER);
        assert_eq!(axis.hold, AXIS_CENTER);
        assert!(!axis.hold_pending);
    }

    #[test]
    fn test_gimbal_axis_new() {
        let axis = GimbalAxis::new();
        assert_eq!(axis.value, AXIS_CENTER);
        assert_eq!(axis.min, ADC_MIN);
        assert_eq!(axis.max, ADC_MAX);
        assert_eq!(axis.center, AXIS_CENTER);
        assert!(!axis.hold_pending);
    }

    #[test]
    fn test_gimbal_axis_with_calibration() {
        let axis = GimbalAxis::with_calibration(100, 3900, 2000);
        assert_eq!(axis.min, 100);
        assert_eq!(axis.max, 3900);
        assert_eq!(axis.center, 2000);
    }

    #[test]
    fn test_gimbal_axis_activity_detection() {
        let mut axis = GimbalAxis::new();

        // Initially no activity (same as previous)
        assert!(!axis.check_activity());

        // Change value
        axis.value = 3000;
        assert!(axis.check_activity());

        // No change from previous check
        assert!(!axis.check_activity());
    }

    #[test]
    fn test_gimbal_axis_throttle_hold_processing() {
        let mut axis = GimbalAxis::new();
        axis.set_hold(1500); // Set hold value below center

        // Test when not held, no processing occurs
        axis.hold = AXIS_CENTER;
        axis.hold_pending = false;
        axis.value = 1000;
        axis.process_throttle_hold(true);
        assert_eq!(axis.value, 1000); // Should remain unchanged

        // Test when held but hold_pending = false, remapping occurs
        axis.set_hold(1500);
        axis.value = 1000;
        axis.hold_pending = false; // This allows remapping
        axis.process_throttle_hold(true);
        let expected = remap(1000, ADC_MIN, AXIS_CENTER, ADC_MIN, 1500);
        assert_eq!(axis.value, expected);

        // Test center value gets hold value and clears pending flag
        axis.set_hold(1500);
        axis.value = AXIS_CENTER;
        axis.process_throttle_hold(true);
        assert_eq!(axis.value, 1500);
        assert!(!axis.hold_pending); // Should clear pending flag

        // Test when hold_pending = true, just uses hold value
        axis.set_hold(1500);
        axis.value = 2000;
        axis.hold_pending = true;
        axis.process_throttle_hold(true);
        assert_eq!(axis.value, 1500);
    }

    #[test]
    fn test_virtual_axis_default() {
        let virtual_axis = VirtualAxis::default();
        assert_eq!(virtual_axis.value, AXIS_CENTER);
        assert_eq!(virtual_axis.step, 5);
    }

    #[test]
    fn test_virtual_axis_movement_up() {
        let mut virtual_axis = VirtualAxis::new(10);

        // Test upward movement
        let activity = virtual_axis.update(true, false, false);
        assert!(activity);
        assert_eq!(virtual_axis.value, AXIS_CENTER + 10);
    }

    #[test]
    fn test_virtual_axis_movement_down() {
        let mut virtual_axis = VirtualAxis::new(10);

        // Test downward movement
        let activity = virtual_axis.update(false, true, false);
        assert!(activity);
        assert_eq!(virtual_axis.value, AXIS_CENTER - 10);
    }

    #[test]
    fn test_virtual_axis_return_to_center() {
        let mut virtual_axis = VirtualAxis::new(10);
        virtual_axis.value = AXIS_CENTER + 20;

        // Test return to center when no buttons pressed
        let activity = virtual_axis.update(false, false, false);
        assert!(activity);
        assert_eq!(virtual_axis.value, AXIS_CENTER + 10);
    }

    #[test]
    fn test_virtual_axis_direction_compensation() {
        let mut virtual_axis = VirtualAxis::new(10);
        virtual_axis.value = AXIS_CENTER - 100;

        // Test direction change compensation (includes compensation + movement)
        virtual_axis.update(true, false, false);
        let compensated = AXIS_CENTER + (AXIS_CENTER - (AXIS_CENTER - 100)) / 2;
        let expected = compensated + 10; // Plus step movement
        assert_eq!(virtual_axis.value, expected);
    }

    #[test]
    fn test_axis_manager_creation() {
        let manager = AxisManager::new();
        assert_eq!(manager.axes.len(), NBR_OF_GIMBAL_AXIS);
        assert_eq!(manager.gimbal_mode, GIMBAL_MODE_M10);
        assert_eq!(manager.virtual_ry.value, AXIS_CENTER);
        assert_eq!(manager.virtual_rz.value, AXIS_CENTER);
    }

    #[test]
    fn test_gimbal_compensation_m10() {
        let manager = AxisManager::new(); // Default is M10
        let mut raw_values = [1000, 1500, 2000, 2500];

        manager.apply_gimbal_compensation(&mut raw_values);

        // M10 mode inverts X1 and Y2
        assert_eq!(raw_values[GIMBAL_AXIS_LEFT_X], ADC_MAX - 1000);
        assert_eq!(raw_values[GIMBAL_AXIS_LEFT_Y], 1500); // Not inverted
        assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_X], 2000); // Not inverted
        assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_Y], ADC_MAX - 2500);
    }

    #[test]
    fn test_gimbal_compensation_m7() {
        let mut manager = AxisManager::new();
        manager.set_gimbal_mode(GIMBAL_MODE_M7);
        let mut raw_values = [1000, 1500, 2000, 2500];

        manager.apply_gimbal_compensation(&mut raw_values);

        // M7 mode inverts Y1 and X2
        assert_eq!(raw_values[GIMBAL_AXIS_LEFT_X], 1000); // Not inverted
        assert_eq!(raw_values[GIMBAL_AXIS_LEFT_Y], ADC_MAX - 1500);
        assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_X], ADC_MAX - 2000);
        assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_Y], 2500); // Not inverted
    }

    #[test]
    fn test_axis_activity_detection() {
        let mut manager = AxisManager::new();

        // No activity initially
        assert!(!manager.check_activity());

        // Change value and check activity
        manager.axes[0].value = 1000;
        assert!(manager.check_activity());

        // No activity after previous_value is updated
        assert!(!manager.check_activity());
    }

    #[test]
    fn test_calculate_axis_value_boundaries() {
        let expo_lut = ExpoLUT::new(0.0); // No expo for testing

        // Test min boundary
        let result = calculate_axis_value(0, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
        assert_eq!(result, ADC_MIN);

        // Test max boundary
        let result = calculate_axis_value(4000, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
        assert_eq!(result, ADC_MAX);
    }

    #[test]
    fn test_calculate_axis_value_deadzone() {
        let expo_lut = ExpoLUT::new(0.0); // No expo for testing

        // Test center deadzone
        let result = calculate_axis_value(2000, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
        assert_eq!(result, AXIS_CENTER);
    }

    #[test]
    fn test_remap_function() {
        // Test basic remapping
        let result = remap(50, 0, 100, 0, 200);
        assert_eq!(result, 100);

        // Test reverse remapping (constrain limits to out_min when out_min > out_max)
        let result = remap(25, 0, 100, 100, 0);
        assert_eq!(result, 100);
    }
}