#include <Keypad.h>
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>

// Configuração do teclado
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {9, 8, 7, 6};
byte colPins[COLS] = {5, 4, 3, 2};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);


MPU6050 mpu;
Servo servoMotor;

// Definição de estados
enum State { STATE_IDLE, STATE_AUTH, STATE_VERIFY, STATE_MOTIONCHECK, STATE_UNLOCK };
State currentState = STATE_IDLE;

String enteredPassword = "";
const String correctPassword = "1234";

unsigned long motionStartTime = 0;
bool passwordValid = false;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("Falha ao conectar com MPU6050");
    while (1);
  }

  servoMotor.attach(10);
  servoMotor.write(0); // Inicialmente trancado
}

void loop() {
  switch (currentState) {
    case STATE_IDLE:
      Serial.println("Estado: IDLE - Pressione * para iniciar");
      if (keypad.getKey() == '*') {
        enteredPassword = "";
        currentState = STATE_AUTH;
      }
      delay(500);
      break;

    case STATE_AUTH:
      Serial.println("Digite a senha:");
      readPassword();
      if (enteredPassword.length() == 4) {
        currentState = STATE_VERIFY;
      }
      break;

    case STATE_VERIFY:
      if (enteredPassword == correctPassword) {
        Serial.println("Senha correta!");
        passwordValid = true;
        currentState = STATE_MOTIONCHECK;
      } else {
        Serial.println("Senha incorreta!");
        passwordValid = false;
        currentState = STATE_IDLE;
      }
      delay(1000);
      break;

    case STATE_MOTIONCHECK:
      Serial.println("Verificando movimento...");
      if (checkMotion()) {
        currentState = STATE_UNLOCK;
      } else {
        Serial.println("Movimento detectado, acesso negado.");
        currentState = STATE_IDLE;
      }
      delay(500);
      break;

    case STATE_UNLOCK:
      Serial.println("Porta liberada por 5 segundos.");
      servoMotor.write(90);
      delay(5000);
      servoMotor.write(0);
      currentState = STATE_IDLE;
      delay(500);
      break;
  }
}

void readPassword() {
  char key = keypad.getKey();
  if (key && isDigit(key)) {
    enteredPassword += key;
    Serial.print("*"); // Feedback
    delay(300); // Evitar leitura dupla
  }
}

bool checkMotion() {
  motionStartTime = millis();
  while (millis() - motionStartTime < 2000) {
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);

    float acceleration = sqrt(sq(ax) + sq(ay) + sq(az)) / 16384.0;

    if (abs(acceleration - 1.0) > 0.05) { // Movimento detectado
      return false;
    }
    delay(100);
  }
  return true; // Parado
}