// Basic FIR filter using timer interrupts for precise sampling time 
// -----------------------------------------
// This implements a low-pass filter with cutoff frequency fc=150Hz @ fs=1kHz
// The impulse response is at coeffs.h.
//
// (c) 2024 Jorge Iván Marín ([email protected])

#include <SPI.h>

#define ADC_IN  36
#define DAC_OUT 25
#define CS_DAC 5

// Driver for DAC TLC5616
// ----------------------

class TLC5616 {
  int CS_pin;
  public:
  TLC5616(int _CS_pin) {
    CS_pin = _CS_pin;
  }
  void begin(void) {
    SPI.begin();
    pinMode(CS_pin, OUTPUT);
    digitalWrite(CS_pin, HIGH);
  }
  void write(uint16_t value) {
    value = (value & 0x3ff) << 2;
    digitalWrite(CS_pin, LOW);
    SPI.beginTransaction(SPISettings(20000000, MSBFIRST, SPI_MODE0));
    SPI.transfer16(value);
    SPI.endTransaction();
    digitalWrite(CS_pin, HIGH);
  }
};

// Digital Filtering
// ------------------------------------------------

#include "coeffs_fix.h"

float w[M][N];
void iniciar_filtro() {
    int k,i;
    //Fija las condiciones iniciales a cero
    for (k=0; k<N; k++) {
      for(i=0;i<N;i++)
        w[i][k] = 0.0;
    }
}

float filtrar(float x_ADC) { 
    int k,i;
    float yn, wn;
    wn = x_ADC;
    float yz=0;
    //Calcula el valor del elemento de retardo w[0]
    for(i=0;i<N;i++){
      for(k=N-1; k>=1; k--) {
          wn -= ak[i][k] * w[i][k];
      }
      w[i][0]=wn;

      //Calcula la salida
      yn=0;
      for(k=0; k<N; k++) {
        yn += bk[i][k] * w[i][k];
      }
      //Realiza el movimiento de datos
      for(k=N-1; k>0; k--) {
        w[i][k]=w[i][k-1];
      }
      wn=yn;
    }
    return yn;
}

// Entry point
// ----------------

TLC5616 dac(CS_DAC);

hw_timer_t* timer = NULL;  //Hardware timer

void ARDUINO_ISR_ATTR isrTimer() {
  //ADC read (Note: ADC is 12-bit resolution)
  int x = analogRead(ADC_IN);

  //Perform filtering
  int y = filtrar((x-2048))+2048;

  //Control output saturation
  if (y>4095) y=4095;
  if (y<0) y=0;

  //DAC Write (Note: DAC is 10-bit resolution, so we need to scale down)
  dac.write(y >> 2);
  //For internal DAC replaces the above line by
  // dacWrite(DAC1, y>>4);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  //DAC Initialization
  dac.begin();
  // For internal DAC initialization replaces the above line by 
  // (Note: Not supported in Wowki):
  // analogReadResolution(12);
  // pinMode(DAC_OUT, OUTPUT);

  //Timer configuration to set the sampling frequency at 1kHz
  //timer = timerBegin(1000000); // Set timer frequency to 1Mhz
  //timerAttachInterrupt(timer, &isrTimer); // Attach ISR to timer
  iniciar_filtro();
  // Set alarm to call isrTimer function every Ts (value in microseconds).
  // Repeat the alarm (third parameter) with unlimited count = 0 (fourth parameter).
  //timerAlarm(timer, 125, true, 0);
}

void loop() {
  int x = analogRead(ADC_IN);

  unsigned long time = micros();
  int y = (filtrar(x-2048)+2048);
  unsigned long fin_time = micros();
  float res_time = fin_time - time;
  Serial.println(res_time);


  dac.write(y>>2);
 
}