#include <MD_Parola.h>
#include <MD_MAX72xx.h>
#include <Keypad.h>
#include <SPI.h>
#include <AccelStepper.h>
#include <Servo.h>

#define STEP_PIN 8
#define DIR_PIN 9
#define ENABLE_PIN 10

#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define MAX_DEVICES 2
#define DATA_PIN 11
#define CS_PIN 12
#define CLK_PIN 13
#define MAX_ZONES 2
MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);

const byte ROWS = 4;
const byte COLS = 2;
char keys[ROWS][COLS] = {
  {'4', '5'},
  {'2', '3'},
  {'B', '1'},  // B = Basement
  {'O', 'C'}
};

byte rowPins[ROWS] = {7, 6, 5, 4};
byte colPins[COLS] = {3, 2};

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

MD_Parola myDisplay = MD_Parola(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);
AccelStepper myStepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
Servo gateServo;

int floorCurrent = 1;
int floorCalled;
int floorSpeed = 1500;
void setup() {
  myDisplay.begin(MAX_ZONES);
  myDisplay.setZone(0, 0, 0);
  myDisplay.setZone(1, 1, 1);
  myDisplay.displayReset();
  myDisplay.setIntensity(15);
  myStepper.setMaxSpeed(1000);
  myStepper.setAcceleration(500);
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);

  gateServo.attach(1);
  gateServo.write(0);
}
void doorOpenAnimation() {
  myDisplay.displayClear();
  myDisplay.displayZoneText(0, ">", PA_CENTER, 0, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  myDisplay.displayZoneText(1, "<", PA_CENTER, 0, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  if (myDisplay.displayAnimate()) {
    myDisplay.displayReset();
  }
  delay(floorSpeed);
  myDisplay.displayClear();
  gateServo.write(90);
  delay(1000);
  return;
}
void doorCloseAnimation() {
  myDisplay.displayClear();
  myDisplay.displayZoneText(0, "<", PA_CENTER, 0, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  myDisplay.displayZoneText(1, ">", PA_CENTER, 0, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  if (myDisplay.displayAnimate()) {
    myDisplay.displayReset();
  }
  delay(floorSpeed);
  myDisplay.displayClear();
  gateServo.write(0);
  delay(1000);
  return;
}
void goingUp() {
  doorCloseAnimation();
  myDisplay.displayClear();
  char buffer[2];  
  itoa(floorCurrent, buffer, 10);
  myDisplay.displayZoneText(0, buffer, PA_CENTER, 1500, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  myDisplay.displayZoneText(1, "^", PA_CENTER, 0, 0, PA_NO_EFFECT, PA_SCROLL_UP);
  if (myDisplay.displayAnimate()) {
    for (uint8_t i = 0; i < MAX_ZONES; i++) {
      if (myDisplay.getZoneStatus(i)) {
        myDisplay.displayReset();
      }
    }
  }
  do {
    myDisplay.displayReset();
    if (floorCurrent != 0) {
      myDisplay.print(floorCurrent);
    } else {
      myDisplay.print("B");
    }
    delay(floorSpeed);
    if (floorCurrent != floorCalled) {
      digitalWrite(DIR_PIN, HIGH);
      floorCurrent++;
      tone(A5, 440, 100);
      myStepper.moveTo(myStepper.currentPosition() + 200);
      myStepper.runToPosition();
    }
  } while (floorCurrent < floorCalled);
  myDisplay.displayReset();
  myDisplay.print(floorCurrent);
  delay(floorSpeed);
  tone(A5, 880, 350);
  doorOpenAnimation();
}
void goingDown() {
  doorCloseAnimation();
  myDisplay.displayClear();
  char buffer[2];  
  itoa(floorCurrent, buffer, 10);
  myDisplay.displayZoneText(0, buffer, PA_CENTER, 0, 1500, PA_NO_EFFECT, PA_NO_EFFECT);
  myDisplay.displayZoneText(1, "v", PA_CENTER, 10, 1500, PA_NO_EFFECT, PA_SCROLL_DOWN);
  do {
    myDisplay.displayReset();
    myDisplay.print(floorCurrent);
    if (myDisplay.displayAnimate()) {
      myDisplay.displayReset();
    }
    delay(floorSpeed);
    if (floorCurrent != floorCalled) {
      digitalWrite(DIR_PIN, LOW);
      floorCurrent--;
      tone(A5, 440, 100);
      myStepper.moveTo(myStepper.currentPosition() - 200);
      myStepper.runToPosition();
    }
  } while (floorCurrent > floorCalled);
  myDisplay.displayReset();
  if (floorCurrent != 0) {
    myDisplay.print(floorCurrent);
  } else {
    myDisplay.print("B");
  }
  delay(floorSpeed);
  tone(A5, 880, 350);
  doorOpenAnimation();
}
void displayCurrentFloor() {
  char buffer[2];
  if (floorCurrent != 0) {
    itoa(floorCurrent, buffer, 10);
  } else {
    strcpy(buffer, "B");
  }
  myDisplay.displayZoneText(0, buffer, PA_CENTER, 100, 1000, PA_PRINT, PA_NO_EFFECT);  // Display current floor in zone 0
  myDisplay.displayAnimate();
}
void loop() {
  char key = keypad.getKey();

  if (key) { 
    myDisplay.displayClear();
    
    switch (key) {
      case 'C' :
        doorCloseAnimation();
        break;
      case 'O' :
        doorOpenAnimation();
        break;
      default:
        switch (key) {
          case '1' :
            floorCalled = 1;
            break;
          case '2' :
            floorCalled = 2;
            break;
          case '3' :
            floorCalled = 3;
            break;
          case '4' :
            floorCalled = 4;
            break;
          case '5' :
            floorCalled = 5;
            break;
          case 'B' :
            floorCalled = 0;  // Basement
            break;
        }
        if (floorCalled > floorCurrent) {
          goingUp();
        }
        if (floorCalled < floorCurrent) {
          goingDown();
        }
        break;
    }
  }
  displayCurrentFloor();
}