#include <Arduino.h>

enum State { STOPPED, MOVING_UP, MOVING_DOWN };
volatile State elevatorState = STOPPED;
volatile int currentFloor = 1; 

volatile bool requests[5] = {false, false, false, false, false};

const int motorUp = PB10;   
const int motorDown = PB11; 

//should also open door if elevator is at that floor
void onBtn1() { 
  requests[0] = true;
  checkNextMove(elevatorState);
}
void onBtnInternal2() { 
  requests[1] = true;
  checkNextMove(elevatorState);
 
}
void onBtn2Up() { 
  requests[2] = true;
  checkNextMove(elevatorState); 
}
void onBtn2Down() { 
  requests[3] = true;
  checkNextMove(elevatorState);
}
void onBtn3() { 
  requests[4] = true;
  checkNextMove(elevatorState);
}

void onSns1() { 
  currentFloor = 1; 
  State prevState = elevatorState;
  if (shouldStop()) stopAndServe(); 
  checkNextMove(prevState);
}
void onSns2() { 
  currentFloor = 2; 
  State prevState = elevatorState;
  if (shouldStop()) stopAndServe(); 
  checkNextMove(prevState);
}
void onSns3() { 
  currentFloor = 3; 
  State prevState = elevatorState;
  if (shouldStop()) stopAndServe(); 
  checkNextMove(prevState);
}

void setup() {
  Serial.begin(115200);
  pinMode(motorUp, OUTPUT); pinMode(motorDown, OUTPUT);

  pinMode(PA0, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA0), onBtn1, RISING);
  pinMode(PA1, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA1), onBtn2Up, RISING);
  pinMode(PA2, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA2), onBtn2Down, RISING);
  pinMode(PA3, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA3), onBtn3, RISING);
  pinMode(PA7, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA7), onBtnInternal2, RISING);

  pinMode(PA4, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA4), onSns1, RISING);
  pinMode(PA5, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA5), onSns2, RISING);
  pinMode(PA6, INPUT_PULLDOWN); attachInterrupt(digitalPinToInterrupt(PA6), onSns3, RISING);
}

void loop() {

}

bool shouldStop() {
  if (currentFloor == 1 && requests[0]) return true;
  if (currentFloor == 3 && requests[4]) return true;
  
  if (currentFloor == 2) {
    if (requests[1]) return true;
    if (elevatorState == MOVING_UP && requests[2]) return true;
    if (elevatorState == MOVING_DOWN && requests[3]) return true;
  }
  return false;
}

void stopAndServe() {
  digitalWrite(motorUp, LOW); 
  digitalWrite(motorDown, LOW);
  
  if (currentFloor == 1) {
    requests[0] = false;
  } else if (currentFloor == 2) {
    requests[1] = false;
    if (elevatorState == MOVING_UP)   requests[2] = false;
    if (elevatorState == MOVING_DOWN) requests[3] = false;
  } else if (currentFloor == 3) {
    requests[4] = false;
  }

  elevatorState = STOPPED;

  Serial.print("Arrived Floor "); Serial.println(currentFloor);
  delay(2000); //should open and close door
}

void checkNextMove(State prevState) {

  if (elevatorState != STOPPED) return;

  if (prevState == MOVING_UP) { //check higher floors first

    if (requests[4]) {
      if (currentFloor != 3) elevatorState = MOVING_UP;
      else requests[4] = false;
    }
    else if (requests[1] || requests[2] || requests[3]) {
      if (currentFloor < 2) {elevatorState = MOVING_UP; requests[1] = true; }
      else if (currentFloor > 2) {elevatorState = MOVING_DOWN; requests[1] = true;}
      else { requests[1] = false; requests[2] = false; requests[3] = false; }
    } 
    else if (requests[0]) {
      if (currentFloor != 1) elevatorState = MOVING_DOWN;
      else requests[0] = false;
    }
  }

  else { //moving down or stopped - check lower floors first

    if (requests[0]) {
      if (currentFloor > 1) elevatorState = MOVING_DOWN;
      else requests[0] = false;
    } 
    else if (requests[1] || requests[2] || requests[3]) {
      if (currentFloor < 2) {elevatorState = MOVING_UP; requests[1] = true; }
      else if (currentFloor > 2) {elevatorState = MOVING_DOWN; requests[1] = true;}
      else { requests[1] = false; requests[2] = false; requests[3] = false; }
    } 
    else if (requests[4]) {
      if (currentFloor < 3) elevatorState = MOVING_UP;
      else requests[4] = false;
    }
  }

  digitalWrite(motorUp, elevatorState == MOVING_UP);
  digitalWrite(motorDown, elevatorState == MOVING_DOWN);

}