// Global variables
// Mode - SIM ONLY
int mode = 12;
// Mode lights
int red_in = 8;
int blue_in = 9;
int green_in = 10;
// Buzzer
int buzzer = 5;

// Initialize modes
int mode_current = 0;
int mode_previous = 0;

void setup() {
  // Define pin I/O
  pinMode(mode, OUTPUT);
  pinMode(red_in, INPUT);
  pinMode(blue_in, INPUT);
  pinMode(green_in, INPUT);
  pinMode(buzzer, OUTPUT);

  // Open serial port
  Serial.begin(115200);
}

void loop() {
  // Pulse mode

  digitalWrite(mode, HIGH);
  delay(250);
  digitalWrite(mode, LOW);
  delay(250);

  // Buzzer logic
  // A-stop
  if (digitalRead(red_in) == HIGH) {
    mode_current = 8;
    Serial.println("A-stop");

    // Buzz thrice if mode changes to A-stop
    if (mode_current != mode_previous) {
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);
      delay(250);
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);
      delay(250);
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);

      // Set mode_current = mode_previous
      mode_previous = mode_current;
    }
  }
  // Autonomous
  if (digitalRead(blue_in) == HIGH) {
    mode_current = 9;
    Serial.println("Autonomous");

    // Buzz once if mode changes to autonomous
    if (mode_current != mode_previous) {
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);

      // Set mode_current = mode_previous
      mode_previous = mode_current;
    }
  }
  // Manual
  if (digitalRead(green_in) == HIGH) {
    mode_current = 10;
    Serial.println("Manual");

    // Buzz twice if mode changes to manual
    if (mode_current != mode_previous) {
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);
      delay(250);
      digitalWrite(buzzer, HIGH);
      delay(250);
      digitalWrite(buzzer, LOW);

      // Set mode_current = mode_previous
      mode_previous = mode_current;
    }
  }
}