#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define air1 9
#define air2 10
#define killout 3
#define buzzer 6
#define precharge 11
#define killswitch1 7
#define killswitch2 8
#define botswitch 2
#define green 12
#define red 5
#define blue 4

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int kstate1 = 0;
int kstate2 = 0;
int botstate = 0;

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  
  Serial.begin(9600);
  pinMode(air1, OUTPUT);
  pinMode(air2, OUTPUT);
  pinMode(killout, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(precharge, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(red, OUTPUT);
  pinMode(blue, OUTPUT);
  pinMode(killswitch1, INPUT);
  pinMode(killswitch2, INPUT);
  pinMode(botswitch, INPUT);

  digitalWrite(air1, LOW);
  digitalWrite(air2, LOW);
  digitalWrite(killout, LOW);
  digitalWrite(precharge, LOW);

  displayMessage("*******************************");
  displayMessage("      MITS_MOTORSPORTS_V2      ");
  displayMessage("*******************************");
  delay(1000);
  kstate1 = digitalRead(killswitch1);
  kstate2 = digitalRead(killswitch2);
  botstate = digitalRead(botswitch);

  if (kstate1 == LOW && kstate2 == LOW && botstate == LOW) {
    digitalWrite(precharge, HIGH);
    digitalWrite(blue, HIGH);
    digitalWrite(red, HIGH);
    digitalWrite(green, LOW);
    displayMessage("PreCharging..!");
    Serial.println("PreCharging..!");
    delay(2000);
    digitalWrite(air1, HIGH);
    digitalWrite(air2, HIGH);
    digitalWrite(blue, HIGH);
    digitalWrite(green, HIGH);
    digitalWrite(red, LOW);
    displayMessage("Accumulator Isolation Relays Activated!");
    Serial.println("Accumulator Isolation Relays Activated!");
    delay(800);
    digitalWrite(precharge, LOW);
    displayMessage("PreCharge cutoff");
    Serial.println("PreCharge cutoff");
    delay(800);
    displayMessage("GoKart Ready to RACE!");
    Serial.println("GoKart Ready to RACE!");
    delay(800);

  } else {
    displayMessage("Warning: RESET ALL KILL SWITCHES!");
    Serial.println("Warning: RESET ALL KILL SWITCHES!");
    digitalWrite(killout, HIGH);
    digitalWrite(green, HIGH);
    digitalWrite(red, HIGH);
    digitalWrite(blue, LOW);
    delay(800);
  }
}

void loop() {
  kstate1 = digitalRead(killswitch1);
  kstate2 = digitalRead(killswitch2);
  botstate = digitalRead(botswitch);

  if (kstate1 == HIGH || kstate2 == HIGH || botstate == HIGH) {
    digitalWrite(air1, LOW);
    digitalWrite(air2, LOW);
    digitalWrite(killout, HIGH);
    digitalWrite(green, HIGH);
    digitalWrite(red, HIGH);
    digitalWrite(blue, HIGH);
    delay(150);
    digitalWrite(green, HIGH);
    digitalWrite(red, HIGH);
    digitalWrite(blue, LOW);
    delay(150);
    displayMessage("EMERGENCY STOP TRIGGERED!");
    Serial.println("EMERGENCY STOP TRIGGERED!");
    displayMessage("Press RESET to Continue");
    Serial.println("Press RESET to Continue");
  } else {
    displayMessage("Everything OK");
    Serial.println("Everything OK");
  }
  delay(900);
}

void displayMessage(String msg) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println(msg);
  display.display();
}

void soundBuzzer(int duration = 350) {
  digitalWrite(buzzer, HIGH);
  delay(duration);
  digitalWrite(buzzer, LOW);
  delay(170);
}

void flashBuzzer(int times) {
  for (int i = 0; i < times; i++) {
    digitalWrite(buzzer, HIGH);
    delay(100);
    digitalWrite(buzzer, LOW);
    delay(80);
  }
}