//#include "biquad_filter.h"

typedef struct biquad_filter_t {
  // Filter coefficients
  double b0, b1, b2; // Feedforward coefficients
  double a1, a2;     // Feedback coefficients
    
  // Delay buffers (history of input and output)
  double x1, x2; // Previous input samples
  double y1, y2; // Previous output samples
} biquad_filter_t;

void
biquad_calculate_coefficients(biquad_filter_t* filter, double freq, double Q, double sampleRate, char type)
{
  double omega = 2.0 * M_PI * freq / sampleRate;
  double alpha = sin(omega) / (2.0 * Q);
  double cos_omega = cos(omega);

  // Initialize coefficients based on filter type
  switch (type) {
  case 'l': // Low-pass filter
    filter->b0 = (1.0 - cos_omega) / 2.0;
    filter->b1 = 1.0 - cos_omega;
    filter->b2 = filter->b0;
    filter->a1 = -2.0 * cos_omega;
    filter->a2 = 1.0 - alpha;
    break;
  case 'h': // High-pass filter
    filter->b0 = (1.0 + cos_omega) / 2.0;
    filter->b1 = -(1.0 + cos_omega);
    filter->b2 = filter->b0;
    filter->a1 = -2.0 * cos_omega;
    filter->a2 = 1.0 - alpha;
    break;
  case 'b': // Band-pass filter
    filter->b0 = alpha;
    filter->b1 = 0.0;
    filter->b2 = -alpha;
    filter->a1 = -2.0 * cos_omega;
    filter->a2 = 1.0 - alpha;
    break;
  default:
    exit(EXIT_FAILURE); // Unsupported filter type
  }

  // Normalize coefficients
  double a0 = 1.0 + alpha;
  filter->b0 /= a0;
  filter->b1 /= a0;
  filter->b2 /= a0;
  filter->a1 /= a0;
  filter->a2 /= a0;
}

double
biquad_process(biquad_filter_t* filter, double input)
{
  double 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;
}