#include <Arduino.h>

const int filterOrder = 50;
float filterCoeffs[filterOrder + 1] = {
  0.0000, 0.0007, 0.0012, 0.0015, 0.0012, -0.0000, -0.0020, -0.0041, -0.0052, -0.0040,
  0.0000, 0.0061, 0.0119, 0.0143, 0.0106, -0.0000, -0.0153, -0.0297, -0.0359, -0.0272,
  0.0000, 0.0440, 0.0974, 0.1488, 0.1860, 0.1995, 0.1860, 0.1488, 0.0974, 0.0440,
  0.0000, -0.0272, -0.0359, -0.0297, -0.0153, -0.0000, 0.0106, 0.0143, 0.0119, 0.0061,
  0.0000, -0.0040, -0.0052, -0.0041, -0.0020, -0.0000, 0.0012, 0.0015, 0.0012, 0.0007, 0.0000
};

float inputSignal[filterOrder + 1] = {0}; // Buffer to store input signal samples
const float sampleFreq = 1000.0;
const float freq1 = 50.0;
const float freq2 = 200.0;
unsigned long previousMillis = 0;
const unsigned long interval = 1; // 1 ms interval for a 1 kHz sampling rate

void setup() {
  Serial.begin(115200);
}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    // Generate the input signal
    float t = currentMillis / 1000.0;
    float newSample = sin(2 * PI * freq1 * t) + 0.5 * sin(2 * PI * freq2 * t);
    
    // Shift the input signal buffer
    for (int i = filterOrder; i > 0; i--) {
      inputSignal[i] = inputSignal[i - 1];
    }
    inputSignal[0] = newSample;

    // Apply FIR filter
    float output = 0;
    for (int i = 0; i <= filterOrder; i++) {
      output += filterCoeffs[i] * inputSignal[i];
    }

    // Print the input and filtered output signals
    Serial.print("Input: ");
    Serial.print(newSample);
    Serial.print(", Output: ");
    Serial.println(output);
  }
}