#include <Arduino.h>
#include <TensorFlowLite_ESP32.h>

#include "tensorflow/lite/micro/all_ops_resolver.h"
#include "tensorflow/lite/micro/micro_interpreter.h"
#include "tensorflow/lite/schema/schema_generated.h"

// ======= MODELO TFLITE EN C =======
#include "model.h"

// ===== PINES QTR-8A =====
int sensores[8] = {4, 5, 6, 7, 8, 9, 10, 11};

// ===== PINES L298N =====
const int ENA = 12; // PWM Motor A
const int IN1 = 13;
const int IN2 = 14;

const int ENB = 15; // PWM Motor B
const int IN3 = 16;
const int IN4 = 17;

// ===== PWM ESP32 =====
const int freqPWM = 1000;
const int resolPWM = 8;

// ===== UMBRAL PARA BINARIZAR =====
const int UMBRAL = 3500;

// ======= BLOQUE TFLM =======
// Tamanho de la memoria de trabajo para el modelo
constexpr int kTensorArenaSize = 4 * 1024;
uint8_t tensor_arena[kTensorArenaSize];

// Objetos principales de TFLM
const tflite::Model* tflm_model;          // Apunta al modelo en model.h
tflite::MicroInterpreter* interpreter;    // Ejecuta el modelo
TfLiteTensor* input;                      // Tensor de entrada
TfLiteTensor* output;                     // Tensor de salida

// Inicializa el motor TFLM
void setup_tflm() {
  // Cargar el modelo desde model.h
  tflm_model = tflite::GetModel(model);

  // Verificar que el modelo es compatible con esta versión
  if (tflm_model->version() != TFLITE_SCHEMA_VERSION) {
    Serial.println("Modelo TFLite incompatible!");
    while (1);
  }

  // Resolver: registra las operaciones del modelo (Dense, ReLU, etc.)
  static tflite::AllOpsResolver resolver;

  // Crear el intérprete con el modelo y la memoria (tensor arena)
  static tflite::MicroInterpreter static_interpreter(
      tflm_model, resolver, tensor_arena, kTensorArenaSize);
  interpreter = &static_interpreter;

  // Asignar memoria a los tensores
  if (interpreter->AllocateTensors() != kTfLiteOk) {
    Serial.println("AllocateTensors() falló!");
    while (1);
  }

  // Obtener punteros a los tensores de entrada y salida
  input = interpreter->input(0);
  output = interpreter->output(0);
}

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

  ledcAttach(ENA, freqPWM, resolPWM);
  ledcAttach(ENB, freqPWM, resolPWM);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Inicializar TFLM
  setup_tflm();
}

void loop() {
  // ===== 1) Leer sensores y binarizar =====
  float x[8];
  for (int i = 0; i < 8; i++) {
    int raw = analogRead(sensores[i]);
    x[i] = (raw >= UMBRAL) ? 1.0 : 0.0;
  }

  // ===== 2) INFERENCIA TFLM =====
  // Copiar entradas al tensor del modelo
  for (int i = 0; i < 8; i++) {
    input->data.f[i] = x[i];
  }

  // Ejecutar la red neuronal
  interpreter->Invoke();

  // Leer salidas del modelo
  float y0 = output->data.f[0];
  float y1 = output->data.f[1];

  // ===== 3) Escalar a PWM 0..255 =====
  int m1 = (int)(y0 * 255.0);
  int m2 = (int)(y1 * 255.0);

  if (m1 < 0) m1 = 0; if (m1 > 255) m1 = 255;
  if (m2 < 0) m2 = 0; if (m2 > 255) m2 = 255;

  // ===== 4) Motores adelante =====
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  ledcWrite(ENA, m1);
  ledcWrite(ENB, m2);

  // ===== 5) Serial para ver =====
  Serial.print("m1="); Serial.print(m1);
  Serial.print(" m2="); Serial.println(m2);
}