//! Calibration management for CMDR Joystick
//!
//! Responsibilities
//! - Manage the calibration lifecycle (start/active/stop)
//! - Track min/max dynamically from smoothed ADC samples
//! - Select gimbal mode (M10/M7) during calibration and reset centers
//! - Persist per‑axis calibration and gimbal mode to EEPROM
//! - Load persisted settings at boot with resilient defaults
//!
//! The manager is intentionally stateful but small. It isolates storage access
//! behind simple `read_fn`/`write_fn` closures so the code can be unit‑tested on
//! a host without hardware.

use crate::axis::{GimbalAxis, GIMBAL_MODE_M10, GIMBAL_MODE_M7};
use crate::hardware::NBR_OF_GIMBAL_AXIS;
use crate::storage;
use dyn_smooth::DynamicSmootherEcoI32;

// ==================== CALIBRATION MANAGER ====================

/// Orchestrates runtime calibration and storage interactions.
pub struct CalibrationManager {
    active: bool,
    gimbal_mode: u8,
}

impl CalibrationManager {
    /// Create a new CalibrationManager with default settings.
    pub fn new() -> Self {
        Self {
            active: false,
            gimbal_mode: GIMBAL_MODE_M10,
        }
    }

    /// Check if calibration mode is currently active.
    pub fn is_active(&self) -> bool {
        self.active
    }

    /// Start calibration mode.
    pub fn start_calibration(&mut self) {
        self.active = true;
    }

    /// Stop calibration mode.
    pub fn stop_calibration(&mut self) {
        self.active = false;
    }

    /// Get current gimbal mode.
    pub fn get_gimbal_mode(&self) -> u8 {
        self.gimbal_mode
    }

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

    /// Update dynamic calibration (continuous min/max tracking) while active.
    ///
    /// Only has effect when calibration is active.
    pub fn update_dynamic_calibration(
        &self,
        axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
        smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
    ) {
        if !self.active {
            return;
        }

        for (index, axis) in axes.iter_mut().enumerate() {
            let current_value = smoothers[index].value() as u16;
            if current_value < axis.min {
                axis.min = current_value;
            } else if current_value > axis.max {
                axis.max = current_value;
            }
        }
    }

    /// Set gimbal mode to M10 and reset each axis (min/max/center) to the current smoothed center.
    /// Returns true if mode was changed (only while active).
    pub fn set_gimbal_mode_m10(
        &mut self,
        axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
        smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
    ) -> bool {
        if !self.active {
            return false;
        }

        self.gimbal_mode = GIMBAL_MODE_M10;
        self.reset_axis_calibration(axes, smoothers);
        true
    }

    /// Set gimbal mode to M7 and reset each axis (min/max/center) to the current smoothed center.
    /// Returns true if mode was changed (only while active).
    pub fn set_gimbal_mode_m7(
        &mut self,
        axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
        smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
    ) -> bool {
        if !self.active {
            return false;
        }

        self.gimbal_mode = GIMBAL_MODE_M7;
        self.reset_axis_calibration(axes, smoothers);
        true
    }

    /// Save calibration data to EEPROM and end calibration mode.
    /// Returns true if data was written and calibration ended.
    pub fn save_calibration<F>(
        &mut self,
        axes: &[GimbalAxis; NBR_OF_GIMBAL_AXIS],
        write_fn: &mut F,
    ) -> bool
    where
        F: FnMut(u32, &[u8]) -> Result<(), ()>,
    {
        if !self.active {
            return false;
        }

        // Prepare calibration data array
        let axis_data: [(u16, u16, u16); NBR_OF_GIMBAL_AXIS] = [
            (axes[0].min, axes[0].max, axes[0].center),
            (axes[1].min, axes[1].max, axes[1].center),
            (axes[2].min, axes[2].max, axes[2].center),
            (axes[3].min, axes[3].max, axes[3].center),
        ];

        // Save calibration data to storage
        if storage::write_calibration_data(write_fn, &axis_data, self.gimbal_mode).is_err() {
            return false;
        }

        // End calibration mode
        self.active = false;

        true
    }

    /// Load per‑axis calibration data from EEPROM.
    ///
    /// Updates the provided axes with loaded values; retains defaults on error.
    pub fn load_axis_calibration<F>(axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS], read_fn: &mut F)
    where
        F: FnMut(u32) -> Result<u8, ()>,
    {
        for (index, axis) in axes.iter_mut().enumerate() {
            match storage::read_axis_calibration(read_fn, index) {
                Ok((min, max, center)) => {
                    *axis = GimbalAxis::with_calibration(min, max, center);
                }
                Err(_) => {
                    // Use factory defaults if EEPROM read fails
                    // axis retains its default values from initialization
                }
            }
        }
    }

    /// Load gimbal mode from EEPROM.
    /// Returns the stored mode or M10 default if read fails.
    pub fn load_gimbal_mode<F>(read_fn: &mut F) -> u8
    where
        F: FnMut(u32) -> Result<u8, ()>,
    {
        storage::read_gimbal_mode(read_fn).unwrap_or(GIMBAL_MODE_M10)
    }

    /// Reset each axis calibration to its current smoothed center.
    fn reset_axis_calibration(
        // Recenter all axis calibration values using current smoother readings.
        &self,
        axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
        smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
    ) {
        for (index, axis) in axes.iter_mut().enumerate() {
            let center_value = smoothers[index].value() as u16;
            axis.center = center_value;
            axis.min = center_value;
            axis.max = center_value;
        }
    }
}

impl Default for CalibrationManager {
    fn default() -> Self {
        // Construct a calibration manager using the Default implementation.
        Self::new()
    }
}

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

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

    #[test]
    fn test_calibration_manager_creation() {
        // Report whether calibration is currently active.
        let manager = CalibrationManager::new();
        assert!(!manager.is_active());
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M10);
    }

    #[test]
    fn test_calibration_state_management() {
        // Start and stop transitions should flip the active flag accordingly.
        let mut manager = CalibrationManager::new();

        // Initially inactive
        assert!(!manager.is_active());

        // Start calibration
        manager.start_calibration();
        assert!(manager.is_active());

        // Stop calibration
        manager.stop_calibration();
        assert!(!manager.is_active());
    }

    #[test]
    fn test_gimbal_mode_management() {
        // Manual mode setter should swap between M10 and M7 without side effects.
        let mut manager = CalibrationManager::new();

        // Default mode
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M10);

        // Set M7 mode
        manager.set_gimbal_mode(GIMBAL_MODE_M7);
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M7);

        // Set M10 mode
        manager.set_gimbal_mode(GIMBAL_MODE_M10);
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M10);
    }

    #[test]
    fn test_dynamic_calibration_inactive() {
        // Inactive calibration should ignore updates and leave min/max untouched.
        let manager = CalibrationManager::new();
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        // Create smoothers with proper parameters
        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let smoothers = [
            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),
        ];

        // Set initial values
        axes[0].min = 1000;
        axes[0].max = 3000;

        // Should not update when inactive
        manager.update_dynamic_calibration(&mut axes, &smoothers);
        assert_eq!(axes[0].min, 1000);
        assert_eq!(axes[0].max, 3000);
    }

    #[test]
    fn test_dynamic_calibration_active() {
        // Active calibration should track new lows/highs as smoothed values change.
        let mut manager = CalibrationManager::new();
        manager.start_calibration();

        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        // Create smoothers with proper parameters
        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let mut smoothers = [
            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),
        ];

        // Initialize smoother to a stable value through multiple ticks
        for _ in 0..10 {
            smoothers[0].tick(2000);
        }
        let initial_value = smoothers[0].value() as u16;

        // Set initial values to the smoothed value
        axes[0].min = initial_value;
        axes[0].max = initial_value;

        // Simulate low value with multiple ticks to ensure smoothing
        for _ in 0..10 {
            smoothers[0].tick(1500);
        }
        let low_value = smoothers[0].value() as u16;
        manager.update_dynamic_calibration(&mut axes, &smoothers);
        assert_eq!(axes[0].min, low_value);
        assert_eq!(axes[0].max, initial_value);

        // Simulate high value with multiple ticks to ensure smoothing
        for _ in 0..10 {
            smoothers[0].tick(2500);
        }
        let high_value = smoothers[0].value() as u16;
        manager.update_dynamic_calibration(&mut axes, &smoothers);
        assert_eq!(axes[0].min, low_value);
        assert_eq!(axes[0].max, high_value);
    }

    #[test]
    fn test_mode_selection_inactive() {
        // Mode change commands should fail when calibration mode is not active.
        let mut manager = CalibrationManager::new();
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        // Create smoothers with proper parameters
        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let smoothers = [
            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),
        ];

        let result = manager.set_gimbal_mode_m10(&mut axes, &smoothers);
        assert!(!result); // Should return false because manager is not active
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M10);
    }

    #[test]
    fn test_load_axis_calibration_success() {
        // Successful EEPROM reads should populate the axis calibration tuple.
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        // Mock EEPROM data simulating successful calibration read
        // Storage format uses little-endian: low byte first, then high byte
        let mut read_fn = |addr: u32| {
            // Simulate EEPROM data for first axis: min=1000, max=3000, center=2000
            match addr {
                1 => Ok(232), // min low byte (1000 = 0x03E8, low byte = 232)
                2 => Ok(3),   // min high byte
                3 => Ok(184), // max low byte (3000 = 0x0BB8, low byte = 184)
                4 => Ok(11),  // max high byte
                5 => Ok(208), // center low byte (2000 = 0x07D0, low byte = 208)
                6 => Ok(7),   // center high byte
                _ => Err(()), // Other addresses fail
            }
        };

        CalibrationManager::load_axis_calibration(&mut axes, &mut read_fn);

        // First axis should have loaded calibration values
        assert_eq!(axes[0].min, 1000);
        assert_eq!(axes[0].max, 3000);
        assert_eq!(axes[0].center, 2000);

        // Other axes should retain default values since EEPROM read fails
        assert_eq!(axes[1].min, crate::hardware::ADC_MIN);
        assert_eq!(axes[1].max, crate::hardware::ADC_MAX);
        assert_eq!(axes[1].center, crate::hardware::AXIS_CENTER);
    }

    #[test]
    fn test_load_axis_calibration_failure() {
        // Failed EEPROM reads should leave default calibration values intact.
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];
        let original_axes = axes;

        // Mock EEPROM read that always fails
        let mut read_fn = |_addr: u32| Err(());

        CalibrationManager::load_axis_calibration(&mut axes, &mut read_fn);

        // All axes should retain their original default values
        for (i, axis) in axes.iter().enumerate() {
            assert_eq!(axis.min, original_axes[i].min);
            assert_eq!(axis.max, original_axes[i].max);
            assert_eq!(axis.center, original_axes[i].center);
        }
    }

    #[test]
    fn test_load_gimbal_mode_success() {
        // Reading the stored gimbal mode should return the persisted value.
        // Mock successful EEPROM read for M7 mode
        let mut read_fn = |addr: u32| {
            match addr {
                25 => Ok(GIMBAL_MODE_M7), // Gimbal mode stored at address 25 (EEPROM_DATA_LENGTH)
                _ => Err(()),
            }
        };

        let mode = CalibrationManager::load_gimbal_mode(&mut read_fn);
        assert_eq!(mode, GIMBAL_MODE_M7);
    }

    #[test]
    fn test_load_gimbal_mode_failure() {
        // Read failures should fall back to the default M10 mode.
        // Mock EEPROM read failure
        let mut read_fn = |_addr: u32| Err(());

        let mode = CalibrationManager::load_gimbal_mode(&mut read_fn);
        assert_eq!(mode, GIMBAL_MODE_M10); // Should return default M10
    }

    #[test]
    fn test_update_calibration_inactive() {
        // When calibration is inactive, mode/set/save helpers should be no-ops.
        let mut manager = CalibrationManager::new();
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        // Create smoothers with proper parameters
        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let smoothers = [
            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),
        ];

        let mut write_fn = |_page: u32, _data: &[u8]| Ok(());

        // Test that individual methods return false when inactive
        let mode_result = manager.set_gimbal_mode_m10(&mut axes, &smoothers);
        let save_result = manager.save_calibration(&axes, &mut write_fn);

        assert!(!mode_result); // Should be false because manager is inactive
        assert!(!save_result); // Should be false because manager is inactive
        assert!(!manager.is_active());
    }

    #[test]
    fn test_process_mode_selection_m10_command() {
        // Active M10 command should set mode and recenter axes using smoother values.
        let mut manager = CalibrationManager::new();
        manager.start_calibration();
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let mut smoothers = [
            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),
        ];

        smoothers[0].tick(2000);
        let expected_value = smoothers[0].value() as u16;

        let result = manager.set_gimbal_mode_m10(&mut axes, &smoothers);

        assert!(result);
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M10);
        assert_eq!(axes[0].center, expected_value);
    }

    #[test]
    fn test_process_mode_selection_m7_command() {
        // Active M7 command should likewise set mode and recenter axes.
        let mut manager = CalibrationManager::new();
        manager.start_calibration();
        let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
        const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
        const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;

        let mut smoothers = [
            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),
        ];

        smoothers[1].tick(1800);
        let expected_value = smoothers[1].value() as u16;

        let result = manager.set_gimbal_mode_m7(&mut axes, &smoothers);

        assert!(result);
        assert_eq!(manager.get_gimbal_mode(), GIMBAL_MODE_M7);
        assert_eq!(axes[1].center, expected_value);
    }

    #[test]
    fn test_save_calibration_command() {
        // Successful saves should write EEPROM data and exit calibration mode.
        let mut manager = CalibrationManager::new();
        manager.start_calibration();
        let axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        let mut write_called = false;
        let mut write_fn = |_page: u32, _data: &[u8]| {
            write_called = true;
            Ok(())
        };

        let result = manager.save_calibration(&axes, &mut write_fn);

        assert!(result);
        assert!(write_called);
        assert!(!manager.is_active()); // Should end calibration
    }

    #[test]
    fn test_save_calibration_failure_keeps_active() {
        // Write failures should keep calibration active for retrials.
        let mut manager = CalibrationManager::new();
        manager.start_calibration();
        let axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        let mut write_fn = |_page: u32, _data: &[u8]| Err(());

        let result = manager.save_calibration(&axes, &mut write_fn);

        assert!(!result);
        assert!(manager.is_active());
    }

    #[test]
    fn test_save_calibration_inactive() {
        // Save attempts while inactive should no-op without touching storage.
        let mut manager = CalibrationManager::new(); // Note: not starting calibration
        let axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];

        let mut write_called = false;
        let mut write_fn = |_page: u32, _data: &[u8]| {
            write_called = true;
            Ok(())
        };

        let result = manager.save_calibration(&axes, &mut write_fn);

        assert!(!result); // Should fail because calibration is not active
        assert!(!write_called);
        assert!(!manager.is_active()); // Should remain inactive
    }
}
