/*
 * ESP32 Second-Order Sections IIR Filter implementation
 *
 * (c)2019 Ivan Kostoski
 *
 * 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 <https://www.gnu.org/licenses/>.
 */

#ifndef SOS_IIR_FILTER_H
#define SOS_IIR_FILTER_H

#include <algorithm>
#include <array>
#include <cstdint>
#include <ranges>
#include <utility>

#include <cmath>
namespace fpm = std;
using sos_t = float;
//#include <fpm/math.hpp>
//using sos_t = fpm::fixed_16_16;

struct SOS_Coefficients {
  sos_t b1;
  sos_t b2;
  sos_t a1;
  sos_t a2;
};

struct SOS_Delay_State {
  sos_t w0;
  sos_t w1;
};

/**
 * Envelops above asm functions into C++ class
 */
template<std::size_t N>
struct SOS_IIR_Filter {
  const sos_t gain;
  std::array<SOS_Coefficients, N> sos;
  std::array<SOS_Delay_State, N> w;

  // Template constructor for const filter declaration
  constexpr SOS_IIR_Filter(const sos_t gain, const SOS_Coefficients (&_sos)[N]):
    gain(gain)
  {
    std::copy(_sos, _sos + N, sos.begin());
  }

  void filter(auto samples) {
    // Apply all but last Second-Order-Section
    for ([[maybe_unused]] auto _ : std::views::zip_transform(
        [](auto samps, const auto& coeffs, auto& ww) {
            // Assumes a0 and b0 coefficients are one (1.0)
            for (auto& s : samps) {
              auto f6 = s + coeffs.a1 * ww.w0 + coeffs.a2 * ww.w1;
              s = f6 + coeffs.b1 * ww.w0 + coeffs.b2 * ww.w1;
              ww.w1 = std::exchange(ww.w0, f6);
            }
            return 0;
        },
        std::views::repeat(samples, N), sos, w)) {}
  }

  sos_t filter_sum_sqr(auto samples) {
    filter(samples);
    return std::ranges::fold_left(samples, sos_t(0),
      [this](auto a, auto b) { return a + b * gain * b * gain; });
  }
};

#endif  // SOS_IIR_FILTER_H

// Knowles SPH0645LM4H-B, rev. B
// https://cdn-shop.adafruit.com/product-files/3421/i2S+Datasheet.PDF
// B ~= [1.001234, -1.991352, 0.990149]
// A ~= [1.0, -1.993853, 0.993863]
// With additional DC blocking component
SOS_IIR_Filter SPH0645LM4H_B_RB = {
  /* gain: */ sos_t(1.00123377961525),
  /* sos: */ { // Second-Order Sections {b1, b2, -a1, -a2}
         { sos_t(-1.0), sos_t(0.0),
           sos_t(+0.9992), sos_t(0) },  // DC blocker, a1 = -0.9992
         { sos_t(-1.988897663539382), sos_t(+0.988928479008099),
           sos_t(+1.993853376183491), sos_t(-0.993862821429572) } }
};

//
// A-weighting IIR Filter, Fs = 48KHz
// (By Dr. Matt L., Source: https://dsp.stackexchange.com/a/36122)
// B = [0.169994948147430, 0.280415310498794, -1.120574766348363, 0.131562559965936, 0.974153561246036, -0.282740857326553, -0.152810756202003]
// A = [1.0, -2.12979364760736134, 0.42996125885751674, 1.62132698199721426, -0.96669962900852902, 0.00121015844426781, 0.04400300696788968]
SOS_IIR_Filter A_weighting = {
  /* gain: */ sos_t(0.169994948147430),
  /* sos: */ { // Second-Order Sections {b1, b2, -a1, -a2}
         { sos_t(-2.00026996133106), sos_t(+1.00027056142719),
           sos_t(-1.060868438509278), sos_t(-0.163987445885926) },
         { sos_t(+4.35912384203144), sos_t(+3.09120265783884),
           sos_t(+1.208419926363593), sos_t(-0.273166998428332) },
         { sos_t(-0.70930303489759), sos_t(-0.29071868393580),
           sos_t(+1.982242159753048), sos_t(-0.982298594928989) } }
};

////
//// C-weighting IIR Filter, Fs = 48KHz
//// Designed by invfreqz curve-fitting, see respective .m file
//// B = [-0.49164716933714026, 0.14844753846498662, 0.74117815661529129, -0.03281878334039314, -0.29709276192593875, -0.06442545322197900, -0.00364152725482682]
//// A = [1.0, -1.0325358998928318, -0.9524000181023488, 0.8936404694728326   0.2256286147169398  -0.1499917107550188, 0.0156718181681081]
//SOS_IIR_Filter C_weighting = {
//  gain: -0.491647169337140,
//  sos: {
//    { +1.4604385758204708, +0.5275070373815286, +1.9946144559930252, -0.9946217070140883 },
//    { +0.2376222404939509, +0.0140411206016894, -1.3396585608422749, -0.4421457807694559 },
//    { -2.0000000000000000, +1.0000000000000000, +0.3775800047420818, -0.0356365756680430 } }
//};