#include <Arduino.h>

// Configuration
enum State { STOPPED, MOVING_UP, MOVING_DOWN };
State elevatorState = STOPPED;

volatile int currentFloor = 1; 
volatile bool requests[4] = {false, false, false, false}; // Index 1-3
volatile bool doorOpen = false;

// Pin Definitions
const int btnPins[] = {PA0, PA1, PA2};
const int snsPins[] = {PA3, PA4, PA5};
const int motorUp = PB0;
const int motorDown = PB1;

// Function Prototypes
void handleMotor();
void checkNextMove();

// --- INTERRUPT HANDLERS ---

// Button Interrupts (Requests)
void onButton1() { requests[1] = true; }
void onButton2() { requests[2] = true; }
void onButton3() { requests[3] = true; }

// Sensor Interrupts (Floor Arrival)
void onSensor1() { currentFloor = 1; }
void onSensor2() { currentFloor = 2; }
void onSensor3() { currentFloor = 3; }

void setup() {
  Serial.begin(115200);

  // Initialize Pins
  for(int i=0; i<3; i++) {
    pinMode(btnPins[i], INPUT_PULLUP);
    pinMode(snsPins[i], INPUT_PULLUP);
  }
  pinMode(motorUp, OUTPUT);
  pinMode(motorDown, OUTPUT);

  // Attach Interrupts
  attachInterrupt(digitalPinToInterrupt(PA0), onButton1, FALLING);
  attachInterrupt(digitalPinToInterrupt(PA1), onButton2, FALLING);
  attachInterrupt(digitalPinToInterrupt(PA2), onButton3, FALLING);
  
  attachInterrupt(digitalPinToInterrupt(PA3), onSensor1, FALLING);
  attachInterrupt(digitalPinToInterrupt(PA4), onSensor2, FALLING);
  attachInterrupt(digitalPinToInterrupt(PA5), onSensor3, FALLING);

  Serial.println("Elevator System Initialized...");
}

void loop() {
  if (elevatorState == STOPPED) {
    checkNextMove();
  } else {
    // Check if we reached a requested floor
    if (requests[currentFloor]) {
      stopAndServeFloor();
    }
  }
  
  handleMotor();
  delay(10); // Small stability delay
}

void stopAndServeFloor() {
  Serial.print("Stopping at Floor ");
  Serial.println(currentFloor);
  
  elevatorState = STOPPED;
  handleMotor(); // Turn off motors immediately
  
  requests[currentFloor] = false;
  
  // Simulate Door Opening/Closing
  doorOpen = true;
  delay(2000); // Wait 2 seconds at the floor
  doorOpen = false;
  
  Serial.println("Doors closed. Checking for next request...");
}

void checkNextMove() {
  for (int f = 1; f <= 3; f++) {
    if (requests[f]) {
      if (f > currentFloor) elevatorState = MOVING_UP;
      else if (f < currentFloor) elevatorState = MOVING_DOWN;
      else stopAndServeFloor(); // Already there
      return;
    }
  }
}

void handleMotor() {
  switch (elevatorState) {
    case MOVING_UP:
      digitalWrite(motorUp, HIGH);
      digitalWrite(motorDown, LOW);
      break;
    case MOVING_DOWN:
      digitalWrite(motorUp, LOW);
      digitalWrite(motorDown, HIGH);
      break;
    case STOPPED:
      digitalWrite(motorUp, LOW);
      digitalWrite(motorDown, LOW);
      break;
  }
}