dsp.rs

use std::cmp::max;
use std::cmp::min;
use std::i16;

const FILTER_SIZE : usize = 16;

pub struct FilterState {
    // Must use doubles for these arrays, otherwise can be noisy.
    input: [f64; FILTER_SIZE],
    output: [f64; FILTER_SIZE],
    current: u32
}

impl FilterState {
     pub fn new() -> FilterState {
          FilterState {input:[0.0; FILTER_SIZE], output:[0.0; FILTER_SIZE], current:0}
     }
}

#[inline]
fn clamp(input: i32) -> i16
{
     return max(i16::MIN as i32, min(i16::MAX as i32, input)) as i16;
}

#[inline]
fn get_offset(filter_state : &FilterState, relative_offset : i32) -> usize
{
     //#[static_assert] static t: bool = (FILTER_SIZE & (FILTER_SIZE - 1)) == 0;
     return ((filter_state.current + relative_offset as u32) % FILTER_SIZE as u32) as usize;
}

#[inline]
fn push_sample(filter_state : &mut FilterState, sample: i16) {
     filter_state.input[get_offset(filter_state, 0)] = sample as f64;     
     filter_state.current = filter_state.current + 1;    
}

#[inline]
fn get_output_sample(filter_state : &FilterState) -> i16
{
     return clamp(filter_state.output[get_offset(filter_state, 0)] as i32);
}

// This is an implementation of a Chebyshev lowpass filter at 5000hz with ripple -0.50dB,
// 10th order, and for an input sample rate of 44100hz.
#[inline]
fn apply_lowpass_single(filter_state : &mut FilterState)
{    
     //#[static_assert] static t: bool = FILTER_SIZE >= 10;

    //let x = filter_state.input;
     let x = &filter_state.input;
     
     // Note: I didn't understand how to reference y; I couldn't make it work without either errors or silently dropping
     // the result (was copying the array?).
     // let y = &mut filter_state.output;
     // let y = mut filter_state.output;

     filter_state.output[get_offset(filter_state, 0)] =
       (  1.0 * (1.0 / 6.928330802e+06) * (x[get_offset(filter_state, -10)] + x[get_offset(filter_state,  -0)]))
     + ( 10.0 * (1.0 / 6.928330802e+06) * (x[get_offset(filter_state,  -9)] + x[get_offset(filter_state,  -1)]))
     + ( 45.0 * (1.0 / 6.928330802e+06) * (x[get_offset(filter_state,  -8)] + x[get_offset(filter_state,  -2)]))
     + (120.0 * (1.0 / 6.928330802e+06) * (x[get_offset(filter_state,  -7)] + x[get_offset(filter_state,  -3)]))
     + (210.0 * (1.0 / 6.928330802e+06) * (x[get_offset(filter_state,  -6)] + x[get_offset(filter_state,  -4)]))
     + (252.0 * (1.0 / 6.928330802e+06) *  x[get_offset(filter_state,  -5)])

     + (  -0.4441854896 * filter_state.output[get_offset(filter_state, -10)])
     + (   4.2144719035 * filter_state.output[get_offset(filter_state,  -9)])
     + ( -18.5365677633 * filter_state.output[get_offset(filter_state,  -8)])
     + (  49.7394321983 * filter_state.output[get_offset(filter_state,  -7)])
     + ( -90.1491003509 * filter_state.output[get_offset(filter_state,  -6)])
     + ( 115.3235358151 * filter_state.output[get_offset(filter_state,  -5)])
     + (-105.4969191433 * filter_state.output[get_offset(filter_state,  -4)])
     + (  68.1964705422 * filter_state.output[get_offset(filter_state,  -3)])
     + ( -29.8484881821 * filter_state.output[get_offset(filter_state,  -2)])
     + (   8.0012026712 * filter_state.output[get_offset(filter_state,  -1)]);   
}

#[inline]
pub fn apply_lowpass(filter_state: &mut FilterState, input: &[i16], output: &mut [i16], length: i32)
{     
     // Better way to do u32 range?
     for i in 0..length {
          push_sample(filter_state, input[i as usize]);
          apply_lowpass_single(filter_state);
          output[i as usize] = get_output_sample(filter_state);
     }
}