#include <Arduino.h>
#include <Bounce2.h>
#include <LiquidCrystal_I2C.h>

#define DEBUG

#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define NUM_SWITCHES 3
#define NUM_BUTTONS 3
#define NUM_DOORS 3
#define NUM_RELAYS 2

#define DOOR_TIME 5 // time in seconds

const int endSwitchPin[NUM_SWITCHES] = {A2, A1, A0};

const int buttonPin[NUM_SWITCHES] = {5, 6, 7};

const int relayPin[NUM_RELAYS] = {4, 3};

const int doorPin[NUM_DOORS] = {10, 9, 8};

byte parkedAt = 0;
byte callingFloor = 0;

unsigned long doorMillis = 0;

bool doorOpened[NUM_DOORS] = {false};
bool moving = false;

Bounce2::Button endSwitch[NUM_SWITCHES] = {Bounce2::Button()};
Bounce2::Button button[NUM_BUTTONS] = {Bounce2::Button()};

#ifdef DEBUG
  #define LOG(...) Serial.print(__VA_ARGS__)
  #define LOGLN(...) Serial.println(__VA_ARGS__)
#else
  #define LOG(...)
  #define LOGLN(...)
#endif

void openDoor(byte pin)
{
  if(doorOpened[pin] == false)
  {
    digitalWrite(doorPin[pin], HIGH);
    doorOpened[pin] = true;
  }
}

void closeDoor(byte pin)
{
  if(doorOpened[pin] == true)
  {
    digitalWrite(doorPin[pin], LOW);
    doorOpened[pin] = false;
  }
}

void checkSwitches(bool onSetup = false)
{
  byte endOpen = 0;

  if(moving == false)
  {
    for (byte i = 0; i < NUM_SWITCHES; i++)
    {
      endSwitch[i].update();

      int switchState = endSwitch[i].read();

      if (switchState == LOW)
      {
        parkedAt = i;
      }
      else
      {
        endOpen++;
      }
    }

    if(endOpen != (NUM_DOORS - 1))
    {
      LOGLN(F("Elevator not found"));
      LOG(F("endOpen:"));
      LOGLN(endOpen);

      lcd.setCursor(0, 0);
      lcd.print("Eleva. not found");
      lcd.setCursor(0, 1);
      lcd.print("                ");

      for (byte i = 0; i < NUM_DOORS; i++)
      {
        closeDoor(i);
      }

      while (1)
      {
      }
    }

    if(onSetup == true)
    {
      lcd.setCursor(0, 0);
      lcd.print("Parked at floor:");
      lcd.setCursor(7, 1);
      lcd.print(parkedAt);
    }
  }
  else
  {
    endSwitch[callingFloor].update();
    endSwitch[parkedAt].update();

    int callingSwitchState = endSwitch[callingFloor].read();
    int parkedAtSwitchState = endSwitch[parkedAt].read();

    if ((callingSwitchState == LOW) && (parkedAtSwitchState == HIGH))
    {
      parkedAt = callingFloor;
      moving = false;
      lcd.setCursor(0, 0);
      lcd.print("Parked at floor:");
      lcd.setCursor(7, 1);
      lcd.print(parkedAt);
    }
  }
}

void setup()
{
  #ifdef DEBUG
    Serial.begin(9600);
  #endif

  lcd.init();
  lcd.backlight();

  for (byte i = 0; i < NUM_SWITCHES; i++)
  {
    endSwitch[i].attach(endSwitchPin[i], INPUT_PULLUP);
    endSwitch[i].setPressedState(LOW);
    endSwitch[i].interval(5);
  }

  for (byte i = 0; i < NUM_BUTTONS; i++)
  {
    button[i].attach(buttonPin[i], INPUT_PULLUP);
    button[i].setPressedState(LOW);
    button[i].interval(5);
  }

  for (byte i = 0; i < NUM_RELAYS; i++)
  {
    pinMode(relayPin[i], OUTPUT);
  }

  for (byte i = 0; i < NUM_RELAYS; i++)
  {
    pinMode(doorPin[i], OUTPUT);
  }

  checkSwitches(true);
}

void loop()
{
  checkSwitches();

  for (byte i = 0; i < NUM_DOORS; i++)
  {
    if(doorOpened[i] == true)
    {
      if((millis() - doorMillis) > DOOR_TIME * 1000UL)
      {
        lcd.setCursor(0, 0);
        lcd.print("Closing door:");
        lcd.print(i);
        lcd.print("  ");
        lcd.setCursor(0, 1);
        lcd.print("                ");
        closeDoor(i);
      }
    }
  }

  for (byte i = 0; i < NUM_BUTTONS; i++)
  {
    button[i].update();

    if (button[i].pressed())
    {
      if(parkedAt == i)
      {
        if(doorOpened[i] == false)
        {
          lcd.setCursor(0, 0);
          lcd.print("Openning door:");
          lcd.print(i);
          lcd.print(" ");
          lcd.setCursor(0, 1);
          lcd.print("                ");
          openDoor(i);
          doorMillis = millis();
        }
      }
      else if(parkedAt > i)
      {
        if(doorOpened[i] == false)
        {
          callingFloor = i;
          moving = true;
          lcd.setCursor(0, 0);
          lcd.print("Going down to:");
          lcd.print(i);
          lcd.print(" ");
          lcd.setCursor(0, 1);
          lcd.print("            ");
        }
      }
      else if(parkedAt < i)
      {
        if(doorOpened[i] == false)
        {
          callingFloor = i;
          moving = true;
          lcd.setCursor(0, 0);
          lcd.print("Going up to:");
          lcd.print(i);
          lcd.print("   ");
          lcd.setCursor(0, 1);
          lcd.print("                ");
        }
      }
    }
  }
}