#include <Wire.h>
#include <Adafruit_MCP4725.h>
#include <Ticker.h>

// ==========================================
// SECCIÓN 1: LIBRERÍAS Y CONFIGURACIÓN DAC
// ==========================================
Adafruit_MCP4725 dac;

// ==========================================
// SECCIÓN 2: PARÁMETROS AJUSTABLES DEL SISTEMA
// ==========================================
const float k = 3.0;    // Ganancia del sistema
const float a = 1.0;    // Polo del sistema (rad/s)
const float T = 0.00819; // Periodo de muestreo (8.19 ms = 122 Hz)

// Coeficientes del sistema discreto
float b0;
float a1;

// ==========================================
// SECCIÓN 3: VARIABLES DE ESTADO DEL SISTEMA
// ==========================================
float estado_anterior = 0.0;
float entrada = 0.0;
float salida = 0.0;

// ==========================================
// SECCIÓN 4: CONFIGURACIÓN TEMPORIZADOR ESP32
// ==========================================
Ticker sampleTimer;
volatile bool timerFlag = false;

void IRAM_ATTR onTimer() {
  timerFlag = true;
}

// ==========================================
// SECCIÓN 5: FUNCIONES DEL SISTEMA
// ==========================================
void calcular_coeficientes() {
  b0 = k * T;
  a1 = 1.0 / (1.0 + a * T);
  
  Serial.print("Coeficientes calculados: b0 = ");
  Serial.print(b0);
  Serial.print(", a1 = ");
  Serial.println(a1);
}

void paso_sistema() {
  salida = a1 * (b0 * entrada + estado_anterior);
  estado_anterior = salida;
}

// ==========================================
// SECCIÓN 6: SETUP Y LOOP PRINCIPAL
// ==========================================
void setup() {
  Serial.begin(9600);
  Wire.begin();
  dac.begin(0x60);
  
  calcular_coeficientes();
  
  // Configurar temporizador con Ticker
  sampleTimer.attach(T, onTimer); // T en segundos
  
  // Configurar pin de entrada analógica
  analogReadResolution(10); // 10 bits (0-1023)
  analogSetAttenuation(ADC_11db); // Rango completo 0-3.3V
}

void loop() {
  if (timerFlag) {
    timerFlag = false;
    
    // Leer entrada (GPIO 36 = ADC1_CH0)
    // int raw = 1.0;
    // entrada = (raw * 3.3) / 1023.0; // ESP32 funciona a 3.3V
    entrada = 1.0;
    paso_sistema();
    
    float salida_voltaje = constrain(salida, 0.0, 3.3); // Máximo 3.3V para ESP32
    dac.setVoltage((uint16_t)(salida_voltaje * 4095 / 3.3), false);
    
    static int contador = 0;
    if (contador++ >= 100) {
      contador = 0;
      Serial.print("Entrada: ");
      Serial.print(entrada);
      Serial.print("V, Salida: ");
      Serial.print(salida_voltaje);
      Serial.println("V");
    }
  }
}