#include <Arduino.h>
#include <driver/ledc.h>
#include <esp32-hal-ledc.h> 

// === DEFINIÇÃO DOS PINOS ===
// Sensores Ultrassônicos
#define TRIG_FRONT     23
#define ECHO_FRONT     22
#define TRIG_RIGHT     18
#define ECHO_RIGHT     5
#define TRIG_BACK      32
#define ECHO_BACK      33
#define TRIG_LEFT      27
#define ECHO_LEFT      14

// LEDs (simulam motores)
#define LED_FRONT  13
#define LED_RIGHT  12
#define LED_BACK   26
#define LED_LEFT   25

// Canais PWM dos motores (LEDs)
#define CH_FRONT  0
#define CH_RIGHT  1
#define CH_BACK   2
#define CH_LEFT   3

// Variáveis globais das distâncias (em cm)
volatile int distFront = 100;
volatile int distRight = 100;
volatile int distBack = 100;
volatile int distLeft = 100;

// === FUNÇÃO DE LEITURA DE DISTÂNCIA ===
int readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000); // timeout de 30 ms
  return duration ? duration * 0.034 / 2 : 400;  // distância em cm
}

// === FUNÇÃO DE CÁLCULO DO PWM EXPONENCIAL ===
int calculatePWM(int distance) {
  float k = 0.05;
  float pwm = 255 * (1 - exp(-k * distance));
  return constrain((int)pwm, 0, 255);
}

// === TAREFAS DOS SENSORES (uma para cada) ===
void taskSensorFront(void *pvParameters) {
  while (true) {
    distFront = readUltrasonic(TRIG_FRONT, ECHO_FRONT);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void taskSensorRight(void *pvParameters) {
  while (true) {
    distRight = readUltrasonic(TRIG_RIGHT, ECHO_RIGHT);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void taskSensorBack(void *pvParameters) {
  while (true) {
    distBack = readUltrasonic(TRIG_BACK, ECHO_BACK);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void taskSensorLeft(void *pvParameters) {
  while (true) {
    distLeft = readUltrasonic(TRIG_LEFT, ECHO_LEFT);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// === TAREFA ÚNICA DE CONTROLE DOS MOTORES ===
void taskMotorControl(void *pvParameters) {
  while (true) {
    ledcWrite(CH_FRONT, calculatePWM(distFront));
    ledcWrite(CH_RIGHT, calculatePWM(distRight));
    ledcWrite(CH_BACK,  calculatePWM(distBack));
    ledcWrite(CH_LEFT,  calculatePWM(distLeft));
    vTaskDelay(pdMS_TO_TICKS(200));
  }
}

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

  // Pinos dos sensores
  pinMode(TRIG_FRONT, OUTPUT);  pinMode(ECHO_FRONT, INPUT);
  pinMode(TRIG_RIGHT, OUTPUT);  pinMode(ECHO_RIGHT, INPUT);
  pinMode(TRIG_BACK,  OUTPUT);  pinMode(ECHO_BACK,  INPUT);
  pinMode(TRIG_LEFT,  OUTPUT);  pinMode(ECHO_LEFT,  INPUT);

  // PWM para os LEDs (motores)
  ledcSetup(CH_FRONT, 5000, 8);  ledcAttachPin(LED_FRONT, CH_FRONT);
  ledcSetup(CH_RIGHT, 5000, 8);  ledcAttachPin(LED_RIGHT, CH_RIGHT);
  ledcSetup(CH_BACK,  5000, 8);  ledcAttachPin(LED_BACK,  CH_BACK);
  ledcSetup(CH_LEFT,  5000, 8);  ledcAttachPin(LED_LEFT,  CH_LEFT);

  // Criação das tarefas
  xTaskCreate(taskSensorFront,  "SensorFront",  2048, NULL, 2, NULL);
  xTaskCreate(taskSensorRight,  "SensorRight",  2048, NULL, 2, NULL);
  xTaskCreate(taskSensorBack,   "SensorBack",   2048, NULL, 2, NULL);
  xTaskCreate(taskSensorLeft,   "SensorLeft",   2048, NULL, 2, NULL);
  xTaskCreate(taskMotorControl, "MotorControl", 2048, NULL, 1, NULL);
}

void loop() {
  // Nada aqui. O FreeRTOS gerencia as tarefas.
}