#define TRIG_PIN_FRONTAL 18
#define ECHO_PIN_FRONTAL 19
#define TRIG_PIN_TRASEIRO 14
#define ECHO_PIN_TRASEIRO 17
#define TRIG_PIN_ESQUERDO 16
#define ECHO_PIN_ESQUERDO 34
#define TRIG_PIN_DIREITO 23
#define ECHO_PIN_DIREITO 22

#define LED_FRENTE_DIREITA_PIN 27
#define LED_RE_FRENTE_DIREITA_PIN 26
#define LED_FRENTE_ESQUERDA_PIN 32
#define LED_RE_FRENTE_ESQUERDA_PIN 33
#define LED_TRAZ_ESQUERDA_PIN 13
#define LED_RE_TRAZ_ESQUERDA_PIN 21
#define LED_TRAZ_DIREITA_PIN 4
#define LED_RE_TRAZ_DIREITA_PIN 25

float distanciaFrontal;
float distanciaTraseira;
float distanciaEsquerda;
float distanciaDireita;
void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN_FRONTAL, OUTPUT);
  pinMode(ECHO_PIN_FRONTAL, INPUT);
  pinMode(TRIG_PIN_TRASEIRO, OUTPUT);
  pinMode(ECHO_PIN_TRASEIRO, INPUT);
  pinMode(TRIG_PIN_ESQUERDO, OUTPUT);
  pinMode(ECHO_PIN_ESQUERDO, INPUT);
  pinMode(TRIG_PIN_DIREITO, OUTPUT);
  pinMode(ECHO_PIN_DIREITO, INPUT);
  ledcAttach(LED_FRENTE_DIREITA_PIN, 5000, 8);  // canal, freq, resolução
  ledcAttach(LED_RE_FRENTE_DIREITA_PIN,     5000, 8);
  
  ledcAttach(LED_FRENTE_ESQUERDA_PIN, 5000, 8);  // canal, freq, resolução
  ledcAttach(LED_RE_FRENTE_ESQUERDA_PIN,     5000, 8);

  ledcAttach(LED_TRAZ_ESQUERDA_PIN, 5000, 8);  // canal, freq, resolução
  ledcAttach(LED_RE_TRAZ_ESQUERDA_PIN,     5000, 8);

  ledcAttach(LED_TRAZ_DIREITA_PIN, 5000, 8);  // canal, freq, resolução
  ledcAttach(LED_RE_TRAZ_DIREITA_PIN,     5000, 8);



}

void loop() {
  medirDistanciaFrontal();
  medirDistanciaTraseira();
  medirDistanciaEsquerda();
  medirDistanciaDireita();
  Serial.print("Distância frontal (cm): ");
  Serial.println(distanciaFrontal);
  Serial.print("Distância Traseira (cm): ");
  Serial.println(distanciaTraseira);
  Serial.print("Distância Esquerda (cm): ");
  Serial.println(distanciaEsquerda);
  Serial.print("Distância Direita (cm): ");
  Serial.println(distanciaDireita);
  controlarMotores();
  delay(300);
}

// === Função modular para o sensor frontal ===
void medirDistanciaFrontal() {
  unsigned long t0 = micros();
  digitalWrite(TRIG_PIN_FRONTAL, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_FRONTAL, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_FRONTAL, LOW);

  long duracao = pulseIn(ECHO_PIN_FRONTAL, HIGH, 30000);  // Timeout 30ms
  float distancia = duracao * 0.0343 / 2.0;
  unsigned long t1 = micros();
  Serial.println("=== MEDIÇÃO INTERNA ===");
  Serial.print("Execução: ");
  Serial.print(t1 - t0);
  Serial.println(" us");
  
  distanciaFrontal = distancia;
}

void medirDistanciaTraseira() {
  unsigned long t0 = micros();
  digitalWrite(TRIG_PIN_TRASEIRO, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_TRASEIRO, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_TRASEIRO, LOW);

  long duracao = pulseIn(ECHO_PIN_TRASEIRO, HIGH, 30000);  // Timeout 30ms
  float distancia = duracao * 0.0343 / 2.0;
  unsigned long t1 = micros();
  Serial.println("=== MEDIÇÃO INTERNA ===");
  Serial.print("Execução: ");
  Serial.print(t1 - t0);
  Serial.println(" us");
  
  distanciaTraseira = distancia;
}

void medirDistanciaEsquerda() {
  unsigned long t0 = micros();
  digitalWrite(TRIG_PIN_ESQUERDO, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_ESQUERDO, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_ESQUERDO, LOW);

  long duracao = pulseIn(ECHO_PIN_ESQUERDO, HIGH, 30000);  // Timeout 30ms
  float distancia = duracao * 0.0343 / 2.0;
  unsigned long t1 = micros();
  Serial.println("=== MEDIÇÃO INTERNA ===");
  Serial.print("Execução: ");
  Serial.print(t1 - t0);
  Serial.println(" us");
  
  distanciaEsquerda = distancia;
}

void medirDistanciaDireita() {
  unsigned long t0 = micros();
  digitalWrite(TRIG_PIN_DIREITO, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_DIREITO, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_DIREITO, LOW);

  long duracao = pulseIn(ECHO_PIN_DIREITO, HIGH, 30000);  // Timeout 30ms
  float distancia = duracao * 0.0343 / 2.0;
  unsigned long t1 = micros();
  Serial.println("=== MEDIÇÃO INTERNA ===");
  Serial.print("Execução: ");
  Serial.print(t1 - t0);
  Serial.println(" us");
  
  distanciaDireita = distancia;
}


int calcularPWM(float distancia_cm) {
  if (distancia_cm <= 0) return 0;
  if (distancia_cm > 400) return 255;

  float k = 0.03;  // sensibilidade da exponencial
  float pwm = 255 * (1 - exp(-k * distancia_cm));

  return (int)constrain(pwm, 0, 255);
}

// === FUNÇÃO DE CONTROLE DO MOTOR FRONTAL (2 LEDs) ===
void controlarMotores() {
  int pwm_frente = calcularPWM(distanciaFrontal);
  int pwm_traz = calcularPWM(distanciaTraseira);
  bool frente = distanciaFrontal > 30;  // limiar de decisão

  if (frente) {
    ledcWrite(LED_FRENTE_DIREITA_PIN, pwm_frente);
    ledcWrite(LED_RE_FRENTE_DIREITA_PIN, 0);

    ledcWrite(LED_FRENTE_ESQUERDA_PIN, pwm_frente);
    ledcWrite(LED_RE_FRENTE_ESQUERDA_PIN, 0);

    ledcWrite(LED_TRAZ_DIREITA_PIN, pwm_frente);
    ledcWrite(LED_RE_TRAZ_DIREITA_PIN, 0);

    ledcWrite(LED_TRAZ_ESQUERDA_PIN, 0);
    ledcWrite(LED_RE_TRAZ_ESQUERDA_PIN, 0);
  } else {
    ledcWrite(LED_FRENTE_DIREITA_PIN, 0);
    ledcWrite(LED_RE_FRENTE_DIREITA_PIN, pwm_traz);

    ledcWrite(LED_FRENTE_ESQUERDA_PIN, 0);
    ledcWrite(LED_RE_FRENTE_ESQUERDA_PIN, pwm_traz);

    ledcWrite(LED_TRAZ_DIREITA_PIN, 0);
    ledcWrite(LED_RE_TRAZ_DIREITA_PIN, pwm_traz);

    ledcWrite(LED_TRAZ_ESQUERDA_PIN, 0);
    ledcWrite(LED_RE_TRAZ_ESQUERDA_PIN, pwm_traz);
  }

  Serial.print("Distância: ");
  Serial.print(distanciaFrontal);
  Serial.print(" cm | PWM: ");
  Serial.print(pwm_frente, pwm_traz);
  Serial.print(" | Sentido: ");
  Serial.println(frente ? "Frente" : "Ré");
}