// Constants
const int PULSER_PIN_BUTTON = 2;
const int RED_PIN_LED = 3;
const int GREEN_PIN_LED = 4;
const int YELLOW_PIN_LED = 5;
// Struct
int floor_number;
byte bussy_elevator;
byte go_up;
byte last_led_ms;
unsigned int last_button_ms;

struct LED {
  byte state;
  int pin;
};

void setup_red_led(LED & redled, int pin){
  redled.state = LOW;
  redled.pin = pin;
}

void setup_green_led(LED & greenled, int pin){
  greenled.state = LOW;
  greenled.pin = pin;
}

void setup_yellow_led(LED & yellowled, int pin){
  yellowled.state = LOW;
  yellowled.pin = pin;
}

// -------------------------------------------------------------

struct LED red_led, green_led, yellow_led;


void setup()
{
  setup_red_led(red_led, RED_PIN_LED);
  setup_green_led(green_led, GREEN_PIN_LED);
  setup_yellow_led(yellow_led, YELLOW_PIN_LED);

  attachInterrupt(digitalPinToInterrupt(PULSER_PIN_BUTTON), change_velocity, RISING);
  floor_number = 0;
  busy_elevator = 0;
  go_up = 0;
  last_led_ms = 0;
}

void loop()
{
  double milliseconds = millis();
  led_schedule(milliseconds);
  button_schedule(milliseconds);
  
}

void led_schedule(double milliseconds){
  if(go_up == 1){
    if(milliseconds - last_led_ms <= 1000 && floor_number >= 1){
      red_led.state = HIGH;
    }else if(milliseconds - last_led_ms > 1000 && milliseconds - last_led_ms <= 2000 && floor_number >= 2){
      red_led.state = LOW;
      green_led.state = HIGH;
    }else if(floor_number >= 3){
      green_led.state = LOW;
      yellow_led.state = HIGH;
    }
      digitalWrite(red_led.pin, red_led.state);
      digitalWrite(green_led.pin, green_led.state);
      digitalWrite(yellow_led.pin, yellow_led.state);
  }
}

void button_schedule(double milliseconds){
  if(busy_elevator == 0 && last_button_ms == 0){
    busy_elevator = 1;
    last_button_ms = milliseconds;
  }else if(busy_elevator == 1 && (milliseconds - last_button_ms <= 5000) ){
    if(floor_number <= 2){
      floor_number ++;
    }
  }
}