/*
   MicroDexed

   MicroDexed is a port of the Dexed sound engine
   (https://github.com/asb2m10/dexed) for the Teensy-3.5/3.6/4.x with audio shield.
   Dexed ist heavily based on https://github.com/google/music-synthesizer-for-android

   (c)2018-2021 H. Wirtz <wirtz@parasitstudio.de>

   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, write to the Free Software Foundation,
   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
*/

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

void AudioControlSGTL5000Plus::init_parametric_eq(void)
{
  eqSelect(PARAMETRIC_EQUALIZER);
  eqFilterCount(num_bands);

  filter_type = new uint8_t[num_bands];
  Fc = new float[num_bands];
  Q = new float[num_bands];
  peakGainDB = new float[num_bands];

  setEQType(1, GRAPHIC_EQ_TYPE_0);
  setEQFc(1, GRAPHIC_EQ_CENTER_FRQ_0);
  setEQQ(1, GRAPHIC_EQ_Q_0);
  setEQGain(1, 0.0);

  if (num_bands > 1)
  {
    setEQType(2, GRAPHIC_EQ_TYPE_1);
    setEQFc(2, GRAPHIC_EQ_CENTER_FRQ_1);
    setEQQ(2, GRAPHIC_EQ_Q_1);
    setEQGain(2, 0.0);
  }

  if (num_bands > 2)
  {
    setEQType(3, GRAPHIC_EQ_TYPE_2);
    setEQFc(3, GRAPHIC_EQ_CENTER_FRQ_2);
    setEQQ(3, GRAPHIC_EQ_Q_2);
    setEQGain(3, 0.0);
  }

  if (num_bands > 3)
  {
    setEQType(4, GRAPHIC_EQ_TYPE_3);
    setEQFc(4, GRAPHIC_EQ_CENTER_FRQ_3);
    setEQQ(4, GRAPHIC_EQ_Q_3);
    setEQGain(4, 0.0);
  }

  if (num_bands > 4)
  {
    setEQType(5, GRAPHIC_EQ_TYPE_4);
    setEQFc(5, GRAPHIC_EQ_CENTER_FRQ_4);
    setEQQ(5, GRAPHIC_EQ_Q_4);
    setEQGain(5, 0.0);
  }

  if (num_bands > 5)
  {
    setEQType(6, GRAPHIC_EQ_TYPE_5);
    setEQFc(6, GRAPHIC_EQ_CENTER_FRQ_5);
    setEQQ(6, GRAPHIC_EQ_Q_5);
    setEQGain(6, 0.0);
  }

  if (num_bands > 6)
  {
    setEQType(7, GRAPHIC_EQ_TYPE_6);
    setEQFc(7, GRAPHIC_EQ_CENTER_FRQ_6);
    setEQQ(7, GRAPHIC_EQ_Q_6);
    setEQGain(7, 0.0);
  }
}

void AudioControlSGTL5000Plus::setEQType(uint8_t band, uint8_t ft)
{
  if (filter_type)
  {
    int filter[5];

    band = constrain(band, 1, num_bands);
    filter_type[band - 1] = ft;
    calcBiquad(filter_type[band - 1], Fc[band - 1], peakGainDB[band - 1],  Q[band - 1],  524288,  AUDIO_SAMPLE_RATE, filter);
    //eqFilter(band, filter);
  }
}

void AudioControlSGTL5000Plus::setEQFc(uint8_t band, float frq)
{
  if (Fc)
  {
    int filter[5];

    band = constrain(band, 1, num_bands);
    Fc[band - 1] = frq;
    calcBiquad(filter_type[band - 1], Fc[band - 1], peakGainDB[band - 1],  Q[band - 1],  524288,  AUDIO_SAMPLE_RATE, filter);
    //eqFilter(band, filter);
  }
}

void AudioControlSGTL5000Plus::setEQQ(uint8_t band, float q)
{
  if (Q)
  {
    int filter[5];

    band = constrain(band, 1, num_bands);
    Q[band - 1] = q;
    calcBiquad(filter_type[band - 1], Fc[band - 1], peakGainDB[band - 1],  Q[band - 1],  524288,  AUDIO_SAMPLE_RATE, filter);
    //eqFilter(band, filter);
  }
}

void AudioControlSGTL5000Plus::setEQGain(uint8_t band, float gain)
{
  if (peakGainDB)
  {
    int filter[5];

    band = constrain(band, 1, num_bands);
    peakGainDB[band - 1] = gain;
    calcBiquad(filter_type[band - 1], Fc[band - 1], peakGainDB[band - 1],  Q[band - 1],  524288,  AUDIO_SAMPLE_RATE, filter);
    //eqFilter(band, filter);
  }
}