#include <Arduino.h>

// === DEFINIÇÕES DE PINOS ===
#define TRIG_F  23
#define ECHO_F  22
#define TRIG_T  19
#define ECHO_T  21
#define TRIG_D  18
#define ECHO_D   5
#define TRIG_E  27
#define ECHO_E  26

#define LED_F   13
#define LED_T   12
#define LED_D   14
#define LED_E   25

#define PWM_FREQ     5000
#define PWM_RES      8
#define PWM_MAX      255

// === VARIÁVEIS COMPARTILHADAS ===
volatile int distF = 0, distT = 0, distD = 0, distE = 0;

// === Função de leitura do sensor ===
int lerDistancia(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duracao = pulseIn(echo, HIGH, 30000);
  return duracao ? (int)(duracao * 0.034 / 2) : 0;
}

// === Função exponencial para PWM ===
int calcularPWM(int dist) {
  if (dist <= 0) return 0;
  float k = 0.05;
  float pwm = PWM_MAX * (1 - exp(-k * dist));
  return constrain((int)pwm, 0, PWM_MAX);
}

// === TAREFAS DOS SENSORES ===
void tarefaSensorFrente(void *pv) {
  while (1) {
    distF = lerDistancia(TRIG_F, ECHO_F);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void tarefaSensorTras(void *pv) {
  while (1) {
    distT = lerDistancia(TRIG_T, ECHO_T);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void tarefaSensorDireita(void *pv) {
  while (1) {
    distD = lerDistancia(TRIG_D, ECHO_D);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void tarefaSensorEsquerda(void *pv) {
  while (1) {
    distE = lerDistancia(TRIG_E, ECHO_E);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// === TAREFA ÚNICA DE CONTROLE DE PWM ===
void tarefaControleMotores(void *pv) {
  while (1) {
    int pwmF = calcularPWM(distF);
    int pwmT = calcularPWM(distT);
    int pwmD = calcularPWM(distD);
    int pwmE = calcularPWM(distE);

    ledcWrite(0, pwmF);
    ledcWrite(1, pwmT);
    ledcWrite(2, pwmD);
    ledcWrite(3, pwmE);

    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

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

  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_T, OUTPUT); pinMode(ECHO_T, INPUT);
  pinMode(TRIG_D, OUTPUT); pinMode(ECHO_D, INPUT);
  pinMode(TRIG_E, OUTPUT); pinMode(ECHO_E, INPUT);

  // LEDs simulando motores
  ledcSetup(0, PWM_FREQ, PWM_RES); ledcAttachPin(LED_F, 0);
  ledcSetup(1, PWM_FREQ, PWM_RES); ledcAttachPin(LED_T, 1);
  ledcSetup(2, PWM_FREQ, PWM_RES); ledcAttachPin(LED_D, 2);
  ledcSetup(3, PWM_FREQ, PWM_RES); ledcAttachPin(LED_E, 3);

  xTaskCreate(tarefaSensorFrente, "SensorFrente", 2048, NULL, 2, NULL);
  xTaskCreate(tarefaSensorTras, "SensorTras", 2048, NULL, 2, NULL);
  xTaskCreate(tarefaSensorDireita, "SensorDireita", 2048, NULL, 2, NULL);
  xTaskCreate(tarefaSensorEsquerda, "SensorEsquerda", 2048, NULL, 2, NULL);
  xTaskCreate(tarefaControleMotores, "ControlePWM", 4096, NULL, 1, NULL);
}

void loop() {
  Serial.printf("F: %3d | T: %3d | D: %3d | E: %3d\n", distF, distT, distD, distE);
  delay(500);
}