// Copyright (c) 2015-2016, XMOS Ltd, All rights reserved
// AN00209 app_design
// Modified by Teig to also print out the params and the coeffs plus fixed some sloppy errors (5May2022)

#include <stdio.h>
#include "dsp.h"

// Results of biquad coeff generation ara normalized such that a0 == 1.
// When using lib_dsp biquad functions that data arrays must be DWORD aligned.
// Declaring coeffs/state data globablly ensures this while locally does not!
// The biquad functions utilize coeffs arrays of length 5 but ...
// The coeffs below are arraned as N x 6 to keep coeff arrays DWORD aligned.

//                                           From dsp_filters.h
#define DSP_NUM_COEFFS_PER_BIQUAD_DWALIGNED (DSP_NUM_COEFFS_PER_BIQUAD + 1) // = 5+1 Double word aligned, else crash
//                                           DSP_NUM_STATES_PER_BIQUAD      // = 4

#define NUM_PASSES  8
#define NUM_FILTERS 8

int32_t coeff[NUM_FILTERS][DSP_NUM_COEFFS_PER_BIQUAD_DWALIGNED]; // Coefficients for each filter
//Note: array size of 6 is chosen to enforce 64 bit alignment for ldd and std
int32_t state[NUM_FILTERS][DSP_NUM_STATES_PER_BIQUAD]; // State data for each filter. Init below

const char* names[NUM_FILTERS] =
{
    "notch    ", "lowpass  ", "highpass ", "allpass  ",
    "bandpass ", "peaking  ", "lowshelf ", "highshelf",
};

#define Q_FORMAT 28

#define GAIN_DB  3.0
#define GAIN_DBs (0b11<<Q_FORMAT)
#define UNIT_1   1.0
#define UNIT_1s  (1<<Q_FORMAT)

#if (Q_FORMAT == 31)                      // one sign bit, 0 integer bits, 31 fraction bits
    #define FNN(int32)     F31(int32)     // to double
    #define QNN(x_point_y) Q31(x_point_y) // to int32
#elif (Q_FORMAT == 30)                    // one sign bit, 1 integer bit,  30 fraction bits
    #define FNN(int32)     F30(int32)     // to double
    #define QNN(x_point_y) Q30(x_point_y) // to int32
#elif (Q_FORMAT == 29)                    // one sign bit, 2 integer bit,  29 fraction bits
    #define FNN(int32)     F29(int32)     // to double
    #define QNN(x_point_y) Q29(x_point_y) // to int32
#elif (Q_FORMAT == 28)                    // one sign bit, 3 integer bits, 28 fraction bits
    #define FNN(int32)     F28(int32)     // to double
    #define QNN(x_point_y) Q28(x_point_y) // to int32
#elif (Q_FORMAT == 24)                    // one sign bit, 7 integer bits, 24 fraction bits
    #define FNN(int32)     F24(int32)     // to double
    #define QNN(x_point_y) Q24(x_point_y) // to int32
#elif (Q_FORMAT == 16)                    // one sign bit, 15 integer bits, 16 fraction bits
    #define FNN(int32)     F16(int32)     // to double
    #define QNN(x_point_y) Q16(x_point_y)
#else
    #error
#endif


int main (void)
{
    // Generate biquad filter coefficients for various filters
    // Frequency specifications are normalized to Fs (i.e. 0.25 = 1/4 Fs) (Teig: fixed bandpass)
    // Q for filters is 0.707 = 1/sqrt(2)                                 (Teig: fixed lowpass, bandpass)
    // Gain for peaking/lowshelf/highshelf filters is 3.0 dB.
    // Q format QNN (4-bit signed integer, 28-bit fraction) is used.

    //                                                 (*)
    dsp_design_biquad_notch     (0.25, 0.707,          coeff[0], Q_FORMAT);
    dsp_design_biquad_lowpass   (0.25, 0.707,          coeff[1], Q_FORMAT); // Teig: was 0.9 second param
    dsp_design_biquad_highpass  (0.25, 0.707,          coeff[2], Q_FORMAT);
    dsp_design_biquad_allpass   (0.25, 0.707,          coeff[3], Q_FORMAT);
    dsp_design_biquad_bandpass  (0.25, 0.707,          coeff[4], Q_FORMAT); // Teig: was 0.24 first param, 0.26 second
    dsp_design_biquad_peaking   (0.25, 0.707, GAIN_DB, coeff[5], Q_FORMAT);
    dsp_design_biquad_lowshelf  (0.25, 0.707, GAIN_DB, coeff[6], Q_FORMAT);
    dsp_design_biquad_highshelf (0.25, 0.707, GAIN_DB, coeff[7], Q_FORMAT);
    //
    // (*) Coeffs are 5, but but C doesn't complain about giving it 6, for the sake of double word alignment

    printf ("XMOS AN00209 app_design, mods by Teig 3May2022\n\n");

    // Several else print buffer overflows:
    printf ("Q_FORMAT %u\n", Q_FORMAT);
    printf ("Max %f as FNN %+04.8f (0x%08x as QNN 0x%08x)\n",   GAIN_DB, FNN(GAIN_DBs), GAIN_DBs, (unsigned) QNN(GAIN_DB));
    printf ("1   %f as FNN %+04.8f (0x%08x as QNN 0x%08x)\n\n", UNIT_1,  FNN(UNIT_1s),  UNIT_1s,  (unsigned) QNN(UNIT_1));
    printf ("                             f_n   Q      gain-dB  \n");
    printf ("dsp_design_biquad_notch     (0.25, 0.707, ..     ) \n");
    printf ("dsp_design_biquad_lowpass   (0.25, 0.707, ..     ) \n");
    printf ("dsp_design_biquad_highpass  (0.25, 0.707, ..     ) \n");
    printf ("dsp_design_biquad_allpass   (0.25, 0.707, ..     ) \n");
    printf ("dsp_design_biquad_bandpass  (0.25, 0.707, ..     ) \n");
    printf ("dsp_design_biquad_peaking   (0.25, 0.707, 3.0, ..) \n");
    printf ("dsp_design_biquad_lowshelf  (0.25, 0.707, 3.0, ..) \n");
    printf ("dsp_design_biquad_highshelf (0.25, 0.707, 3.0, ..) \n\n");

    // Print out the coeffs
    printf ("                    b0           b1           b2           -a1          -a2\n");

    for (int32_t ii = 0; ii < NUM_FILTERS; ++ii) {
        int32_t sample;
        printf ("Coeffs of %s ", names[ii]);
        for (int32_t jj = 0; jj < DSP_NUM_COEFFS_PER_BIQUAD; ++jj) {
            printf ("%+04.8f%s ", FNN(coeff[ii][jj]), jj<(DSP_NUM_COEFFS_PER_BIQUAD-1) ? "," : "");
        }
        printf ("\n");
    }

    printf ("\n");

    // Initialize filter state data to zeros

    for (int32_t ii = 0; ii < NUM_FILTERS; ++ii) {
        for (int32_t jj = 0; jj < DSP_NUM_STATES_PER_BIQUAD; ++jj) {
            state[ii][jj] = 0;
        }
    }

    // Perform 8 passes through filters independently using step function

    for (int32_t ii = 0; ii < NUM_FILTERS; ++ii) {

        int32_t sample;
        printf ("Impulse response of %s ", names[ii]);

        for (int32_t jj = 0; jj < NUM_PASSES; ++jj) {

            if (jj==0) { // Dirac delta function. 1 at 0, 0 otherwise (Teig: Kronecker delta)
                sample = QNN(1.0);
            } else {
                sample = 0;
            }

            //                                      (*)
            int32_t y = dsp_filters_biquad (sample, coeff[ii], state[ii], Q_FORMAT);
            printf ("%+04.8f%s ", FNN(y), jj<(NUM_PASSES-1) ? "," : "");

            // (*) Coeffs are 5, but xCore asm doesn't complain about giving it 6, for the sake of double word alignment
        }
        printf ("\n");
    }

    return 0;
}