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

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

Stepper stepper(200, 10, 11, 12, 13);
int floorBefore = 1; //ini

// // Inisialisasi pin
const int btnLantai1 = 2;
const int btnLantai2 = 3;
// const int btnLantai3 = 0;
// const int btnLantai4 = 1;

const int btnOpen = 4;
const int btnClose = 5;

// const int ledPin = 9;
const int buzzerPin = 7;
const int servoPin = 9;





void setup() {
  Serial.begin(9600);

  lcd.init();
  doorServo.attach(servoPin);
  // stepper.setSpeed(500); //max
  stepper.setSpeed(50);

  // pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  
  pinMode(btnLantai1, INPUT_PULLUP);
  pinMode(btnLantai2, INPUT_PULLUP);
  // pinMode(btnLantai3, INPUT_PULLUP);
  // pinMode(btnLantai4, INPUT_PULLUP);
  pinMode(btnOpen, INPUT_PULLUP);
  pinMode(btnClose, INPUT_PULLUP);
}


void loop() {
  // Cek status push button dan lakukan aksi sesuai kondisi
  checkDoor();
  checkButton(btnLantai1, "1");
  checkButton(btnLantai2, "2");
//   checkButton(btnLantai3, "3");
//   checkButton(btnLantai4, "4");
  
}

// Fungsi untuk mengecek status push button lantai
void checkButton(int buttonPin, String floor) {
  if (digitalRead(buttonPin) == LOW) {
    if (doorServo.read() == 0){
      doorAction("close");
    }
    moveStepper(floor);
    doorAction("open");
    delay(1000);
    doorAction("close");
  }
}

void checkDoor(){ //oke
  if (digitalRead(btnOpen) == LOW) {
    doorAction("open");
    delay(1000);
    doorAction("close");
  }
  if (digitalRead(btnClose) == LOW) {
    doorAction("close");
  }
}

// Fungsi untuk menggerakkan stepper motor sesuai dengan lantai
void moveStepper(String floor) {
  int floorAfter = floor.toInt();

  // int steps = floorAfter * 200; // 200 steps per rev
  int steps = abs(floorAfter - floorBefore) * 200;
  
  lcd.setCursor(0,1);
  if (floorBefore < floorAfter){
    stepper.step(steps);  
    lcd.print("( Lift Naik )"); //suara
  } 
  if (floorBefore > floorAfter){
    stepper.step(-steps);
    lcd.print("( Lift Turun )"); //suara
  }

  // Mengatur LED dan buzzer selama motor bergerak
//   digitalWrite(ledPin, HIGH);
  tone(buzzerPin, 1000);
  delay(500);
//   digitalWrite(ledPin, LOW);
  noTone(buzzerPin);
  lcd.setCursor(0,0);
  lcd.print("* Lantai " + floor + " *");

  floorBefore = floorAfter;
  delay(1000); //hapus
  lcd.clear(); //hapus
}

// Fungsi untuk menggerakkan servo pintu
void doorAction(String action) {
  lcd.setCursor(0,1);
  
  if (action == "open") {
    doorServo.write(0);
    lcd.print("( Pintu Dibuka )"); //suara
  } else if (action == "close") {
    doorServo.write(180);
    lcd.print("( Pintu Ditutup )"); //suara
  }
  delay(1000);
  lcd.clear();
}





/*
Cara kerja:
1. Pintu di tutup
2. Ke lantai atas/bawah
3. Lantai X, 
4. Pintu dibuka

Teknis:
1. PB X di tekan, (servo on) pintu ditutup 
   > Motor on > buzzer on > (servo on) Pintu dibuka
2. PB Buka/tutup di tekan, servo on

Minor:
- Belum bisa include suara, DFPlayer simulator gak punya

*/