#include <LiquidCrystal_I2C.h>
#include <Servo.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servoMotor;

#define motorSobe 16
#define motorDesce 17

// Servo
#define servoPin 13  // <- Usar outro pino se estiver no ESP32
int ultimoAndar = -1;

#define button0 19
#define button1 18
#define button2 5
#define button3 23

#define sensor0 35
#define sensor1 34
#define sensor2 39
#define sensor3 36

int Andar;
int Destino;

// Declaração da função
void giraServo(int voltas);

void setup() {
  lcd.init();
  lcd.backlight();

  pinMode(motorSobe, OUTPUT);
  pinMode(motorDesce, OUTPUT);

  pinMode(button0, INPUT);
  pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);

  pinMode(sensor0, INPUT);
  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);
  pinMode(sensor3, INPUT);

  servoMotor.attach(servoPin);
}

void loop() {
  // Ler sensores
  if (digitalRead(sensor0)) Andar = 0;
  if (digitalRead(sensor1)) Andar = 1;
  if (digitalRead(sensor2)) Andar = 2;
  if (digitalRead(sensor3)) Andar = 3;

  // Mostrar andar
  lcd.setCursor(0, 1);
  lcd.print("                ");
  lcd.setCursor(0, 1);
  lcd.print("Andar ");
  lcd.print(Andar);

  // Gira servo ao mudar de andar
  if (Andar != ultimoAndar) {
    giraServo(Andar);
    ultimoAndar = Andar;
  }

  // Botões
  if (digitalRead(button0)) Destino = 0;
  if (digitalRead(button1)) Destino = 1;
  if (digitalRead(button2)) Destino = 2;
  if (digitalRead(button3)) Destino = 3;

  // Controle motor
  if (Andar > Destino) {
    digitalWrite(motorSobe, LOW);
    digitalWrite(motorDesce, HIGH);
    lcd.setCursor(0, 0);
    lcd.print("Descendo...     ");
  } else if (Andar < Destino) {
    digitalWrite(motorSobe, HIGH);
    digitalWrite(motorDesce, LOW);
    lcd.setCursor(0, 0);
    lcd.print("Subindo...      ");
  } else {
    digitalWrite(motorSobe, LOW);
    digitalWrite(motorDesce, LOW);
    lcd.setCursor(0, 0);
    lcd.print("Parado no destino");
  }
}

// Gira o servo de acordo com o andar
void giraServo(int voltas) {
  for (int i = 0; i < voltas; i++) {
    servoMotor.write(0);
    delay(500);
    servoMotor.write(180);
    delay(500);
  }
  servoMotor.write(90); // Parar
}