#include <Bounce2.h>

#define i_FL1     41
#define i_FL2     43
#define i_FL3     45
#define i_FL4     47
#define i_FL5     49
#define i_FL6     51

#define i_BTN1    29
#define i_BTN2    31
#define i_BTN3    33
#define i_BTN4    35
#define i_BTN5    37
#define i_BTN6    39

#define LED_UP    2
#define LED_DOWN  3

const uint8_t BUTTON_PINS[] = {i_BTN1, i_BTN2, i_BTN3, i_BTN4, i_BTN5, i_BTN6};
Bounce * buttons = new Bounce[sizeof(BUTTON_PINS)];

void setup() {

  for (int i = 0; i < sizeof(BUTTON_PINS); i++) {
    buttons[i].attach(BUTTON_PINS[i], INPUT_PULLUP);
    buttons[i].interval(25);
  }

  pinMode(i_FL1, INPUT_PULLUP);
  pinMode(i_FL2, INPUT_PULLUP);
  pinMode(i_FL3, INPUT_PULLUP);
  pinMode(i_FL4, INPUT_PULLUP);
  pinMode(i_FL5, INPUT_PULLUP);
  pinMode(i_FL6, INPUT_PULLUP);

  pinMode(LED_UP, OUTPUT);
  pinMode(LED_DOWN, OUTPUT);

  Serial.begin(9600);
  Serial.println("Elevator v1.0");

  digitalWrite(LED_UP, HIGH);
  delay(500);
  digitalWrite(LED_UP, LOW);
  delay(500);
  digitalWrite(LED_DOWN, HIGH);
  delay(500);
  digitalWrite(LED_DOWN, LOW);
  delay(500);
}

uint8_t FL1, FL2, FL3, FL4, FL5, FL6;
uint8_t BTN1, BTN2, BTN3, BTN4, BTN5, BTN6;

uint8_t buttons_state, buttons_state_last;

void loop() {

  uint8_t btn_changed = false;
  for (int i = 0; i < sizeof(BUTTON_PINS); i++) {
    buttons[i].update();
    if (buttons[i].fell()) {
      btn_changed = true;
      break;
    }
  }

  if (!btn_changed)
    return;

  FL1 = digitalRead(i_FL1);
  FL2 = digitalRead(i_FL2);
  FL3 = digitalRead(i_FL3);
  FL4 = digitalRead(i_FL4);
  FL5 = digitalRead(i_FL5);
  FL6 = digitalRead(i_FL6);

  uint8_t floors = concat(FL1, FL2, FL3, FL4, FL5, FL6);

  uint8_t found = 0;
  for (uint8_t i = 0; i < 6; i++)
    if (!((floors >> i) & 1))
        if (++found > 1) {
          Serial.print("err: More sensors activated: "); Serial.println(found);
          return;
        }

  if (found == 0) {
    Serial.println("err: Cabin not on a floor.");
    return;
  }

  uint8_t calling;
  BTN1 = digitalRead(i_BTN1);
  BTN2 = digitalRead(i_BTN2);
  BTN3 = digitalRead(i_BTN3);
  BTN4 = digitalRead(i_BTN4);
  BTN5 = digitalRead(i_BTN5);
  BTN6 = digitalRead(i_BTN6);

  calling = concat(BTN1, BTN2, BTN3, BTN4, BTN5, BTN6);
  Serial.print("floors = "); Serial.print(floors, BIN); Serial.print(", calling = "); Serial.print(calling, BIN);

  Serial.print("\n\tfloors < calling : "); Serial.print(floors < calling);
  Serial.print("\n\tfloors = calling : "); Serial.print(floors == calling);
  Serial.print("\n\tfloors > calling : "); Serial.println(floors > calling);

  Serial.print("\n\tfloors != calling : "); Serial.println(floors != calling);
  while (floors != calling) {
    if (floors < calling) {
      // going up
      Serial.println("Going UP");
      digitalWrite(LED_UP, HIGH);
      digitalWrite(LED_DOWN, LOW);
    }
    else if (floors > calling) {
      // going down
      Serial.println("Going DOWN");
      digitalWrite(LED_UP, LOW);
      digitalWrite(LED_DOWN, HIGH);
    }
  }

  Serial.println("STOP: On the floor");

  if (true)
    return;

  // if (floors < calling) {
  //   // going up
  //   Serial.println("Going UP");
  //   digitalWrite(LED_UP, HIGH);
  //   digitalWrite(LED_DOWN, LOW);
  // }
  // else if (floors == calling) {
  //   // stop
  //   Serial.println("On the floor");
  //   digitalWrite(LED_UP, HIGH);
  //   digitalWrite(LED_DOWN, HIGH);
  // }
  // else if (floors > calling) {
  //   // going down
  //   Serial.println("Going DOWN");
  //   digitalWrite(LED_UP, LOW);
  //   digitalWrite(LED_DOWN, HIGH);
  // }
  // else {
  //   digitalWrite(LED_UP, LOW);
  //   digitalWrite(LED_DOWN, LOW);
  // }
}

uint8_t concat(uint8_t a, uint8_t b, uint8_t c, uint8_t d, uint8_t e, uint8_t f) {
  uint8_t res = 0<<7 | 0<<6 | a<<5 | b<<4 | c<<3 | d<<2 | e<<1 | f;
  return res;
}