#include <Wire.h>
#include <U8x8lib.h>
#include <Keypad.h>

// =====================================================
// OLED
// =====================================================

U8X8_SSD1306_128X64_NONAME_HW_I2C oled(
  U8X8_PIN_NONE
);

// =====================================================
// LED
// =====================================================

#define LED_RED    PB1
#define LED_YELLOW PA11
#define LED_BLUE   PA12

// =====================================================
// RELAY + BUZZER
// =====================================================

#define MOTOR_RELAY PA1
#define BUZZER      PA2

// =====================================================
// KEYPAD
// =====================================================

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] = {

  PA15,
  PB5,
  PB4,
  PB10
};

byte colPins[COLS] = {

  PB3,
  PA10,
  PB6,
  PB7
};

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

// =====================================================
// DATA
// =====================================================

int currentFloor = 1;

int targetFloor = 1;

bool moving = false;

bool fireAlarm = false;

bool doorsOpen = false;

unsigned long moveTimer = 0;

// =====================================================
// SETUP
// =====================================================

void setup() {

  pinMode(LED_RED, OUTPUT);

  pinMode(LED_YELLOW, OUTPUT);

  pinMode(LED_BLUE, OUTPUT);

  pinMode(MOTOR_RELAY, OUTPUT);

  pinMode(BUZZER, OUTPUT);

  // OLED I2C

  Wire.setSDA(PB9);

  Wire.setSCL(PB8);

  Wire.begin();

  oled.begin();

  oled.setPowerSave(0);

  oled.setFont(
    u8x8_font_chroma48medium8_r
  );

  // START

  beep(100);

  startupScreen();
}

// =====================================================
// LOOP
// =====================================================

void loop() {

  keypadControl();

  moveLift();

  updateOLED();

  delay(20);
}

// =====================================================
// START SCREEN
// =====================================================

void startupScreen() {

  oled.clear();

  oled.drawString(0,1,"SMART LIFT");

  oled.drawString(0,3,"SYSTEM READY");

  delay(1500);
}

// =====================================================
// KEYPAD
// =====================================================

void keypadControl() {

  char k = keypad.getKey();

  if (!k) return;

  beep(30);

  // =================================================
  // FIRE MODE
  // =================================================

  if (fireAlarm) {

    if (k == 'D') {

      resetAlarm();
    }

    return;
  }

  // =================================================
  // FLOOR SELECT
  // =================================================

  if (k >= '1' && k <= '9') {

    targetFloor = k - '0';

    if (targetFloor != currentFloor) {

      moving = true;

      doorsOpen = false;
    }
  }

  // =================================================
  // OPEN DOOR
  // =================================================

  if (k == 'A') {

    openDoors();
  }

  // =================================================
  // CLOSE DOOR
  // =================================================

  if (k == 'B') {

    closeDoors();
  }

  // =================================================
  // FIRE ALARM
  // =================================================

  if (k == 'C') {

    fireMode();
  }
}

// =====================================================
// MOVE LIFT
// =====================================================

void moveLift() {

  if (fireAlarm) {

    targetFloor = 1;

    if (currentFloor != 1)
      moving = true;
  }

  if (!moving) return;

  if (millis() - moveTimer < 2000)
    return;

  moveTimer = millis();

  digitalWrite(MOTOR_RELAY, HIGH);

  digitalWrite(LED_BLUE, HIGH);

  doorsOpen = false;

  movingScreen();

  beep(50);

  // UP/DOWN

  if (currentFloor < targetFloor)
    currentFloor++;

  else if (currentFloor > targetFloor)
    currentFloor--;

  // ARRIVED

  if (currentFloor == targetFloor) {

    moving = false;

    digitalWrite(MOTOR_RELAY, LOW);

    digitalWrite(LED_BLUE, LOW);

    beep(200);

    openDoors();

    delay(5000);

    if (!fireAlarm)
      closeDoors();
  }
}

// =====================================================
// OPEN DOORS
// =====================================================

void openDoors() {

  doorsOpen = true;

  digitalWrite(LED_YELLOW, HIGH);

  oled.clear();

  oled.drawString(0,1,"DOORS OPEN");

  oled.drawString(0,3,"[      ]");

  oled.drawString(1,3,"<    >");

  delay(1000);
}

// =====================================================
// CLOSE DOORS
// =====================================================

void closeDoors() {

  doorsOpen = false;

  digitalWrite(LED_YELLOW, LOW);

  oled.clear();

  oled.drawString(0,1,"DOORS CLOSE");

  oled.drawString(0,3,"<      >");

  oled.drawString(2,3,"[][]");

  delay(1000);
}

// =====================================================
// MOVING SCREEN
// =====================================================

void movingScreen() {

  oled.clear();

  oled.drawString(0,1,"LIFT MOVING");

  oled.drawString(0,3,"==========");

  oled.drawString(0,5,"PLEASE WAIT");
}

// =====================================================
// FIRE MODE
// =====================================================

void fireMode() {

  fireAlarm = true;

  moving = false;

  targetFloor = 1;

  digitalWrite(LED_RED, HIGH);

  oled.clear();

  oled.drawString(0,1,"!!! FIRE !!!");

  oled.drawString(0,3,"GO TO FLOOR1");

  // SIREN

  for(int i=0;i<5;i++) {

    beep(150);

    delay(150);
  }
}

// =====================================================
// RESET
// =====================================================

void resetAlarm() {

  fireAlarm = false;

  moving = false;

  digitalWrite(LED_RED, LOW);

  digitalWrite(MOTOR_RELAY, LOW);

  digitalWrite(LED_BLUE, LOW);

  oled.clear();

  oled.drawString(0,2,"SYSTEM RESET");

  beep(200);

  delay(1000);
}

// =====================================================
// OLED MAIN
// =====================================================

void updateOLED() {

  static unsigned long oledTimer = 0;

  if (millis() - oledTimer < 300)
    return;

  oledTimer = millis();

  oled.clear();

  oled.setCursor(0,0);

  oled.print("FLOOR: ");

  oled.print(currentFloor);

  oled.setCursor(0,2);

  oled.print("TARGET:");

  oled.print(targetFloor);

  oled.setCursor(0,4);

  if (fireAlarm) {

    oled.print("FIRE MODE");
  }
  else if (moving) {

    oled.print("MOVING");
  }
  else if (doorsOpen) {

    oled.print("DOORS OPEN");
  }
  else {

    oled.print("READY");
  }
}

// =====================================================
// BUZZER
// =====================================================

void beep(int t) {

  digitalWrite(BUZZER, HIGH);

  delay(t);

  digitalWrite(BUZZER, LOW);
}