#include "esp_timer.h"

#define PWM_FREQ 1000
#define PWM_RESOLUTION 12

#define MOTOR_1_PIN 5   // Cambia según tu conexión real
#define MOTOR_2_PIN 18  // Cambia según tu conexión real

int const MAX_DUTY_CYCLE = (int)(pow(2, PWM_RESOLUTION) - 1);

String command = "";
bool commandReaded = false;
bool processingCommand = false;
bool skipTask = false;

enum Command {
  CMD_UNKNOWN,
  CMD_OFF,
  CMD_FWD,
  CMD_LEFT,
  CMD_RIGHT
};

/* 
=============== Prototipos funciones ===============
Primero se definen los prototipos de las 
funciones para que se puedan llamar despues
Normalmente esto va en un .h o .hpp y se 
hace el include para llamar la definicion 
de las funciones, pero tambien vale con defnirlas
al inicio del archivo y antes de cualquier 
declaración de funcionalidad
==================================================*/
void create_task_commander(void* args);
void setup_pwm_dc_motor_1();
void setup_pwm_dc_motor_2();
void change_percent_pwm_motor_1(int percent);
void change_percent_pwm_motor_2(int percent);
void serial_isr();
Command parseCommand(String cmd);
//================================================




void setup() {
  pinMode(MOTOR_1_PIN, OUTPUT);
  pinMode(MOTOR_2_PIN, OUTPUT);
  digitalWrite(MOTOR_1_PIN, LOW);
  digitalWrite(MOTOR_2_PIN, LOW);
  Serial.begin(115200);
  Serial.onReceive(serial_isr); // Aquí asigna la func serial_isr para que atienda la int del serial
  Serial.println("Iniciando la aplicación ..."); 
  create_task_commander(); // Aqui se crea la tarea para ejecutar los comandos, se ejecuta cada 1ms
  ledcSetClockSource(LEDC_AUTO_CLK);
  setup_pwm_dc_motor_1();
  change_percent_pwm_motor_1(0);
  setup_pwm_dc_motor_2();
  change_percent_pwm_motor_2(0);
  Serial.println("Aplicación Iniciada ..."); 
}

void loop() {
  // Aqui no hay que hacer nada
}


void setup_pwm_dc_motor_1(){
  ledcAttach(MOTOR_1_PIN, PWM_FREQ, PWM_RESOLUTION);
}
void setup_pwm_dc_motor_2(){
  ledcAttach(MOTOR_2_PIN, PWM_FREQ, PWM_RESOLUTION);
}

void change_percent_pwm_motor_1(int percent){
  // percent 0-100
  int duty = (percent * MAX_DUTY_CYCLE) / 100;
  ledcWrite(MOTOR_1_PIN, duty);
}

void change_percent_pwm_motor_2(int percent){
  int duty = (percent * MAX_DUTY_CYCLE) / 100;
  ledcWrite(MOTOR_2_PIN, duty);
}

void serial_isr(){
  /* Aqui cae cuando llega un comando por el serial
  no hace falta hacer nada mas aqui, el comando que
  llega se lee y se almacena en la variable command
  y se deja en true la variable de processingCommand
  para que la función commander la interprete
  */
  command = "";
  while(Serial.available()) {
    char inChar = (char)Serial.read();
    if (inChar == '\n') {
      commandReaded = true;
      processingCommand = true;
    } else {
      command += inChar;
    }
  }
}

void commander(void* args) {
  /* 
  === Esta función se ejecuta cada 1 ms ===
  ===    Aquí va tu lógica (¡rápida!)   ===

  Si se leyó un comando por serial la variable 
  processingCommand estará en true y el commander
  procede a ejecutar la acción requerida según el
  comando enviado y antes de salir deja la variable
  processingCommand en false, de esta forma el 
  comando solo se ejecuta una vez.

  IMPORTANTE: No enviar comandos muy rapidos porque
  puede llegar a pasar que no se ha procesado el 
  comando anterior cuando llega uno nuevo, dejar al menos 100ms. 

  Si se quiere poder recibir comandos mas rapido habría
  que implementar una cola FIFO para almacenarlos e irlos
  procesando.
  */
  if(skipTask){
    return;
  }
  if(processingCommand){
    skipTask = true;
    Command cmd = parseCommand(command);
    Serial.print("Procesando comando: ");
    Serial.println(command);
    switch (cmd) {
      case CMD_OFF:
        change_percent_pwm_motor_1(0);
        change_percent_pwm_motor_2(0);
        Serial.println("Motores detenidos");
        break;
      case CMD_FWD:
        change_percent_pwm_motor_1(50);
        change_percent_pwm_motor_2(50);
        Serial.println("Adelante");
        break;
      case CMD_LEFT:
        change_percent_pwm_motor_1(25);
        change_percent_pwm_motor_2(50);
        Serial.println("Izquierda");
        break;
      case CMD_RIGHT:
        change_percent_pwm_motor_1(50);
        change_percent_pwm_motor_2(25);
        Serial.println("Derecha");
        break;
      case CMD_UNKNOWN:
      default:
        Serial.println("Comando no reconocido");
        break;
    }
    processingCommand = false;
    skipTask = false;
  }
}

void create_task_commander(){
  const esp_timer_create_args_t timer_args = {
    .callback = &commander,
    .arg = nullptr,
    .dispatch_method = ESP_TIMER_TASK,
    .name = "1ms_commander_timer"
  };

  esp_timer_handle_t periodic_timer;
  esp_timer_create(&timer_args, &periodic_timer);
  esp_timer_start_periodic(periodic_timer, 1000); // 1000 us = 1 ms
}

Command parseCommand(String cmd) {
  cmd.trim();
  cmd.toUpperCase();
  if (cmd.startsWith("OFF")) return CMD_OFF;
  if (cmd.startsWith("FWD")) return CMD_FWD;
  if (cmd.startsWith("LEFT")) return CMD_LEFT;
  if (cmd.startsWith("RIGHT")) return CMD_RIGHT;
  return CMD_UNKNOWN;
}