#ifdef SGTL5000_AUDIO_ENHANCE

#include <Arduino.h>
#include <Audio.h>

extern AudioControlSGTL5000 sgtl5000_1;

typedef struct biquad_coefficients_s {
  float32_t a0;
  float32_t a1;
  float32_t a2;
  float32_t b1;
  float32_t b2;
} biquad_coefficients_t;

typedef struct biquad_params_s {
  uint8_t filter_type;
  float32_t Fc;
  float32_t Q;
} biquad_params_t;

enum {
  bq_type_lowpass = 0,
  bq_type_highpass,
  bq_type_bandpass,
  bq_type_notch,
  bq_type_peak,
  bq_type_lowshelf,
  bq_type_highshelf
};

#define NUM_GRAPHIC_EQ_BANDS 7

biquad_coefficients_t biquad_coefficients[NUM_GRAPHIC_EQ_BANDS];
biquad_params_t biquad_params[NUM_GRAPHIC_EQ_BANDS];

void set_sgtl5000_7band_eq(uint8_t band, uint8_t filter_type, float32_t Fc, float32_t Q);
void set_sgtl5000_7band_eq_gain(uint8_t band, float32_t peakGainDB);
void setup_sgtl5000_graphic_7band_eq(void);
void calcBiquadCoefficients(biquad_coefficients_t* biquad_coefficients, uint8_t filter_type, float32_t Fc, float32_t Q, float32_t peakGain);

void set_sgtl5000_7band_eq(uint8_t band, uint8_t filter_type, float32_t Fc, float32_t Q)
{
  biquad_params[band].filter_type = filter_type;
  biquad_params[band].Fc = Fc;
  biquad_params[band].Q = Q;

  calcBiquadCoefficients(&biquad_coefficients[band], filter_type,  Fc,  Q,  0.0);

  //sgtl5000_1.eqFilter(band, biquad_coefficients);
}

void set_sgtl5000_7band_eq_gain(uint8_t band, float32_t peakGainDB)
{
  calcBiquadCoefficients(&biquad_coefficients[band], biquad_params[band].filter_type,  biquad_params[band].Fc,  biquad_params[band].Q, peakGainDB);
  //sgtl5000_1.eqFilter(band, biquad_coefficients);
}

void setup_sgtl5000_graphic_7band_eq(void)
{
  //sgtl5000_1.eqFilterCount(7); // enable 7 bands
  set_sgtl5000_7band_eq(0, GRAPHIC_EQ_TYPE_0, GRAPHIC_EQ_CENTER_FRQ_0, GRAPHIC_EQ_Q_0);
  set_sgtl5000_7band_eq(1, GRAPHIC_EQ_TYPE_1, GRAPHIC_EQ_CENTER_FRQ_1, GRAPHIC_EQ_Q_1);
  set_sgtl5000_7band_eq(2, GRAPHIC_EQ_TYPE_2, GRAPHIC_EQ_CENTER_FRQ_2, GRAPHIC_EQ_Q_2);
  set_sgtl5000_7band_eq(3, GRAPHIC_EQ_TYPE_3, GRAPHIC_EQ_CENTER_FRQ_3, GRAPHIC_EQ_Q_3);
  set_sgtl5000_7band_eq(4, GRAPHIC_EQ_TYPE_4, GRAPHIC_EQ_CENTER_FRQ_4, GRAPHIC_EQ_Q_4);
  set_sgtl5000_7band_eq(5, GRAPHIC_EQ_TYPE_5, GRAPHIC_EQ_CENTER_FRQ_5, GRAPHIC_EQ_Q_5);
  set_sgtl5000_7band_eq(6, GRAPHIC_EQ_TYPE_6, GRAPHIC_EQ_CENTER_FRQ_6, GRAPHIC_EQ_Q_6);
}

// Taken from https://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
//
//  Biquad.h
//
//  Created by Nigel Redmon on 11/24/12
//  EarLevel Engineering: earlevel.com
//  Copyright 2012 Nigel Redmon
//
//  For a complete explanation of the Biquad code:
//  http://www.earlevel.com/main/2012/11/25/biquad-c-source-code/
//
//  License:
//
//  This source code is provided as is, without warranty.
//  You may copy and distribute verbatim copies of this document.
//  You may modify and use this source code to create binary code
//  for your own purposes, free or commercial.
//
void calcBiquadCoefficients(biquad_coefficients_t* biquad_coefficients, uint8_t filter_type, float32_t Fc, float32_t Q, float32_t peakGain)
{
  float32_t norm;
  float32_t V = pow(10, fabs(peakGain) / 20.0);
  float32_t K = tan(M_PI * Fc);
  switch (filter_type) {
    case bq_type_lowpass:
      norm = 1 / (1 + K / Q + K * K);
      biquad_coefficients->a0 = K * K * norm;
      biquad_coefficients->a1 = 2 * biquad_coefficients->a0;
      biquad_coefficients->a2 = biquad_coefficients->a0;
      biquad_coefficients->b1 = 2 * (K * K - 1) * norm;
      biquad_coefficients->b2 = (1 - K / Q + K * K) * norm;
      break;

    case bq_type_highpass:
      norm = 1 / (1 + K / Q + K * K);
      biquad_coefficients->a0 = 1 * norm;
      biquad_coefficients->a1 = -2 * biquad_coefficients->a0;
      biquad_coefficients->a2 = biquad_coefficients->a0;
      biquad_coefficients->b1 = 2 * (K * K - 1) * norm;
      biquad_coefficients->b2 = (1 - K / Q + K * K) * norm;
      break;

    case bq_type_bandpass:
      norm = 1 / (1 + K / Q + K * K);
      biquad_coefficients->a0 = K / Q * norm;
      biquad_coefficients->a1 = 0;
      biquad_coefficients->a2 = -biquad_coefficients->a0;
      biquad_coefficients->b1 = 2 * (K * K - 1) * norm;
      biquad_coefficients->b2 = (1 - K / Q + K * K) * norm;
      break;

    case bq_type_notch:
      norm = 1 / (1 + K / Q + K * K);
      biquad_coefficients->a0 = (1 + K * K) * norm;
      biquad_coefficients->a1 = 2 * (K * K - 1) * norm;
      biquad_coefficients->a2 = biquad_coefficients->a0;
      biquad_coefficients->b1 = biquad_coefficients->a1;
      biquad_coefficients->b2 = (1 - K / Q + K * K) * norm;
      break;

    case bq_type_peak:
      if (peakGain >= 0) {    // boost
        norm = 1 / (1 + 1 / Q * K + K * K);
        biquad_coefficients->a0 = (1 + V / Q * K + K * K) * norm;
        biquad_coefficients->a1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->a2 = (1 - V / Q * K + K * K) * norm;
        biquad_coefficients->b1 = biquad_coefficients->a1;
        biquad_coefficients->b2 = (1 - 1 / Q * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (1 + V / Q * K + K * K);
        biquad_coefficients->a0 = (1 + 1 / Q * K + K * K) * norm;
        biquad_coefficients->a1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->a2 = (1 - 1 / Q * K + K * K) * norm;
        biquad_coefficients->b1 = biquad_coefficients->a1;
        biquad_coefficients->b2 = (1 - V / Q * K + K * K) * norm;
      }
      break;
    case bq_type_lowshelf:
      if (peakGain >= 0) {    // boost
        norm = 1 / (1 + sqrt(2) * K + K * K);
        biquad_coefficients->a0 = (1 + sqrt(2 * V) * K + V * K * K) * norm;
        biquad_coefficients->a1 = 2 * (V * K * K - 1) * norm;
        biquad_coefficients->a2 = (1 - sqrt(2 * V) * K + V * K * K) * norm;
        biquad_coefficients->b1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->b2 = (1 - sqrt(2) * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (1 + sqrt(2 * V) * K + V * K * K);
        biquad_coefficients->a0 = (1 + sqrt(2) * K + K * K) * norm;
        biquad_coefficients->a1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->a2 = (1 - sqrt(2) * K + K * K) * norm;
        biquad_coefficients->b1 = 2 * (V * K * K - 1) * norm;
        biquad_coefficients->b2 = (1 - sqrt(2 * V) * K + V * K * K) * norm;
      }
      break;
    case bq_type_highshelf:
      if (peakGain >= 0) {    // boost
        norm = 1 / (1 + sqrt(2) * K + K * K);
        biquad_coefficients->a0 = (V + sqrt(2 * V) * K + K * K) * norm;
        biquad_coefficients->a1 = 2 * (K * K - V) * norm;
        biquad_coefficients->a2 = (V - sqrt(2 * V) * K + K * K) * norm;
        biquad_coefficients->b1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->b2 = (1 - sqrt(2) * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (V + sqrt(2 * V) * K + K * K);
        biquad_coefficients->a0 = (1 + sqrt(2) * K + K * K) * norm;
        biquad_coefficients->a1 = 2 * (K * K - 1) * norm;
        biquad_coefficients->a2 = (1 - sqrt(2) * K + K * K) * norm;
        biquad_coefficients->b1 = 2 * (K * K - V) * norm;
        biquad_coefficients->b2 = (V - sqrt(2 * V) * K + K * K) * norm;
      }
      break;
  }
  return;
}
#endif