cmdr-joystick/rp2040/src/expo.rs

//! Exponential response curves for joystick axes
//!
//! This module provides a small, allocation‑free way to apply an exponential
//! response to 12‑bit axis values. A lookup table (LUT) is generated once and
//! then used at runtime to map linear input into an expo‑shaped output without
//! floating‑point math.
//!
//! Notes
//! - LUT length is `ADC_MAX + 1` (4096 entries for 12‑bit ADC)
//! - `ExpoLUT::new(factor)` builds the table up front; higher factors increase
//!   curve intensity near the ends and soften around center
//! - `apply_expo_curve` performs a single array lookup with a boundary guard

use crate::{ADC_MAX, ADC_MIN};
use core::cmp::PartialOrd;
use libm::powf;

/// Precomputed exponential lookup table (12‑bit domain).
pub struct ExpoLUT {
    lut: [u16; 4096],
}

impl ExpoLUT {
    /// Build a new lookup table for the provided expo factor.
    ///
    /// Recommended range for `factor` is 0.0 (linear) to 1.0 (strong expo).
    pub fn new(factor: f32) -> Self {
        let lut = generate_expo_lut(factor);
        Self { lut }
    }

    /// Apply the precomputed expo mapping to a single axis value.
    pub fn apply(&self, value: u16) -> u16 {
        apply_expo_curve(value, &self.lut)
    }
}

/// Generate a 12‑bit LUT for an exponential response curve.
///
/// - `expo` in [0.0, 1.0] recommended; values outside that range may exaggerate results.
/// - Output is clamped to `ADC_MIN..=ADC_MAX`.
pub 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
}

/// Apply an expo LUT to an input value.
///
/// If the value exceeds the table length, returns `ADC_MAX` as a guard.
pub fn apply_expo_curve(value: u16, expo_lut: &[u16]) -> u16 {
    if value >= expo_lut.len() as u16 {
        return ADC_MAX;
    }
    expo_lut[value as usize]
}

/// Clamp `value` into the inclusive range `[out_min, out_max]`.
pub 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
    }
}

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

    #[test]
    fn test_generate_expo_lut_boundaries() {
        let lut = generate_expo_lut(0.5);
        assert_eq!(lut[0], ADC_MIN);
        assert_eq!(lut[ADC_MAX as usize], ADC_MAX);
    }

    #[test]
    fn test_generate_expo_lut_center_point() {
        let lut = generate_expo_lut(0.5);
        let center_index = (ADC_MAX / 2) as usize;
        let center_value = lut[center_index];
        assert!((center_value as i32 - AXIS_CENTER as i32).abs() < 50);
    }

    #[test]
    fn test_generate_expo_lut_different_factors() {
        let lut_linear = generate_expo_lut(0.0);
        let lut_expo = generate_expo_lut(1.0);
        let quarter_point = (ADC_MAX / 4) as usize;
        assert_ne!(lut_linear[quarter_point], lut_expo[quarter_point]);
    }

    #[test]
    fn test_apply_expo_curve_no_expo() {
        let lut = generate_expo_lut(0.0);
        let input_value = 1000u16;
        let result = apply_expo_curve(input_value, &lut);
        // With no expo (0.0 factor), should be close to linear
        assert!((result as i32 - input_value as i32).abs() < 50);
    }

    #[test]
    fn test_apply_expo_curve_with_expo() {
        let lut_linear = generate_expo_lut(0.0);
        let lut_expo = generate_expo_lut(0.5);
        let test_value = 1000u16;

        let result_linear = apply_expo_curve(test_value, &lut_linear);
        let result_expo = apply_expo_curve(test_value, &lut_expo);

        assert_ne!(result_linear, result_expo);
    }

    #[test]
    fn test_apply_expo_curve_center_unchanged() {
        let lut = generate_expo_lut(0.5);
        let result = apply_expo_curve(AXIS_CENTER, &lut);
        // Center point should remain close to center
        assert!((result as i32 - AXIS_CENTER as i32).abs() < 50);
    }

    #[test]
    fn test_constrain_within_range() {
        assert_eq!(constrain(50u16, 0u16, 100u16), 50u16);
    }

    #[test]
    fn test_constrain_above_range() {
        assert_eq!(constrain(150u16, 0u16, 100u16), 100u16);
    }

    #[test]
    fn test_constrain_below_range() {
        assert_eq!(constrain(0u16, 50u16, 100u16), 50u16);
    }

    #[test]
    fn test_expo_integration_real_world_values() {
        let lut = generate_expo_lut(0.3);
        let test_values = [500u16, 1000u16, 2000u16, 3000u16];

        for &value in &test_values {
            let result = apply_expo_curve(value, &lut);
            assert!(result >= ADC_MIN);
            assert!(result <= ADC_MAX);
        }
    }
}