#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>

Adafruit_MPU6050 mpu;
Servo myservo;

const int ledPin = 12;
const int servoPin = 14;

// ----- Sensibilidade -----
float threshold = 1.0;                    // ajuste conforme necessário
const unsigned long alertCooldown = 1000; // intervalo mínimo entre alertas

// ----- Movimento do servo (mais rápido) -----
int    servoAngle = 0;              
int    servoDir   = +1;             
const  int STEP_DEG = 3;           // passo maior → mais rápido (antes era 1)
const  unsigned long STEP_MS = 20; // intervalo menor → mais rápido (antes era 30ms)
unsigned long lastStepMs = 0;

// ----- Estado de alerta -----
bool inAlert = false;
bool servoDetached = false;
unsigned long alertStartMs = 0;
const unsigned long ALERT_PAUSE_MS  = 1600; // pausa adicional após alerta (~1,6s)
unsigned long lastAlert = 0;

// ----- Última leitura do sensor -----
float lastX = 0, lastY = 0, lastZ = 0;

void startAlert() {
  inAlert = true;
  alertStartMs = millis();
  digitalWrite(ledPin, HIGH);

  // Destaca o servo para parar movimento durante alerta
  if (myservo.attached()) {
    myservo.detach();
    servoDetached = true;
  }
}

void handleAlert() {
  unsigned long now = millis();

  // Quando terminar a janela de alerta, retoma movimento
  if (now - alertStartMs >= ALERT_PAUSE_MS) {
    digitalWrite(ledPin, LOW);

    // Reanexa o servo se foi destacado
    if (servoDetached) {
      myservo.attach(servoPin, 500, 2400);
      myservo.write(servoAngle); // volta do ponto onde parou
      servoDetached = false;
    }

    inAlert = false;
    lastAlert = now;
  }
}

void stepServoIfNeeded() {
  unsigned long now = millis();
  if (now - lastStepMs < STEP_MS) return; // ainda não é hora do próximo passo

  lastStepMs = now;

  // Atualiza ângulo e direção
  servoAngle += servoDir * STEP_DEG;
  if (servoAngle >= 180) { servoAngle = 180; servoDir = -1; }
  if (servoAngle <= 0)   { servoAngle = 0;   servoDir = +1; }

  // Só escreve se estiver anexado (fora do alerta)
  if (myservo.attached()) {
    myservo.write(servoAngle);
  }
}

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

  pinMode(ledPin, OUTPUT);

  if (!mpu.begin()) {
    Serial.println("MPU6050 não encontrado!");
    while (1) delay(10);
  }

  // Configura servo (50 Hz, limites seguros)
  myservo.setPeriodHertz(50);
  myservo.attach(servoPin, 500, 2400);
  myservo.write(servoAngle);

  // Leitura inicial do sensor
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);
  lastX = a.acceleration.x;
  lastY = a.acceleration.y;
  lastZ = a.acceleration.z;

  Serial.println("Sistema iniciado!");
}

void loop() {
  // 1) Lê o sensor e calcula deltaRMS
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  float dx = a.acceleration.x - lastX;
  float dy = a.acceleration.y - lastY;
  float dz = a.acceleration.z - lastZ;
  float deltaRMS = sqrtf(dx*dx + dy*dy + dz*dz);

  lastX = a.acceleration.x;
  lastY = a.acceleration.y;
  lastZ = a.acceleration.z;

  // Log
  Serial.print("DeltaRMS: ");
  Serial.println(deltaRMS, 3);

  // 2) Se detectar vibração e não estiver em alerta, inicia alerta
  if (!inAlert && (millis() - lastAlert) > alertCooldown && deltaRMS > threshold) {
    Serial.println("⚠ ALERTA: Vibração detectada!");
    startAlert();
  }

  // 3) Se está em alerta, gerencia alerta (não trava o loop)
  if (inAlert) {
    handleAlert();
    return; // durante alerta, não movemos o servo
  }

  // 4) Caso normal: move o servo suavemente (não-bloqueante)
  stepServoIfNeeded();

  // Pequeno respiro para reduzir carga de CPU/I2C
  delay(5);
}