#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);

int count;
String FLR = "1";
void setup() {
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);

  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  digitalWrite(5, HIGH);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.clearDisplay();
  display.setCursor(20, 1);
  display.println("Please");
  display.setCursor(50, 23);
  display.println("Enter the");
  display.setCursor(33, 45);
  display.println("Elevator!");
  display.display();
  delay(1000);
}

void loop() {
  show();
  int bt1 = digitalRead(2);
  int bt2 = digitalRead(3);
  int bt3 = digitalRead(4);

  int led1 = digitalRead(5);
  int led2 = digitalRead(6);
  int led3 = digitalRead(7);

  if(bt1 == HIGH){
    if(led2 == HIGH){
      FLR = "2";
      minus();
      digitalWrite(6, LOW);
      digitalWrite(5, HIGH);
      FLR = "1";
    } else if (led3 == HIGH){
      FLR = "3";
      minus();
      digitalWrite(7, LOW);
      digitalWrite(6, HIGH);
      FLR = "2";
      minus();
      digitalWrite(6, LOW);
      digitalWrite(5, HIGH);
      FLR = "1";
    } 
  }

  else if (bt2 == HIGH){
    if(led1 == HIGH){
      plus();
      digitalWrite(5, LOW);
      digitalWrite(6, HIGH);
      FLR = "2";
    } else if (led3 == HIGH){
      FLR = "3";
      minus();
      digitalWrite(7, LOW);
      digitalWrite(6, HIGH);
      FLR = "2";
    } 
  }

  else if (bt3 == HIGH){
    if(led1 == HIGH){
      FLR = "1";
      plus();
      digitalWrite(5, LOW);
      digitalWrite(6, HIGH);
      FLR = "2";
      plus();
      digitalWrite(6, LOW);
      digitalWrite(7, HIGH);
      FLR = "3";
    } else if (led2 == HIGH){
      FLR = "2";
      plus();
      digitalWrite(6, LOW);
      digitalWrite(7, HIGH);
      FLR = "3";
    }
  }
}

void plus(){
  for(int i = 1; i <= 10; i++){
    show();
    up();
    count += 1;
    for(int i = 8; i <= 11; i++){
      digitalWrite(i, HIGH);
      delay(50);
      digitalWrite(i, LOW);
      delay(50);
    }
  }
}

void minus(){
  for(int i = 10; i >= 1; i--){
    show();
    down();
    count -= 1;
    for(int i = 8; i <= 11; i++){
      digitalWrite(i, HIGH);
      delay(50);
      digitalWrite(i, LOW);
      delay(50);
    } 
  }
}

void show(){
  display.clearDisplay();
  display.setCursor(0, 10);
  settingText();
  display.println("Floor: ");
  display.setCursor(90, 10);
  display.print(FLR);
  display.display();
}

void up(){
  display.setCursor(0,45);
  settingText();
  display.println("<<<<<");
  display.setCursor(40, 45);
  display.display();
}

void down(){
  display.setCursor(0,45);
  settingText();
  display.println(">>>>>");
  display.setCursor(80, 45);
  display.display();
}

void settingText(){
  display.setTextSize(2);
  display.setTextColor(WHITE);
}