StateVariable.h

/*
 * StateVariable.h
 *
 * This file is part of Mozzi.
 *
 * Copyright 2012-2024 Tim Barrass and the Mozzi Team
 *
 * Mozzi is licensed under the GNU Lesser General Public Licence (LGPL) Version 2.1 or later.
 *
 */
 
#ifndef STATEVARIABLE_H_
#define STATEVARIABLE_H_
 
#include "Arduino.h"
#include "math.h"
#include "meta.h"
#include "mozzi_fixmath.h"
#include "mozzi_utils.h"
#include "ResonantFilter.h"
 
//enum filter_types { LOWPASS, BANDPASS, HIGHPASS, NOTCH };
 
template <int8_t FILTER_TYPE> class StateVariable {
 
public:
  StateVariable() {}
 
  void setResonance(Q0n8 resonance) {
    // qvalue goes from 255 to 0, representing .999 to 0 in fixed point
    // lower q, more resonance
    q = resonance;
    scale = (Q0n8)sqrt((unsigned int)resonance << 8);
  }
 
  void setCentreFreq(unsigned int centre_freq) {
    // simple frequency tuning with error towards nyquist (reference?  where did
    // this come from?)
    // f = (Q1n15)(((Q16n16_2PI*centre_freq)>>AUDIO_RATE_AS_LSHIFT)>>1);
    f = (Q15n16)((Q16n16_2PI * centre_freq) >>
                 (AUDIO_RATE_AS_LSHIFT)); // this works best for now
    // f = (Q15n16)(((Q16n16_2PI*centre_freq)<<(16-AUDIO_RATE_AS_LSHIFT))>>16);
    // // a small shift left and a round 16 right is faster than big
    // non-byte-aligned right in one go float ff =
    // Q16n16_to_float(((Q16n16_PI*centre_freq))>>AUDIO_RATE_AS_LSHIFT); f =
    // float_to_Q15n16(2.0f *sin(ff));
  }
 
  inline int next(int input) {
    // chooses a different next() function depending on whether the
    // filter is declared as LOWPASS, BANDPASS, HIGHPASS or NOTCH.
    // See meta.h.
    return next(input, Int2Type<FILTER_TYPE>());
  }
 
private:
  int low, band;
  Q0n8 q, scale;
  volatile Q15n16 f;
 
  inline int next(int input, Int2Type<LOWPASS>) {
    // setPin13High();
    low += ((f * band) >> 16);
    int high = (((long)input - low - (((long)band * q) >> 8)) * scale) >> 8;
    band += ((f * high) >> 16);
    // int notch = high + low;
    // setPin13Low();
    return low;
  }
 
  inline int next(int input, Int2Type<BANDPASS>) {
    // setPin13High();
    low += ((f * band) >> 16);
    int high = (((long)input - low - (((long)band * q) >> 8)) * scale) >> 8;
    band += ((f * high) >> 16);
    // int notch = high + low;
    // setPin13Low();
    return band;
  }
 
  inline int next(int input, Int2Type<HIGHPASS>) {
    // setPin13High();
    low += ((f * band) >> 16);
    int high = (((long)input - low - (((long)band * q) >> 8)) * scale) >> 8;
    band += ((f * high) >> 16);
    // int notch = high + low;
    // setPin13Low();
    return high;
  }
 
  inline int next(int input, Int2Type<NOTCH>) {
    // setPin13High();
    low += ((f * band) >> 16);
    int high = (((long)input - low - (((long)band * q) >> 8)) * scale) >> 8;
    band += ((f * high) >> 16);
    int notch = high + low;
    // setPin13Low();
    return notch;
  }
};
 
#endif /* STATEVARIABLE_H_ */