#include <Servo.h>

#define SERVO_PIN PA7
#define BUZZER_PIN PA6
#define RED_LED PA5

#define LOCK_POS 90
#define UNLOCK_POS 0

Servo doorServo;

unsigned long blinkTimer = 0;

bool emergency = false;

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

  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  doorServo.attach(SERVO_PIN);

  doorServo.write(LOCK_POS);

  Serial.println("Emergency Door System Started");
}

void blinkEmergency()
{
  if (millis() - blinkTimer >= 300)
  {
    digitalWrite(
      RED_LED,
      !digitalRead(RED_LED));

    blinkTimer = millis();
  }
}

void loop()
{
  //------------------------------------------------
  // CONDITION 1
  //------------------------------------------------

  Serial.println("");
  Serial.println("CONDITION 1");
  Serial.println("ACCIDENT DETECTED");
  Serial.println("G FORCE = 2g");
  Serial.println("BUS STOPPED");

  emergency = true;

  unsigned long start1 = millis();

  while (millis() - start1 < 5000)
  {
    digitalWrite(BUZZER_PIN, HIGH);

    blinkEmergency();

    doorServo.write(UNLOCK_POS);

    Serial.println("DOOR OPEN");
    delay(200);
  }

  //------------------------------------------------
  // RESET
  //------------------------------------------------

  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(RED_LED, LOW);

  doorServo.write(LOCK_POS);

  delay(3000);

  //------------------------------------------------
  // CONDITION 2
  //------------------------------------------------

  Serial.println("");
  Serial.println("CONDITION 2");
  Serial.println("TEMP = 70 DEG");
  Serial.println("BUS MOVING");

  unsigned long start2 = millis();

  while (millis() - start2 < 5000)
  {
    digitalWrite(BUZZER_PIN, HIGH);

    blinkEmergency();

    doorServo.write(LOCK_POS);

    Serial.println("DOOR LOCKED");
    delay(200);
  }

  //------------------------------------------------
  // RESET
  //------------------------------------------------

  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(RED_LED, LOW);

  delay(3000);

  //------------------------------------------------
  // CONDITION 3
  //------------------------------------------------

  Serial.println("");
  Serial.println("CONDITION 3");
  Serial.println("TEMP = 70 DEG");
  Serial.println("BUS STOPPED");

  unsigned long start3 = millis();

  while (millis() - start3 < 5000)
  {
    digitalWrite(BUZZER_PIN, HIGH);

    blinkEmergency();

    doorServo.write(UNLOCK_POS);

    Serial.println("DOOR OPEN");
    delay(200);
  }

  //------------------------------------------------
  // RESET FOR NEXT LOOP
  //------------------------------------------------

  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(RED_LED, LOW);

  doorServo.write(LOCK_POS);

  delay(5000);
}