#include <Wire.h>
#include <Ultrasonic.h>

// Inicialização dos sensores ultrassônicos - FRENTE
const int trigPin1 = 13;
const int echoPin1 = 12;
Ultrasonic ultrasonic1(trigPin1, echoPin1);

// ATRÁS
const int trigPin2 = 5;
const int echoPin2 = 4;
Ultrasonic ultrasonic2(trigPin2, echoPin2);

// ESQUERDA
const int trigPin3 = 19;
const int echoPin3 = 18;
Ultrasonic ultrasonic3(trigPin3, echoPin3);

// DIREITA
const int trigPin4 = 17;
const int echoPin4 = 16;
Ultrasonic ultrasonic4(trigPin4, echoPin4);


// Parâmetros de controle
const float distanciaMin = 10.0; // em cm, distância mínima segura
const float distanciaMax = 100.0; // em cm, distância máxima que afeta o controle
const int pwmMin = 50;   // PWM mínimo (duty cycle mínimo)
const int pwmMax = 255;  // PWM máximo (duty cycle máximo)

// Função para calcular o duty cycle exponencialmente
int calcularPWM(float distancia) {
  if (distancia <= distanciaMin) {
    return pwmMin;
  }
  if (distancia >= distanciaMax) {
    return pwmMax;
  }

  // Normaliza distância para [0, 1]
  float x = (distancia - distanciaMin) / (distanciaMax - distanciaMin);

  // Mapeia exponencialmente
  float pwm = pwmMin + (pwmMax - pwmMin) * pow(x, 2.5); // expoente ajustável

  return (int)pwm;
}


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

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);

  pinMode(trigPin4, OUTPUT);
}

void loop() {
  delay(200); // Tempo entre as leituras

  // Leitura das distâncias
  long distancia1 = ultrasonic1.read(CM);
  long distancia2 = ultrasonic2.read(CM);
  long distancia3 = ultrasonic3.read(CM);
  long distancia4 = ultrasonic4.read(CM);

  // Exibição no monitor serial
  Serial.println("Distâncias (cm):");
  Serial.print("Frente: ");
  Serial.print(distancia1);
  Serial.print(" | Trás: ");
  Serial.print(distancia2);
  Serial.print(" | Esquerda: ");
  Serial.print(distancia3);
  Serial.print(" | Direita: ");
  Serial.println(distancia4);

  int pwm = calcularPWM(distancia4);
  Serial.print(" cm | PWM: ");
  Serial.println(pwm);

  Serial.println("-----------------------------");
}