/**
 * Copyright 2022 Colin Lam, Ploopy Corporation
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * SPECIAL THANKS TO:
 * Robert Bristow-Johnson, a.k.a. RBJ
 * for his exceptional work on biquad formulae as applied to digital
 * audio filtering, summarised in his pamphlet, "Audio EQ Cookbook".
 */

#include <math.h>
#include <stdio.h>

#include "bqf.h"

int filter_stages = 0;
bqf_coeff_t bqf_filters_left[MAX_FILTER_STAGES];
bqf_coeff_t bqf_filters_right[MAX_FILTER_STAGES];
bqf_mem_t bqf_filters_mem_left[MAX_FILTER_STAGES];
bqf_mem_t bqf_filters_mem_right[MAX_FILTER_STAGES];

/**
 * Configure a low-pass filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. This is hard to explain. If you want to sound smart,
 * though, just start saying things like "Linkwitz-Riley filters are superior
 * due to their multi-stage flat summations to unity gain". Some example
 * values for this:
 *  - 1/sqrt(2). A "Butterworth" filter. Use this by default; it results in
 *    maximally-flat passband.
 *  - 1/sqrt(3). A "Bessel" filter. Results in maximally-flat group delay.
 *  - 1/2. A "Linkwitz-Riley" filter. Used for sounding smart. Optionally,
 *    used to make lowpass and highpass sections that sum flat to unity gain.
 */
void bqf_lowpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = (1.0 - cosw0) / 2.0;
    double b1 = 1.0 - cosw0;
    double b2 = (1.0 - cosw0) / 2.0;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a high-pass filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. This is hard to explain. If you want to sound smart,
 * though, just start saying things like "Linkwitz-Riley filters are superior
 * due to their multi-stage flat summations to unity gain". Some example
 * values for this:
 *  - 1/sqrt(2). A "Butterworth" filter. Use this by default; it results in
 *    maximally-flat passband.
 *  - 1/sqrt(3). A "Bessel" filter. Results in maximally-flat group delay.
 *  - 1/2. A "Linkwitz-Riley" filter. Used for sounding smart. Optionally,
 *    used to make lowpass and highpass sections that sum flat to unity gain.
 */
void bqf_highpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = (1.0 + cosw0) / 2.0;
    double b1 = -(1.0 + cosw0);
    double b2 = (1.0 + cosw0) / 2.0;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a band-pass filter, with constant skirt gain - which has a peak
 * gain of Q. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. It defines how aggressive the band pass attenuates
 * from the centre frequency. Some example values for Q:
 *  - sqrt(2) is 1 octave wide
 */
void bqf_bandpass_skirt_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = sinw0 / 2.0;
    double b1 = 0.0;
    double b2 = -sinw0 / 2.0;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a band-pass filter, with constant peak gain of 0 dB. Parameters
 * are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. It defines how aggressive the band pass attenuates
 * from the centre frequency. Some example values for Q:
 *  - sqrt(2) is 1 octave wide
 */
void bqf_bandpass_peak_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = alpha;
    double b1 = 0.0;
    double b2 = -alpha;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a notch filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. It defines how aggressive the notch attenuates
 * from the centre frequency. Some example values for Q:
 *  - sqrt(2) is 1 octave wide
 */
void bqf_notch_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = 1.0;
    double b1 = -2.0 * cosw0;
    double b2 = 1.0;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure an allpass filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * Q: The quality factor. I don't actually know what this is for an allpass.
 * Try experimenting. Why do I have to do all the work?
 */
void bqf_allpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) {
    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = 1.0 - alpha;
    double b1 = -2.0 * cosw0;
    double b2 = 1.0 + alpha;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - alpha;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a peaking filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * dBgain: The gain at the centre frequency, in dB. Positive for boost,
 * negative for cut.
 *
 * Q: The quality factor. It defines the bandwidth from the centre frequency.
 * For the purposes of this filter, the bandwidth is defined using the points
 * on the curve at which the gain in dB is half of the peak gain. Some
 * example values for Q:
 *  - sqrt(2) is 1 octave wide
 */
void bqf_peaking_config(double fs, double f0, double dBgain, double Q,
        bqf_coeff_t *coefficients) {

    double A = pow(10.0, (dBgain/40));

    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double b0 = 1.0 + (alpha * A);
    double b1 = -2.0 * cosw0;
    double b2 = 1.0 - (alpha * A);
    double a0 = 1.0 + (alpha / A);
    double a1 = -2.0 * cosw0;
    double a2 = 1.0 - (alpha / A);

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a low-shelf filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * dBgain: The gain at the centre frequency, in dB. Positive for boost,
 * negative for cut.
 *
 * Q: The quality factor. It defines the steepness of the shelf's slope. I
 * don't actually know what this is for a shelf filter. Try experimenting.
 * Why do I have to do all the work?
 */
void bqf_lowshelf_config(double fs, double f0, double dBgain, double Q,
        bqf_coeff_t *coefficients) {

    double A = pow(10.0, (dBgain/40));

    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double trAa = 2 * sqrt(A) * alpha;

    double b0 = A * ((A + 1) - ((A - 1) * cosw0) + trAa);
    double b1 = 2 * A * ((A - 1) - ((A + 1) * cosw0));
    double b2 = A * ((A + 1) - ((A - 1) * cosw0) - trAa);
    double a0 = (A + 1) + ((A - 1) * cosw0) + trAa;
    double a1 = -2 * ((A - 1) + ((A + 1) * cosw0));
    double a2 = (A + 1) + ((A - 1) * cosw0) - trAa;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

/**
 * Configure a high-shelf filter. Parameters are as follows:
 *
 * fs: The sampling frequency. This is usually defined for you by
 * SAMPLING_FREQ in run.h. It's the sampling frequency of the DAC on the
 * board.
 *
 * f0: The centre frequency. this is where the signal starts getting
 * attenuated.
 *
 * dBgain: The gain at the centre frequency, in dB. Positive for boost,
 * negative for cut.
 *
 * Q: The quality factor. It defines the steepness of the shelf's slope. I
 * don't actually know what this is for a shelf filter. Try experimenting.
 * Why do I have to do all the work?
 */
void bqf_highshelf_config(double fs, double f0, double dBgain, double Q,
        bqf_coeff_t *coefficients) {

    double A = pow(10.0, (dBgain/40));

    double w0 = 2.0 * M_PI * f0 / fs;

    double cosw0 = cos(w0);
    double sinw0 = sin(w0);
    double alpha = sinw0 / (2.0 * Q);

    double trAa = 2 * sqrt(A) * alpha;

    double b0 = A * ((A + 1) + ((A - 1) * cosw0) + trAa);
    double b1 = -2 * A * ((A - 1) + ((A + 1) * cosw0));
    double b2 = A * ((A + 1) + ((A - 1) * cosw0) - trAa);
    double a0 = (A + 1) - ((A - 1) * cosw0) + trAa;
    double a1 = 2 * ((A - 1) - ((A + 1) * cosw0));
    double a2 = (A + 1) - ((A - 1) * cosw0) - trAa;

    // Normalise all values to a0
    b0 = b0 / a0;
    b1 = b1 / a0;
    b2 = b2 / a0;
    a1 = a1 / a0;
    a2 = a2 / a0;
    a0 = 1.0;

    coefficients->b0 = fix3_28_from_dbl(b0);
    coefficients->b1 = fix3_28_from_dbl(b1);
    coefficients->b2 = fix3_28_from_dbl(b2);
    coefficients->a0 = fix3_28_from_dbl(a0);
    coefficients->a1 = fix3_28_from_dbl(a1);
    coefficients->a2 = fix3_28_from_dbl(a2);
}

void bqf_memreset(bqf_mem_t *memory) {
    memory->x_1 = fix16_zero;
    memory->x_2 = fix16_zero;
    memory->y_1 = fix16_zero;
    memory->y_2 = fix16_zero;
}