#include <PID_v1.h>
#include <Servo.h>

// Definição de pinos
const int trigFrontPin = 2;
const int echoFrontPin = 3;
const int trigLeftPin  = 4;
const int echoLeftPin  = 5;
const int trigRightPin = 6;
const int echoRightPin = 7;
const int in1Pin = 8;
const int in2Pin = 9;
const int enaPin = 10;    // PWM controle de velocidade
const int servoPin = 11;  // PWM para servo

// Variáveis de distância
double distFront, distLeft, distRight;
double setDist = 300.0;   // setpoint em mm para distância frontal
double pidTractionOutput;
double pidDirOutput;

// Setup PID para tração e direção
double tractionInput, tractionOutput;
double directionInput, directionOutput;
PID pidTraction(&tractionInput, &tractionOutput, &setDist, 1.0, 0.1, 0.01, DIRECT);
PID pidDirection(&directionInput, &directionOutput, 0.0, 2.0, 0.0, 0.1, DIRECT);

Servo steerServo;

void setup() {
  // Configura pinos do motor
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(enaPin, OUTPUT);
  // Estado inicial: motor parado
  digitalWrite(in1Pin, LOW);
  digitalWrite(in2Pin, LOW);
  analogWrite(enaPin, 0);

  // Configura sensores ultrassônicos
  pinMode(trigFrontPin, OUTPUT); pinMode(echoFrontPin, INPUT);
  pinMode(trigLeftPin, OUTPUT);  pinMode(echoLeftPin, INPUT);
  pinMode(trigRightPin, OUTPUT); pinMode(echoRightPin, INPUT);

  // Configura servo
  steerServo.attach(servoPin);
  steerServo.write(90); // posição central (reto)

  // Inicializa PIDs
  pidTraction.SetMode(AUTOMATIC);
  pidDirection.SetMode(AUTOMATIC);
  // Permite saída PID negativa para motor inverso
  pidTraction.SetOutputLimits(-255, 255);
  // Saída de direção como ângulo (±90°)
  pidDirection.SetOutputLimits(-90, 90);
}

void loop() {
  // Leitura do sensor frontal (em mm)
  distFront = readUltrasonic(trigFrontPin, echoFrontPin);
  tractionInput = distFront;
  pidTraction.Compute();
  // Controle de direção do motor DC
  int speed = abs((int)tractionOutput);
  if (tractionOutput >= 0) {
    // distância > setpoint: mover para frente
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
  } else {
    // distância < setpoint: mover para trás
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
  }
  analogWrite(enaPin, speed);

  // Leitura dos sensores laterais
  distLeft  = readUltrasonic(trigLeftPin, echoLeftPin);
  distRight = readUltrasonic(trigRightPin, echoRightPin);
  // Erro lateral: positiva se espaço maior à esquerda
  directionInput = distLeft - distRight;
  pidDirection.Compute();
  // Converte saída PID em ângulo do servo (90° = centro)
  int angle = constrain(90 - (int)directionOutput, 0, 180);
  steerServo.write(angle);

  delay(50); // breve pausa antes da próxima leitura
}

// Função auxiliar: lê HC-SR04 (retorna distância em mm)
long 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 30ms
  if (duration == 0) {
    // Sem leitura: retorna valor alto (max 4000mm)
    return 4000;
  }
  // Tempo de ida e volta do som: distância = (duração/2)*0.343 mm/µs
  return (long)(duration * 0.1715);
}