filter.cpp

#include <circle/logger.h>
#include <math.h>
#include "filter.h"

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795f
#endif

static const char FromFilter[] = "filter";

CFilter::CFilter (float sampleRate)
: m_sampleRate(sampleRate),
  m_oneOverSampleRate(1.0f/sampleRate),
  m_cutoff(0.0f)
{
	m_LPF1.setSampleRate(m_sampleRate);
    m_LPF2.setSampleRate(m_sampleRate);
    m_LPF3.setSampleRate(m_sampleRate);
    m_LPF4.setSampleRate(m_sampleRate);
    
	// init
	m_k       = 0.0f;
	m_alpha_0 = 1.0f;
	m_a       = 0.0f;
	m_b       = 0.0f;
	m_c       = 0.0f;
	m_d       = 0.0f;

	// set all as LPF types
	m_LPF1.setLP();
	m_LPF2.setLP();
	m_LPF3.setLP();
	m_LPF4.setLP();

	// set default filter type
	m_filter_type = FILTERTYPE::LP4;

	// flush everything
	Reset();
}

void CFilter::Reset()
{
	// flush everything
	m_LPF1.reset();
	m_LPF2.reset();
	m_LPF3.reset();
	m_LPF4.reset();
}

void CFilter::SetCutoff(float cut)
{
	m_cutoff = (cut<10.0f) ? 10.0f : (cut>20000.0f) ? 20000.0f : cut;
	
    // prewarp for BZT
    float wd = 2.0f * M_PI * m_cutoff;
    //float T = 1 / m_sampleRate;

    //note: measured input to tan function, it seemed limited to (0.005699, 1.282283). 
    //input for fasttan shall be limited to (-pi/2, pi/2) according to documentation
    float wa = (2 * m_sampleRate) * fast_tanf(wd * m_oneOverSampleRate * 0.5f);
    float g = wa * m_oneOverSampleRate * 0.5f;

    // G - the feedforward coeff in the VA One Pole
    //     same for LPF, HPF
    float G = g / (1.0f + g);

    // set alphas
    m_LPF1.m_alpha = G;
    m_LPF2.m_alpha = G;
    m_LPF3.m_alpha = G;
    m_LPF4.m_alpha = G;

    // set betas
    m_LPF1.m_beta = G * G * G / (1.0f + g);
    m_LPF2.m_beta = G * G / (1.0f + g);
    m_LPF3.m_beta = G / (1.0f + g);
    m_LPF4.m_beta = 1.0f / (1.0f + g);

    m_gamma = G * G * G * G; // G^4

    m_alpha_0 = 1.0f / (1.0f + m_k * m_gamma);

    // Oberheim variation
    switch (m_filter_type) {
		case FILTERTYPE::LP4:
							  m_a = 0.0;
							  m_b = 0.0;
							  m_c = 0.0;
							  m_d = 0.0;
							  m_e = 1.0;
							  break;

		case FILTERTYPE::LP2:
							  m_a = 0.0;
							  m_b = 0.0;
							  m_c = 1.0;
							  m_d = 0.0;
							  m_e = 0.0;
							  break;

		case FILTERTYPE::BP4:
							  m_a = 0.0;
							  m_b = 0.0;
							  m_c = 4.0;
							  m_d = -8.0;
							  m_e = 4.0;
							  break;

		case FILTERTYPE::BP2:
							  m_a = 0.0;
							  m_b = 2.0;
							  m_c = -2.0;
							  m_d = 0.0;
							  m_e = 0.0;
							  break;

		case FILTERTYPE::HP4:
							  m_a = 1.0;
							  m_b = -4.0;
							  m_c = 6.0;
							  m_d = -4.0;
							  m_e = 1.0;
							  break;

		case FILTERTYPE::HP2:
							  m_a = 1.0;
							  m_b = -2.0;
							  m_c = 1.0;
							  m_d = 0.0;
							  m_e = 0.0;
							  break;

		default: // LP4
							  m_a = 0.0;
							  m_b = 0.0;
							  m_c = 0.0;
							  m_d = 0.0;
							  m_e = 1.0;
							  break;
    }
}

void CFilter::SetResonance(float res)
{		
	m_k = 3.88 * res;
}

float CFilter::Process(float xn)
{
    float dSigma = m_LPF1.getFeedbackOutput() + m_LPF2.getFeedbackOutput() +
                    m_LPF3.getFeedbackOutput() + m_LPF4.getFeedbackOutput();

    // calculate input to first filter
    float dU = (xn - m_k * dSigma) * m_alpha_0;

    // --- cascade of 4 filters
    float dLP1 = m_LPF1.process(dU);
    float dLP2 = m_LPF2.process(dLP1);
    float dLP3 = m_LPF3.process(dLP2);
    float dLP4 = m_LPF4.process(dLP3);

    // --- Oberheim variations
    float output = m_a * dU + m_b * dLP1 + m_c * dLP2 + m_d * dLP3 + m_e * dLP4;

    return output;
}

float CFilter::fast_tanf(float x)
{
	float x2 = x*x;
	
	return x * (1.0f + 0.333333333f * x2) / (1.0f - 0.333333333f * x2);
}