const int dacPin = 25;    // Pin DAC untuk output simulasi
const int sampleRate = 8000; // Sampling rate (Hz)
const float feedbackGain = 0.8; // Gain feedback (0-1)
const float systemGain = 0.8;   // Gain sistem utama
const int delayLength = 50;     // Panjang delay buffer (sampel)

// Buffer untuk simulasi feedback
float delayBuffer[delayLength];
int delayIndex = 0;

// RLS parameters
const int filterOrder = 4;      // Orde filter RLS
float weights[filterOrder];     // Koefisien filter RLS
float P[filterOrder][filterOrder]; // Matriks kovarian
float inputBuffer[filterOrder]; // Buffer input filter

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

  // Inisialisasi buffer delay
  for (int i = 0; i < delayLength; i++) {
    delayBuffer[i] = 0.0;
  }

  // Inisialisasi filter RLS
  for (int i = 0; i < filterOrder; i++) {
    weights[i] = 0.0;
    inputBuffer[i] = 0.0;
    for (int j = 0; j < filterOrder; j++) {
      P[i][j] = (i == j) ? 1.0 : 0.0; // Matriks identitas
    }
  }
}

void loop() {
  // Sinyal input (contoh sinusoidal)
  float inputSignal = sin(2 * PI * 1.0 * millis() / 1000.0); // 440 Hz sinusoidal

  // Ambil sinyal feedback dari buffer delay
  float feedbackSignal = delayBuffer[delayIndex];

  // Simulasi sinyal output dengan feedback
  float outputSignal = systemGain * inputSignal + feedbackGain * feedbackSignal;

  // Simpan sinyal output ke buffer delay
  delayBuffer[delayIndex] = outputSignal;

  // Perbarui indeks buffer delay
  delayIndex = (delayIndex + 1) % delayLength;

  // Geser buffer input untuk RLS
  for (int i = filterOrder - 1; i > 0; i--) {
    inputBuffer[i] = inputBuffer[i - 1];
  }
  inputBuffer[0] = outputSignal;

  // Estimasi feedback menggunakan RLS
  float feedbackEstimation = 0.0;
  for (int i = 0; i < filterOrder; i++) {
    feedbackEstimation += weights[i] * inputBuffer[i];
  }

  // Hitung error (sinyal bersih)
  float cleanSignal = inputSignal - feedbackEstimation;

  // Update RLS weights
  float k[filterOrder]; // Gain adaptif
  float denominator = 1.0;
  for (int i = 0; i < filterOrder; i++) {
    float temp = 0.0;
    for (int j = 0; j < filterOrder; j++) {
      temp += P[i][j] * inputBuffer[j];
    }
    k[i] = temp;
    denominator += inputBuffer[i] * temp;
  }

  for (int i = 0; i < filterOrder; i++) {
    k[i] /= denominator;
  }

  // Perbarui matriks kovarian
  for (int i = 0; i < filterOrder; i++) {
    for (int j = 0; j < filterOrder; j++) {
      P[i][j] -= k[i] * inputBuffer[j];
    }
  }

  // Perbarui bobot filter
  for (int i = 0; i < filterOrder; i++) {
    weights[i] += k[i] * cleanSignal;
  }

  // Output sinyal bersih ke DAC
  int dacValue = (int)((cleanSignal + 1.0) * 127.5); // Normalisasi ke 0-255
  dacWrite(dacPin, dacValue);


  Serial.print(inputSignal*1000);
  Serial.print(' ');
  Serial.print(feedbackSignal*500);
  Serial.print(' ');
  Serial.print(outputSignal*1000);
  Serial.print(' ');
  Serial.print(cleanSignal*1000);
  Serial.println();

  // Visualisasi hasil di Serial Plotter


  //Serial.println(cleanSignal);

  // Tunggu untuk mencapai sampling rate
  delayMicroseconds(1000000 / sampleRate);
}