NaxGCC-FW/src/filter.rs

use defmt::Format;
use libm::{fminf, powf};

use crate::input::{ControllerConfig, Stick};

/// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains`
pub const FILTER_GAINS: FilterGains = FilterGains {
    max_stick: 100.,
    x_vel_decay: 0.1,
    y_vel_decay: 0.1,
    x_vel_pos_factor: 0.01,
    y_vel_pos_factor: 0.01,
    x_vel_damp: 0.125,
    y_vel_damp: 0.125,
    vel_thresh: 1.,
    accel_thresh: 3.,
    x_smoothing: 0.0,
    y_smoothing: 0.0,
    c_xsmoothing: 0.0,
    c_ysmoothing: 0.0,
};

#[derive(Debug, Clone, Default)]
pub struct WaveshapingValues {
    pub old_x_pos: f32,
    pub old_y_pos: f32,
    pub old_x_vel: f32,
    pub old_y_vel: f32,
    pub old_x_out: f32,
    pub old_y_out: f32,
}

fn calc_waveshaping_mult(setting: u8) -> f32 {
    if setting > 0 && setting <= 5 {
        1. / (440. - 40. * setting as f32)
    } else if setting > 5 && setting <= 15 {
        1. / (340. - 20. * setting as f32)
    } else {
        0.
    }
}

fn vel_damp_from_snapback(snapback: i8) -> f32 {
    match snapback {
        a if a >= 0 => 0.125 * powf(2., (snapback - 4) as f32 / 3.0),
        _ => 1. - 0.25 * powf(2., (snapback + 4) as f32 / 3.0),
    }
}

#[derive(Clone, Debug, Default, Format)]
pub struct FilterGains {
    /// What's the max stick distance from the center
    pub max_stick: f32,
    /// filtered velocity terms
    /// how fast the filtered velocity falls off in the absence of stick movement.
    /// Probably don't touch this.
    pub x_vel_decay: f32, //0.1 default for 1.2ms timesteps, larger for bigger timesteps
    pub y_vel_decay: f32,
    /// how much the current position disagreement impacts the filtered velocity.
    /// Probably don't touch this.
    pub x_vel_pos_factor: f32, //0.01 default for 1.2ms timesteps, larger for bigger timesteps
    pub y_vel_pos_factor: f32,
    /// how much to ignore filtered velocity when computing the new stick position.
    /// DO CHANGE THIS
    /// Higher gives shorter rise times and slower fall times (more pode, less snapback)
    pub x_vel_damp: f32, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps
    pub y_vel_damp: f32,
    /// speed and accel thresholds below which we try to follow the stick better
    /// These may need tweaking according to how noisy the signal is
    /// If it's noisier, we may need to add additional filtering
    /// If the timesteps are *really small* then it may need to be increased to get
    ///   above the noise floor. Or some combination of filtering and playing with
    ///   the thresholds.
    pub vel_thresh: f32, //1 default for 1.2ms timesteps, larger for bigger timesteps
    pub accel_thresh: f32, //5 default for 1.2ms timesteps, larger for bigger timesteps
    /// This just applies a low-pass filter.
    /// The purpose is to provide delay for single-axis ledgedashes.
    /// Must be between 0 and 1. Larger = more smoothing and delay.
    pub x_smoothing: f32,
    pub y_smoothing: f32,
    /// Same thing but for C-stick
    pub c_xsmoothing: f32,
    pub c_ysmoothing: f32,
}

impl FilterGains {
    /// Returns filter gains for 1000Hz polling rate
    pub fn normalize_gains(&self, controller_config: &ControllerConfig) -> Self {
        let mut gains = self.clone();

        gains.x_vel_damp = vel_damp_from_snapback(controller_config.x_snapback);
        gains.y_vel_damp = vel_damp_from_snapback(controller_config.y_snapback);

        gains.x_smoothing = controller_config.x_smoothing as f32 / 10.;
        gains.y_smoothing = controller_config.y_smoothing as f32 / 10.;

        gains.c_xsmoothing = controller_config.c_xsmoothing as f32 / 10.;
        gains.c_ysmoothing = controller_config.c_ysmoothing as f32 / 10.;

        // The below is assuming the sticks to be polled at 1000Hz
        let time_factor = 1.0 / 1.2;
        let time_divisor = 1.2 / 1.0;

        let vel_thresh = 1.0 / (gains.vel_thresh * time_factor);
        let accel_thresh = 1.0 / (gains.accel_thresh * time_factor);

        FilterGains {
            max_stick: gains.max_stick * gains.max_stick,
            x_vel_decay: gains.x_vel_decay * time_factor,
            y_vel_decay: gains.y_vel_decay * time_factor,
            x_vel_pos_factor: gains.x_vel_pos_factor * time_factor,
            y_vel_pos_factor: gains.y_vel_pos_factor * time_factor,
            x_vel_damp: gains.x_vel_damp
                * match controller_config.x_snapback {
                    a if a >= 0 => time_factor,
                    _ => 1.0,
                },
            y_vel_damp: gains.y_vel_damp
                * match controller_config.y_snapback {
                    a if a >= 0 => time_factor,
                    _ => 1.0,
                },
            vel_thresh,
            accel_thresh,
            x_smoothing: powf(1.0 - gains.x_smoothing, time_divisor),
            y_smoothing: powf(1.0 - gains.y_smoothing, time_divisor),
            c_xsmoothing: powf(1.0 - gains.c_xsmoothing, time_divisor),
            c_ysmoothing: powf(1.0 - gains.c_ysmoothing, time_divisor),
        }
    }
}

pub fn run_kalman(
    x_z: f32,
    y_z: f32,
    controller_config: &ControllerConfig,
    filter_gains: &FilterGains,
) -> (f32, f32) {
    todo!()
}

/// This simulates an idealized sort of pode:
///
///  if the stick is moving fast, it responds poorly, while
///  if the stick is moving slowly, it follows closely.
///
/// It's not suitable to be the sole filter, but when put after
///  the smart snapback filter, it should be able to hold the
///  output at the rim longer when released.
///
/// Output is a tuple of the x and y positions.
pub fn run_waveshaping(
    x_pos: f32,
    y_pos: f32,
    x_waveshaping: u8,
    y_waveshaping: u8,
    waveshaping_values: &mut WaveshapingValues,
    filter_gains: &FilterGains,
) -> (f32, f32) {
    let x_factor = calc_waveshaping_mult(x_waveshaping);
    let y_factor = calc_waveshaping_mult(y_waveshaping);

    let x_vel = x_pos - waveshaping_values.old_x_pos;
    let y_vel = y_pos - waveshaping_values.old_y_pos;

    let x_vel_smooth = 0.5 * (x_vel + waveshaping_values.old_x_vel);
    let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_y_vel);

    let old_x_pos_weight = fminf(
        1.,
        x_vel_smooth * x_vel_smooth * filter_gains.vel_thresh * x_factor,
    );
    let new_x_pos_weight = 1. - old_x_pos_weight;
    let old_y_pos_weight = fminf(
        1.,
        y_vel_smooth * y_vel_smooth * filter_gains.vel_thresh * y_factor,
    );
    let new_y_pos_weight = 1. - old_y_pos_weight;

    let x_out = x_pos * new_x_pos_weight + waveshaping_values.old_x_out * old_x_pos_weight;
    let y_out = y_pos * new_y_pos_weight + waveshaping_values.old_y_out * old_y_pos_weight;

    waveshaping_values.old_x_pos = x_pos;
    waveshaping_values.old_y_pos = y_pos;
    waveshaping_values.old_x_vel = x_vel_smooth;
    waveshaping_values.old_y_vel = y_vel_smooth;
    waveshaping_values.old_x_out = x_out;
    waveshaping_values.old_y_out = y_out;

    (x_out, y_out)
}