#include "Biquad.h"
#include <math.h>

static constexpr float PI = 3.14159265359f;

void lpf2p_init(BiquadFilter *filter, float sample_freq, float cutoff_freq)
{
  // Некорректные параметры, фильтр не будет работать
  if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) return;
  
  float fr = sample_freq / cutoff_freq;
  float ohm = tanf(PI / fr);
  float c = 1.0f + 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm;
  
  filter->b0 = ohm * ohm / c;
  filter->b1 = 2.0f * filter->b0;
  filter->b2 = filter->b0;

  filter->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
  filter->a2 = (1.0f - 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm) / c;

  // Сбрасываем историю
  filter->x1 = filter->x2 = 0.0f;
  filter->y1 = filter->y2 = 0.0f;
}

void notch_init(BiquadFilter *filter, float sample_freq, float center_freq, float bandwidth)
{
  // Некорректные параметры
  if (center_freq <= 0.0f || sample_freq <= 0.0f || bandwidth <= 0.0f) return;
  
  // Промежуточные расчеты по стандартным формулам для biquad-фильтра
  float w0 = 2.0f * PI * center_freq / sample_freq;
  float alpha = sinf(w0) * sinhf(logf(2.0f) / 2.0f * bandwidth * w0 / sinf(w0));
  
  // Рассчитываем коэффициенты b (для входа x) и a (для выхода y)
  float b0_temp = 1.0f;
  float b1_temp = -2.0f * cosf(w0);
  float b2_temp = 1.0f;
  
  float a0_temp = 1.0f + alpha;
  float a1_temp = -2.0f * cosf(w0);
  float a2_temp = 1.0f - alpha;
  
  // Нормализуем коэффициенты (делим на a0), чтобы использовать в уравнении
  filter->b0 = b0_temp / a0_temp;
  filter->b1 = b1_temp / a0_temp;
  filter->b2 = b2_temp / a0_temp;
  
  filter->a1 = a1_temp / a0_temp;
  filter->a2 = a2_temp / a0_temp;
  
  // Сбрасываем историю
  filter->x1 = filter->x2 = 0.0f;
  filter->y1 = filter->y2 = 0.0f;
}

float biquad_apply(BiquadFilter *filter, float input)
{
  // Применяем разностное уравнение: y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
  float output = filter->b0 * input +
                filter->b1 * filter->x1 +
                filter->b2 * filter->x2 -
                filter->a1 * filter->y1 -
                filter->a2 * filter->y2;
  
  // Обновляем историю для следующего шага
  filter->x2 = filter->x1;
  filter->x1 = input;
  
  filter->y2 = filter->y1;
  filter->y1 = output;
  
  return output;
}