#ifdef SGTL5000_AUDIO_ENHANCE

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

#define EQ_LOWPASS 0
#define EQ_HIGHPASS 1
#define EQ_BANDPASS 2
#define EQ_NOTCH 3
#define EQ_PEAK 4
#define EQ_LOWSHELF 5
#define EQ_HIGHSHELF 6

#define GRAPHIC_EQ_TYPE_0 EQ_HIGHPASS
#define GRAPHIC_EQ_CENTER_FRQ_0 115.0
#define GRAPHIC_EQ_Q_0 2.0

#define GRAPHIC_EQ_TYPE_1 EQ_BANDPASS
#define GRAPHIC_EQ_CENTER_FRQ_1 330.0
#define GRAPHIC_EQ_Q_1 2.0

#define GRAPHIC_EQ_TYPE_2 EQ_BANDPASS
#define GRAPHIC_EQ_CENTER_FRQ_2 990.0
#define GRAPHIC_EQ_Q_2 2.0

#define GRAPHIC_EQ_TYPE_3 EQ_BANDPASS
#define GRAPHIC_EQ_CENTER_FRQ_3 2000.0
#define GRAPHIC_EQ_Q_3 2.0

#define GRAPHIC_EQ_TYPE_4 EQ_BANDPASS
#define GRAPHIC_EQ_CENTER_FRQ_4 4000.0
#define GRAPHIC_EQ_Q_4 2.0

#define GRAPHIC_EQ_TYPE_5 EQ_BANDPASS
#define GRAPHIC_EQ_CENTER_FRQ_5 9900.0
#define GRAPHIC_EQ_Q_5 2.0

#define GRAPHIC_EQ_TYPE_6 EQ_LOWPASS
#define GRAPHIC_EQ_CENTER_FRQ_6 11000.0
#define GRAPHIC_EQ_Q_6 2.0

extern AudioControlSGTL5000 sgtl5000_1;

class BiquadCoef
{
  public:
    BiquadCoef(uint8_t num_bands);
    ~BiquadCoef();

    void set_eq_type(uint8_t band, uint8_t ft);
    void set_eq_Fc(uint8_t band, float32_t frq);
    void set_eq_Q(uint8_t band, float32_t q);
    void set_gain(uint8_t band, float32_t gain);
    void get_coef(uint8_t band, int* c);

  private:
    void calcBiquadCoefficients(uint8_t band);

    uint8_t num_bands;

    uint8_t *filter_type;
    float32_t *Fc;
    float32_t *Q;
    float32_t *peakGainDB;

    float32_t *a0;
    float32_t *a1;
    float32_t *a2;
    float32_t *b1;
    float32_t *b2;
};

BiquadCoef::BiquadCoef(uint8_t num_bands)
{
  num_bands = constrain(num_bands, 1, 7);

  sgtl5000_1.eqFilterCount(num_bands);

  filter_type = new uint8_t[num_bands];
  Fc = new float32_t[num_bands];
  Q = new float32_t[num_bands];
  peakGainDB = new float32_t[num_bands];
  a0 = new float32_t[num_bands];
  a1 = new float32_t[num_bands];
  a2 = new float32_t[num_bands];
  b1 = new float32_t[num_bands];
  b2 = new float32_t[num_bands];

  set_eq_type(0, GRAPHIC_EQ_TYPE_0);
  set_eq_Fc(0, GRAPHIC_EQ_CENTER_FRQ_0);
  set_eq_Q(0, GRAPHIC_EQ_Q_0);
  set_gain(0, 0.0);

  if (num_bands > 1)
  {
    set_eq_type(1, GRAPHIC_EQ_TYPE_1);
    set_eq_Fc(1, GRAPHIC_EQ_CENTER_FRQ_1);
    set_eq_Q(1, GRAPHIC_EQ_Q_1);
    set_gain(1, 0.0);
  }

  if (num_bands > 2)
  {
    set_eq_type(2, GRAPHIC_EQ_TYPE_2);
    set_eq_Fc(2, GRAPHIC_EQ_CENTER_FRQ_2);
    set_eq_Q(2, GRAPHIC_EQ_Q_2);
    set_gain(2, 0.0);
  }

  if (num_bands > 3)
  {
    set_eq_type(3, GRAPHIC_EQ_TYPE_3);
    set_eq_Fc(3, GRAPHIC_EQ_CENTER_FRQ_3);
    set_eq_Q(3, GRAPHIC_EQ_Q_3);
    set_gain(3, 0.0);
  }

  if (num_bands > 4)
  {
    set_eq_type(4, GRAPHIC_EQ_TYPE_4);
    set_eq_Fc(4, GRAPHIC_EQ_CENTER_FRQ_4);
    set_eq_Q(4, GRAPHIC_EQ_Q_4);
    set_gain(4, 0.0);
  }

  if (num_bands > 5)
  {
    set_eq_type(5, GRAPHIC_EQ_TYPE_5);
    set_eq_Fc(5, GRAPHIC_EQ_CENTER_FRQ_5);
    set_eq_Q(5, GRAPHIC_EQ_Q_5);
    set_gain(5, 0.0);
  }

  if (num_bands > 6)
  {
    set_eq_type(6, GRAPHIC_EQ_TYPE_6);
    set_eq_Fc(6, GRAPHIC_EQ_CENTER_FRQ_6);
    set_eq_Q(6, GRAPHIC_EQ_Q_6);
    set_gain(6, 0.0);
  }

  for (uint8_t i = 0; i < num_bands; i++)
  {
    int tmp[num_bands];

    calcBiquadCoefficients(i);
    get_coef(i, tmp);
    sgtl5000_1.eqFilter(i, tmp);
  }
}

BiquadCoef::~BiquadCoef()
{
  ;
}

void BiquadCoef::set_eq_type(uint8_t band, uint8_t ft)
{
  int tmp[num_bands];

  filter_type[band] = ft;
  calcBiquadCoefficients(band);
  get_coef(band, tmp);
  sgtl5000_1.eqFilter(band, tmp);
}

void BiquadCoef::set_eq_Fc(uint8_t band, float32_t frq)
{
  int tmp[num_bands];

  Fc[band] = frq;
  calcBiquadCoefficients(band);
  get_coef(band, tmp);
  sgtl5000_1.eqFilter(band, tmp);
}

void BiquadCoef::set_eq_Q(uint8_t band, float32_t q)
{
  int tmp[num_bands];

  Q[band] = q;
  calcBiquadCoefficients(band);
  get_coef(band, tmp);
  sgtl5000_1.eqFilter(band, tmp);
}

void BiquadCoef::set_gain(uint8_t band, float32_t gain)
{
  int tmp[num_bands];

  peakGainDB[band] = gain;
  calcBiquadCoefficients(band);
  get_coef(band, tmp);
  sgtl5000_1.eqFilter(band, tmp);
}

void BiquadCoef::get_coef(uint8_t band, int* c)
{
  if (c != NULL)
  {
    c[0] = a0[band] * 0x8000;
    c[1] = a1[band] * 0x8000;
    c[2] = a2[band] * 0x8000;
    c[3] = b1[band] * 0x8000;
    c[4] = b2[band] * 0x8000;
  }
}

// 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 BiquadCoef::calcBiquadCoefficients(uint8_t band)
{
  if (band > num_bands)
    band = num_bands;

  float32_t norm;
  float32_t V = pow(10, fabs(peakGainDB[band]) / 20.0);
  float32_t K = tan(M_PI * Fc[band]);
  switch (filter_type[band]) {
    case EQ_LOWPASS:
      norm = 1 / (1 + K / Q[band] + K * K);
      a0[band] = K * K * norm;
      a1[band] = 2 * a0[band];
      a2[band] = a0[band];
      b1[band] = 2 * (K * K - 1) * norm;
      b2[band] = (1 - K / Q[band] + K * K) * norm;
      break;

    case EQ_HIGHPASS:
      norm = 1 / (1 + K / Q[band] + K * K);
      a0[band] = 1 * norm;
      a1[band] = -2 * a0[band];
      a2[band] = a0[band];
      b1[band] = 2 * (K * K - 1) * norm;
      b2[band] = (1 - K / Q[band] + K * K) * norm;
      break;

    case EQ_BANDPASS:
      norm = 1 / (1 + K / Q[band] + K * K);
      a0[band] = K / Q[band] * norm;
      a1[band] = 0;
      a2[band] = -a0[band];
      b1[band] = 2 * (K * K - 1) * norm;
      b2[band] = (1 - K / Q[band] + K * K) * norm;
      break;

    case EQ_NOTCH:
      norm = 1 / (1 + K / Q[band] + K * K);
      a0[band] = (1 + K * K) * norm;
      a1[band] = 2 * (K * K - 1) * norm;
      a2[band] = a0[band];
      b1[band] = a1[band];
      b2[band] = (1 - K / Q[band] + K * K) * norm;
      break;

    case EQ_PEAK:
      if (peakGainDB[band] >= 0) {    // boost
        norm = 1 / (1 + 1 / Q[band] * K + K * K);
        a0[band] = (1 + V / Q[band] * K + K * K) * norm;
        a1[band] = 2 * (K * K - 1) * norm;
        a2[band] = (1 - V / Q[band] * K + K * K) * norm;
        b1[band] = a1[band];
        b2[band] = (1 - 1 / Q[band] * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (1 + V / Q[band] * K + K * K);
        a0[band] = (1 + 1 / Q[band] * K + K * K) * norm;
        a1[band] = 2 * (K * K - 1) * norm;
        a2[band] = (1 - 1 / Q[band] * K + K * K) * norm;
        b1[band] = a1[band];
        b2[band] = (1 - V / Q[band] * K + K * K) * norm;
      }
      break;
    case EQ_LOWSHELF:
      if (peakGainDB[band] >= 0) {    // boost
        norm = 1 / (1 + sqrt(2) * K + K * K);
        a0[band] = (1 + sqrt(2 * V) * K + V * K * K) * norm;
        a1[band] = 2 * (V * K * K - 1) * norm;
        a2[band] = (1 - sqrt(2 * V) * K + V * K * K) * norm;
        b1[band] = 2 * (K * K - 1) * norm;
        b2[band] = (1 - sqrt(2) * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (1 + sqrt(2 * V) * K + V * K * K);
        a0[band] = (1 + sqrt(2) * K + K * K) * norm;
        a1[band] = 2 * (K * K - 1) * norm;
        a2[band] = (1 - sqrt(2) * K + K * K) * norm;
        b1[band] = 2 * (V * K * K - 1) * norm;
        b2[band] = (1 - sqrt(2 * V) * K + V * K * K) * norm;
      }
      break;
    case EQ_HIGHSHELF:
      if (peakGainDB[band] >= 0) {    // boost
        norm = 1 / (1 + sqrt(2) * K + K * K);
        a0[band] = (V + sqrt(2 * V) * K + K * K) * norm;
        a1[band] = 2 * (K * K - V) * norm;
        a2[band] = (V - sqrt(2 * V) * K + K * K) * norm;
        b1[band] = 2 * (K * K - 1) * norm;
        b2[band] = (1 - sqrt(2) * K + K * K) * norm;
      }
      else {    // cut
        norm = 1 / (V + sqrt(2 * V) * K + K * K);
        a0[band] = (1 + sqrt(2) * K + K * K) * norm;
        a1[band] = 2 * (K * K - 1) * norm;
        a2[band] = (1 - sqrt(2) * K + K * K) * norm;
        b1[band] = 2 * (K * K - V) * norm;
        b2[band] = (V - sqrt(2 * V) * K + K * K) * norm;
      }
      break;
  }
  return;
}
#endif