#include <Bounce2.h>

#define STATE_IDLE  0
#define STATE_UP    1
#define STATE_DOWN  2
#define STATE_STOP  3

#define PIN_B1      A0
#define PIN_B2      A1
#define PIN_B3      A2
#define PIN_B4      A3
#define PIN_B5      A4
#define PIN_B6      A5

byte state;

const uint8_t BUTTON_PINS[] = {PIN_B1, PIN_B2, PIN_B3, PIN_B4, PIN_B5, PIN_B6};
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);
  }

  Serial.begin(9600);
  Serial.println("Elevator v1\n");
}

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;

  uint8_t BTN1, BTN2, BTN3, BTN4, BTN5, BTN6;
  BTN1 = digitalRead(PIN_B1);
  BTN2 = digitalRead(PIN_B2);
  BTN3 = digitalRead(PIN_B3);
  BTN4 = digitalRead(PIN_B4);
  BTN5 = digitalRead(PIN_B5);
  BTN6 = digitalRead(PIN_B6);

  uint8_t floors = concat(1, 0, 1, 1, 1, 1); // FAKE

  uint8_t 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);
 }

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;
}