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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define LED_RED   PB0
#define LED_GREEN PB1
#define LED_BLUE  PA7

#define BTN_OPEN      PA4
#define BTN_EMERGENCY PA5

#define BUZZER PA6

Servo doorServo;

// ================= RGB =================

void setGreen() {
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_BLUE, LOW);
}

void setRed() {
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_BLUE, LOW);
}

void setBlue() {
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_BLUE, HIGH);
}

// ================= OLED TEXT =================

void showStandby() {

  oled.clearDisplay();

  oled.setTextSize(2);
  oled.setTextColor(SSD1306_WHITE);

  String line1 = "SELAMAT";
  String line2 = "DATANG";

  int16_t x1, y1;
  uint16_t w, h;

  oled.getTextBounds(line1, 0, 0, &x1, &y1, &w, &h);
  oled.setCursor((128 - w) / 2, 12);
  oled.print(line1);

  oled.getTextBounds(line2, 0, 0, &x1, &y1, &w, &h);
  oled.setCursor((128 - w) / 2, 36);
  oled.print(line2);

  oled.display();
}

void showEmergency() {

  oled.clearDisplay();

  oled.setTextSize(2);
  oled.setTextColor(SSD1306_WHITE);

  oled.setCursor(5, 24);
  oled.print("EMERGENCY");

  oled.display();
}

// ================= STICKMAN =================

void drawStickman(int x, int frame)
{
  oled.clearDisplay();

  int headY = 15 + (frame % 2);

  // Kepala
  oled.drawCircle(x, headY, 4, SSD1306_WHITE);

  // Badan
  oled.drawLine(x, headY + 4, x, headY + 19, SSD1306_WHITE);

  switch(frame)
  {
    case 0:
      oled.drawLine(x, headY+10, x-6, headY+5, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x+6, headY+15, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x-7, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x+2, headY+30, SSD1306_WHITE);
      break;

    case 1:
      oled.drawLine(x, headY+10, x-5, headY+6, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x+5, headY+14, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x-5, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x+4, headY+30, SSD1306_WHITE);
      break;

    case 2:
      oled.drawLine(x, headY+10, x-3, headY+8, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x+3, headY+12, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x-3, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x+6, headY+30, SSD1306_WHITE);
      break;

    case 3:
      oled.drawLine(x, headY+10, x-1, headY+10, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x+1, headY+10, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x-1, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x+7, headY+30, SSD1306_WHITE);
      break;

    case 4:
      oled.drawLine(x, headY+10, x+6, headY+5, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x-6, headY+15, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x+7, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x-2, headY+30, SSD1306_WHITE);
      break;

    case 5:
      oled.drawLine(x, headY+10, x+5, headY+6, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x-5, headY+14, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x+5, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x-4, headY+30, SSD1306_WHITE);
      break;

    case 6:
      oled.drawLine(x, headY+10, x+3, headY+8, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x-3, headY+12, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x+3, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x-6, headY+30, SSD1306_WHITE);
      break;

    case 7:
      oled.drawLine(x, headY+10, x+1, headY+10, SSD1306_WHITE);
      oled.drawLine(x, headY+10, x-1, headY+10, SSD1306_WHITE);

      oled.drawLine(x, headY+19, x+1, headY+30, SSD1306_WHITE);
      oled.drawLine(x, headY+19, x-7, headY+30, SSD1306_WHITE);
      break;
  }

  oled.display();
}

void walkingAnimation()
{
  uint8_t frame = 0;

  for(int x = -10; x < 138; x += 6)
  {
    drawStickman(x, frame);

    frame++;
    if(frame >= 8)
      frame = 0;

    delay(5);
  }
}

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

void emergencyAlarm()
{
  for(int i = 0; i < 5; i++)
  {
    tone(BUZZER, 1000);
    delay(300);

    noTone(BUZZER);
    delay(200);
  }
}

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

void setup()
{
  pinMode(BTN_OPEN, INPUT_PULLUP);
  pinMode(BTN_EMERGENCY, INPUT_PULLUP);

  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);

  pinMode(BUZZER, OUTPUT);

  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C))
  {
    while(1);
  }

  doorServo.attach(PA0);
  doorServo.write(0);

  setBlue();
  showStandby();
}

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

void loop()
{
  if(digitalRead(BTN_OPEN) == LOW)
  {
    delay(200);

    setGreen();

    doorServo.write(90);

    walkingAnimation();

    delay(1000);

    doorServo.write(0);

    setBlue();
    showStandby();
  }

  if(digitalRead(BTN_EMERGENCY) == LOW)
  {
    delay(200);

    setRed();

    showEmergency();

    emergencyAlarm();

    delay(2000);

    setBlue();
    showStandby();
  }
}