/*
 * Controlador PID de Distância - Baseado em HC-SR04
 *
 * Utiliza um controlador PID para manter uma "distância ideal"
 * ajustada por um potenciômetro. A saída do PID controla o brilho
 * do LED e a frequência do bipe.
 *
 * Refatorado por Gemini
 */

#include <PID_v1.h>

#define ECHO_PIN 2
#define TRIG_PIN 3
#define SPEAKER_PIN 8
#define LED_PIN 13
#define POTENTIOMETER_PIN A1

// Limites de saída do PID
#define MIN_PID_OUTPUT 0
#define MAX_PID_OUTPUT 255

// Variáveis para o PID
double pid_input;    // Distância atual lida
double pid_output;   // Saída do PID (controle)
double pid_setpoint; // Distância ideal (ajustada pelo potenciômetro)

// Constantes de ajuste do PID
// Estes valores podem precisar de "tuning" para a sua aplicação
const double Kp = 1.0;
const double Ki = 0.05;
const double Kd = 0.5;

// Cria a instância do objeto PID
PID myPID(&pid_input, &pid_output, &pid_setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(SPEAKER_PIN, OUTPUT);
  pinMode(POTENTIOMETER_PIN, INPUT);

  // Configura o PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(MIN_PID_OUTPUT, MAX_PID_OUTPUT);
}

// Função para ler a distância em centímetros
float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  // 1. Lê a distância atual e o setpoint
  pid_input = readDistanceCM();
  
  // Mapeia o valor do potenciômetro (0-1023) para uma distância ideal (0-200 cm)
  pid_setpoint = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 0, 200);

  // 2. Calcula a nova saída do PID
  myPID.Compute();

  // 3. Aplica a saída do PID
  // Para o LED, um erro maior (saída do PID) resulta em mais brilho
  // A saída do PID é mapeada para o brilho (0-255)
  analogWrite(LED_PIN, pid_output);
  
  // Para o speaker, um erro maior resulta em uma frequência mais alta
  // A saída do PID é mapeada para uma frequência audível
  int speakerFrequency = map(pid_output, 0, 255, 100, 1000);
  
  // Toca o tom se a saída do PID for maior que zero
  if (pid_output > 0) {
    tone(SPEAKER_PIN, speakerFrequency);
  } else {
    noTone(SPEAKER_PIN);
  }

  // 4. Mostra os valores no Serial Monitor para depuração
  Serial.print("Distancia: ");
  Serial.print(pid_input);
  Serial.print(" cm | Setpoint: ");
  Serial.print(pid_setpoint);
  Serial.print(" cm | Saida PID: ");
  Serial.println(pid_output);

  // Pequeno delay para evitar leituras muito rápidas
  delay(100);
}