#include <ESP32Servo.h>
#include <NewPing.h>

// Definições de pinos para ESP32
#define TRIG_PIN1 12
#define ECHO_PIN1 14
#define TRIG_PIN2 27
#define ECHO_PIN2 26
#define SERVO_PIN 25
#define MOTOR_IN1 32
#define MOTOR_IN2 33
#define MOTOR_IN3 22
#define MOTOR_IN4 23

// Configurações dos sensores ultrassônicos
#define MAX_DISTANCE 200
NewPing sonar1(TRIG_PIN1, ECHO_PIN1, MAX_DISTANCE);
NewPing sonar2(TRIG_PIN2, ECHO_PIN2, MAX_DISTANCE);

// Servo
Servo myServo;

// Variáveis de controle
unsigned long lastScanTime = 0;
unsigned long lastStaticTime = 0;
bool objectDetected = false;
int servoPos = 90;

// Tolerância de distância
#define DESIRED_DISTANCE 60
#define TOLERANCE 5

void setup() {
  Serial.begin(115200);
  myServo.attach(SERVO_PIN);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  pinMode(MOTOR_IN3, OUTPUT);
  pinMode(MOTOR_IN4, OUTPUT);
  myServo.write(servoPos); // Posiciona o servo em 90 graus
}

void loop() {
  unsigned long currentTime = millis();

  // Escaneia o ambiente a cada 15 segundos
  if (currentTime - lastScanTime > 15000) {
    scanEnvironment();
    lastScanTime = currentTime;
  }

  // Se um objeto for detectado
  if (objectDetected) {
    trackObject();
  }
}

void scanEnvironment() {
  for (int pos = 5; pos <= 175; pos += 5) {
    myServo.write(pos);
    delay(500);
    int distance1 = sonar1.ping_cm();
    int distance2 = sonar2.ping_cm();

    if (distance1 < DESIRED_DISTANCE || distance2 < DESIRED_DISTANCE) {
      objectDetected = true;
      lastStaticTime = millis();
      break;
    }
  }
  myServo.write(90); // Retorna o servo para a posição central
}

void trackObject() {
  int distance1 = sonar1.ping_cm();
  int distance2 = sonar2.ping_cm();

  if (abs(distance1 - distance2) < 5) {
    if (distance1 < DESIRED_DISTANCE + TOLERANCE && distance1 > DESIRED_DISTANCE - TOLERANCE) {
      stopMotors();
    } else if (distance1 < DESIRED_DISTANCE - TOLERANCE) {
      moveBackward();
    } else if (distance1 > DESIRED_DISTANCE + TOLERANCE) {
      moveForward();
    }
  } else if (distance1 < distance2) {
    turnLeft();
  } else {
    turnRight();
  }

  if (millis() - lastStaticTime > 5000) {
    objectDetected = false;
  }
}

void moveForward() {
  digitalWrite(MOTOR_IN1, HIGH);
  digitalWrite(MOTOR_IN2, LOW);
  digitalWrite(MOTOR_IN3, HIGH);
  digitalWrite(MOTOR_IN4, LOW);
}

void moveBackward() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, HIGH);
  digitalWrite(MOTOR_IN3, LOW);
  digitalWrite(MOTOR_IN4, HIGH);
}

void turnLeft() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, HIGH);
  digitalWrite(MOTOR_IN3, HIGH);
  digitalWrite(MOTOR_IN4, LOW);
}

void turnRight() {
  digitalWrite(MOTOR_IN1, HIGH);
  digitalWrite(MOTOR_IN2, LOW);
  digitalWrite(MOTOR_IN3, LOW);
  digitalWrite(MOTOR_IN4, HIGH);
}

void stopMotors() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, LOW);
  digitalWrite(MOTOR_IN3, LOW);
  digitalWrite(MOTOR_IN4, LOW);
}