#include <Servo.h>
Servo servo;
byte servopin = 7;

byte masterpin = 12;
byte lightpin = 11;
byte flashpin = 10;
byte ledpin = 2;
unsigned long timer, timeout = 500;

void setup() {
  Serial.begin(115200);
  servo.attach(servopin);
  servo.write(180);

  pinMode(ledpin, OUTPUT);

  pinMode(masterpin, INPUT_PULLUP);
  pinMode(lightpin, INPUT_PULLUP);
  pinMode(flashpin, INPUT_PULLUP);
}

void loop() {
  if (digitalRead(masterpin) == 0 || digitalRead(lightpin) == 0 || digitalRead(flashpin) == 0) {
    servo.write(90);
  } else {
    servo.write(180);
    digitalWrite(ledpin, LOW);
  }

  // lights
  if (digitalRead(lightpin) == 0 && digitalRead(flashpin) == 1) {
    digitalWrite(ledpin, HIGH);
  }

  // hazzard
  if (digitalRead(flashpin) == 0) {
    blink(); // blink LED
  }
  delay(100); // for serial printing
  displaystate();
}

void blink() {
  if (millis() - timer > timeout) { // timer overflow
    timer = millis(); // reset timer
    digitalWrite(ledpin, !digitalRead(ledpin)); // toggle LED
  }
}

void displaystate() {
  Serial.print(digitalRead(masterpin));
  Serial.print(digitalRead(lightpin));
  Serial.print(digitalRead(flashpin));

  if (!digitalRead(lightpin) && digitalRead(flashpin))
    Serial.print(" <- LIGHTS ONLY");
  if (!digitalRead(flashpin) && digitalRead(lightpin))
    Serial.print(" <- FLASH ONLY");
  if (!digitalRead(lightpin) && !digitalRead(flashpin))
    Serial.print(" <- FLASH OVERRIDE");

  Serial.println();
}