#include <ESP32Servo.h> 
#include <limits.h> 
#include <algorithm> 
#include <cmath>     

// ----------------------
// Configuração do HC-SR04
// ----------------------
const int TRIG_PIN = 13; 
const int ECHO_PIN = 12; 

// ----------------------
// Configuração dos Servos e Laser
// ----------------------
const int SERVO_PIN_PAN = 26; // Servo 1: VARREDURA CONTÍNUA (Radar)
Servo servoPan;

const int SERVO_PIN_TILT = 32; // Servo 2: MIRA TRAVADA POR EVENTO (Mira)
Servo servoTilt;

const int LASER_PIN = 4; // Pino para o Laser

// ----------------------
// Parâmetros
// ----------------------
const int ANGULO_INICIAL = 0;
const int ANGULO_FINAL = 180;

// Velocidade do Radar (PAN)
const int PASSO_VARREDURA = 5;      
// REMOVIDO: PASSO_MIRA_TILT não é mais necessário para movimento máximo

const int DISTANCIA_MAX_ALVO = 400; 
const int DISTANCIA_MIN_ALVO = 2;   
const long INT_MAX_SAFE = 401;      

const int LIMITE_VARIACAO_CM = 10;  // Variação mínima em CM para acionar o evento de mira.
const int DELAY_LOOP = 20;          // Acelera a taxa de processamento/varredura

// Variáveis de Estado
int anguloAtualPan = 90; 
int anguloAtualTilt = 90; 
int direcaoVarredura = 1; 

// Variáveis de Rastreamento de Evento
int anguloDoAlvoTravado = 90;           
long ultimaDistanciaLida = INT_MAX_SAFE; 
long distanciaDoEvento = INT_MAX_SAFE;   

// ----------------------------------------------------------------------
// FUNÇÕES AUXILIARES (Não alteradas)
// ----------------------------------------------------------------------

void moverServo(Servo& servo, int& anguloAtual, int novoAngulo) {
    if (novoAngulo < ANGULO_INICIAL) {
        novoAngulo = ANGULO_INICIAL;
    } else if (novoAngulo > ANGULO_FINAL) {
        novoAngulo = ANGULO_FINAL;
    }
    
    // O comando só é enviado se o ângulo for diferente, garantindo o travamento
    if (novoAngulo != anguloAtual) {
        servo.write(novoAngulo);
        anguloAtual = novoAngulo;
    }
}

long medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duracao = pulseIn(ECHO_PIN, HIGH, 30000); 
  
  if (duracao == 0) return INT_MAX_SAFE; 
  
  long distancia = duracao / 58;
  
  if (distancia > DISTANCIA_MAX_ALVO || distancia < DISTANCIA_MIN_ALVO) {
    return INT_MAX_SAFE;
  }
  return distancia;
}

// ----------------------------------------------------------------------
// SETUP E LOOP
// ----------------------------------------------------------------------

void setup() {
  Serial.begin(115200);
  Serial.println("Sistema Radar (PAN) e Mira por Eventos (TILT) de Velocidade Máxima Iniciado");

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LASER_PIN, OUTPUT);
  
  servoPan.attach(SERVO_PIN_PAN);
  servoTilt.attach(SERVO_PIN_TILT); 
  
  // Movimento inicial dos servos para a posição central (90 graus)
  moverServo(servoPan, anguloAtualPan, 90);
  moverServo(servoTilt, anguloAtualTilt, 90); 
  digitalWrite(LASER_PIN, LOW); 
  
  delay(1000); 
}

void loop() {
    // --- 1. LEITURA DE DISTÂNCIA E DETECÇÃO DE EVENTOS ---
    long distanciaAtual = medirDistancia(); 
    bool eventoDetectado = false;
    
    // Verifica se houve uma transição de/para "fora de alcance" ou uma mudança significativa em CM
    if (ultimaDistanciaLida != INT_MAX_SAFE || distanciaAtual != INT_MAX_SAFE) {
        
        if (ultimaDistanciaLida == INT_MAX_SAFE || distanciaAtual == INT_MAX_SAFE) {
            // Transição para/de 'N/A' (alvo apareceu/desapareceu) é um evento
            eventoDetectado = true;
        } 
        else if (std::abs(distanciaAtual - ultimaDistanciaLida) >= LIMITE_VARIACAO_CM) {
            // Mudança em CM maior que o limite é um evento
            eventoDetectado = true;
        }
    }

    // --- 2. AÇÃO DE TRAVAMENTO POR EVENTO (Atualização do TILT) ---
    if (eventoDetectado) {
        // Um evento de mudança ocorreu!
        anguloDoAlvoTravado = anguloAtualPan;
        distanciaDoEvento = distanciaAtual; 
        
        Serial.print(">>> EVENTO DE MUDANÇA (");
        Serial.print(distanciaAtual);
        Serial.print("cm) detectado em: ");
        Serial.println(anguloDoAlvoTravado);
    }
    
    // Atualiza a última distância lida para o próximo ciclo
    ultimaDistanciaLida = distanciaAtual;

    // --- 3. VARREDURA CONTÍNUA DO SERVO PAN (Radar) ---
    
    int anguloAlvoPan = anguloAtualPan + (direcaoVarredura * PASSO_VARREDURA);
    
    // Verifica os limites da varredura (0 a 180)
    if (anguloAlvoPan >= ANGULO_FINAL) {
        direcaoVarredura = -1; 
        anguloAlvoPan = ANGULO_FINAL;
        Serial.println("--- 180° ALCANÇADO ---");
        
    } else if (anguloAlvoPan <= ANGULO_INICIAL) {
        direcaoVarredura = 1; 
        anguloAlvoPan = ANGULO_INICIAL;
        Serial.println("--- 0° ALCANÇADO ---");
    }
    
    moverServo(servoPan, anguloAtualPan, anguloAlvoPan);
    
    // --- 4. APONTAMENTO DO SERVO TILT (Mira Travada - Velocidade Máxima) ---
    
    if (distanciaDoEvento != INT_MAX_SAFE) {
        // Alvo válido: TILT se move DIRETAMENTE para o ângulo travado
        
        // NOVO CÓDIGO: Movimento direto e instantâneo, sem passos.
        // O servo se moverá o mais rápido que sua mecânica permitir até o alvo.
        moverServo(servoTilt, anguloAtualTilt, anguloDoAlvoTravado);
        
        // Ativa o Laser quando o TILT estiver na posição alvo
        if (anguloAtualTilt == anguloDoAlvoTravado) {
            digitalWrite(LASER_PIN, HIGH);
        } else {
            digitalWrite(LASER_PIN, LOW);
        }
        
    } else {
        // Nenhuma leitura válida ocorreu (estado inicial ou alvo perdido)
        digitalWrite(LASER_PIN, LOW);
        moverServo(servoTilt, anguloAtualTilt, 90);
    }
    
    // --- LOGS E DELAY ---
    Serial.print("PAN (Radar): ");
    Serial.print(anguloAtualPan);
    Serial.print(", TILT (Mira): ");
    Serial.print(anguloAtualTilt);
    Serial.print(" (Trava em: ");
    Serial.print(anguloDoAlvoTravado);
    Serial.print(" | Dist. Evento: ");
    Serial.print(distanciaDoEvento == INT_MAX_SAFE ? "N/A" : String(distanciaDoEvento) + "cm");
    Serial.print(" | Laser: ");
    Serial.println(digitalRead(LASER_PIN) == HIGH ? "ON" : "OFF");

    delay(DELAY_LOOP);
}