#include <U8g2lib.h>

// OLED
U8X8_SSD1306_128X64_NONAME_HW_I2C oled(U8X8_PIN_NONE);
// Variables
String currentDisplay = "";
String demandDisplay = "";
bool displayBlank = false;
int tripTime = 0;
bool isNotify = false;
unsigned long smNotify = 0;
int timerNotify = 0;
unsigned long smBlank = 0;
int timerBlank = 0;
const int notifyTime = 5000; // milliseconds
const int blankTime = 10000; // milliseconds

// Buzzer
int buzzerPin = 9;
const int beepNotifyTime = 100;
const int beepWarningTime = 1000;

// Start-stop stuff
int keyState = 0;
int lastKeyState = 0;
// Button, brake and handbrake
bool btnPowerIsPressed = false;
int btnPowerLastState = HIGH;
bool brakePressed = false;
int brakeLastState = HIGH;
bool hBrakeEngaged = false;
int hBrakeLastState = HIGH;

bool isRunning = false;
bool isPrimed = false;
bool canStart = false;
bool hasStarted = false;
bool autoStart = false;

bool relayACC = false;
bool relayOn1 = false;
bool relayOn2 = false;
bool relayStart = false;

bool keyDetect = false;
bool doorOpen = false;
bool doorWasOpen = false;

// LED flash
bool flashStart = false;
bool lastFlashStart = false;
bool flashOn = false;
bool lastFlashOn = false;

// Timers
unsigned long smLedStart = 0;
int timerLedStart = 0;
unsigned long smLedOn = 0;
int timerLedOn = 0;
unsigned long smPrime = 0;
int timerPrime = 0; // timer for priming before starting
unsigned long smStart = 0;
int timerStart = 0; // timer for starting
unsigned long smRun = 0;
int timerRun = 0;

// Pins
int senseBtnPin = A1;
int senseBrkPin = A2;
int senseHBrkPin = A3;
int senseKeyPin = A0;
int relayAccPin = 5;
int relayOn1Pin = 6;
int relayOn2Pin = 7;
int relayStartPin = 8;
int indRpin = 2;
int indGpin = 3;
int indBpin = 4;

// Pre-set variables
const bool crankOnBrakeRelease = false;
// if true, when starting sequence is initiated and then brake is released before engine starts, 
// engine will continue to start. Else, the engine starting sequence will cancel.
const int primeTime = 1000; // milisecond(s); delay to prime fuel before starting
const int startMinTime = 1000; // milisecond(s); minimum time to start the engine
const int runMinTime = 1000;
const int ledFlashTime = 500;

void setupDisplay() {
  // drawString
  oled.drawString(0, 1, "OFF");
  oled.drawString(4, 1, "ACC");
  oled.drawString(8, 1, "ON");
  oled.drawString(11, 1, "START");
  oled.drawString(0, 6, "Initializing...");
}

void setupLED() {
  // RED
  digitalWrite(indRpin, HIGH);
  digitalWrite(indGpin, LOW);
  digitalWrite(indBpin, LOW);
  delay(250);
  // GREEN
  digitalWrite(indRpin, LOW);
  digitalWrite(indGpin, HIGH);
  digitalWrite(indBpin, LOW);
  delay(250);
  // BLUE
  digitalWrite(indRpin, LOW);
  digitalWrite(indGpin, LOW);
  digitalWrite(indBpin, HIGH);
  delay(250);
  // ALL
  digitalWrite(indRpin, HIGH);
  digitalWrite(indGpin, HIGH);
  digitalWrite(indBpin, HIGH);
  delay(250);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  // OLED
  oled.begin();
  oled.setPowerSave(0);
  oled.setFont(u8x8_font_pxplusibmcgathin_f);

  // PINS
  // Input
  pinMode(senseBtnPin, INPUT_PULLUP);
  pinMode(senseBrkPin, INPUT_PULLUP);
  pinMode(senseKeyPin, INPUT_PULLUP);
  // Output
  pinMode(relayAccPin, OUTPUT);
  pinMode(relayOn1Pin, OUTPUT);
  pinMode(relayOn2Pin, OUTPUT);
  pinMode(relayStartPin, OUTPUT);
  pinMode(indRpin, OUTPUT);
  pinMode(indGpin, OUTPUT);
  pinMode(indBpin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);

  // Splash Screen & LED Test
  setupDisplay();
  setupLED();
  oled.clear();
}

void notifyOLED(String display = "") {
  if(smBlank > 0 || displayBlank) {
    smBlank = 0;
    timerBlank = 0;
    displayBlank = false;
  }
  if(display != "") {
    demandDisplay = display;
  }
}

void updateOLED() {
  if(demandDisplay == "blank") {
    currentDisplay = demandDisplay;
    demandDisplay = "";
  } else if(demandDisplay != "") {
    Serial.println(demandDisplay);
    currentDisplay = demandDisplay;
    demandDisplay = "";
    isNotify = true;
    smNotify = 0;
    timerNotify = 0;
  }

  if((isNotify && smNotify == 0) || currentDisplay == "") {
    bool flash = flashStart != lastFlashStart || flashOn != lastFlashOn || isNotify;
    bool stateChanged = keyState != lastKeyState || flash;
    if(stateChanged) {
      smBlank = 0;
      timerBlank = 0;
      oled.clearLine(1);
      // oled.setCursor(20, 1);

      if(keyState == 0) {
        oled.drawString(0, 1, "OFF");
        smBlank = millis();
      }
      if(keyState == 1 && flashOn) {
        oled.drawString(4, 1, "ACC");
      }
      if((keyState == 2 && (isRunning || autoStart)) || (keyState == 2 && flashOn && !autoStart)) {
        oled.drawString(8, 1, "ON");
        if(isRunning) 
          smBlank = millis();
      }
      if(keyState == 3 || flashStart && !isRunning || autoStart) {
        oled.drawString(11, 1, "START");
        // tone(buzzerPin, 1500, beepNotifyTime);
      }
    }

    // Timers
    if(smBlank > 0) {
      if(timerBlank >= blankTime) {
        demandDisplay = "blank";
        smBlank = 0;
        timerBlank = 0;
      }

      timerBlank = millis() - smBlank;
    }
  }
  
  if(isNotify) {
    if(smNotify == 0) {
      smNotify = millis();

      oled.setCursor(0, 2);
      oled.print(F("Warning:"));

      // Notifications
      /*
      if(currentDisplay == "offTrip") {
        // convert ms to seconds, minutes & hours
        int seconds = tripTime / 1000;
        int hours = seconds / 3600;
        int minutes = seconds - (hours * 60);
        // oled.clear();
        oled.drawString(0, 3, "Trip time:");
        oled.drawString(0, 4, (to_string(hours) + " h " + to_string(minutes) .. " m"));
        tone(buzzerPin, 1500, beepNotifyTime);
      } 
      */

      // Warnings
      if(currentDisplay == "brakeToStart") {
        // oled.clear();
        oled.drawString(0, 3, "Depress brake");
        oled.drawString(0, 4, "pedal and press");
        oled.drawString(0, 5, "Start/Stop");
        oled.drawString(0, 6, "button to start.");
        tone(buzzerPin, 1500, beepWarningTime);
      } 

      if(currentDisplay == "hbToOff") {
        // oled.clear();
        oled.drawString(0, 3, "Pull handbrake");
        oled.drawString(0, 4, "to turn off");
        oled.drawString(0, 5, "engine!");
        tone(buzzerPin, 1500, beepWarningTime);
      } 
    } else if(timerNotify < notifyTime) {
      timerNotify = millis() - smNotify;
    } else {
      oled.clearLine(2);
      oled.clearLine(3);
      oled.clearLine(4);
      oled.clearLine(5);
      oled.clearLine(6);
      oled.clearLine(7);
      smNotify = 0;
      timerNotify = 0;
      isNotify = false;
      currentDisplay = "";
    }
  }
  
  if(currentDisplay == "blank") {
    oled.clear();
    displayBlank = true;
    currentDisplay = "";
  }
}

void updateLED() {
  if(!isRunning) {
    // Startup LED
    if(brakePressed) {
      if(keyState < 3) {
        if(smLedStart == 0 || timerLedStart >= ledFlashTime) {
          smLedStart = millis();
          timerLedStart = 0;
          flashStart = !flashStart;
        } else {
          timerLedStart = millis() - smLedStart;
        }
      }
    } else if(smLedStart > 0) {
      smLedStart = 0;
      timerLedStart = 0;
      flashStart = false;
    }
    // ON but not running LED
    if((keyState > 0 && keyState < 3) && !isRunning && !flashStart) {
      if(smLedOn == 0 || timerLedOn >= ledFlashTime) {
        smLedOn = millis();
        timerLedOn = 0;
        flashOn = !flashOn;
      } else {
        timerLedOn = millis() - smLedOn;
      }
    } else {
      smLedOn = 0;
      timerLedOn = 0;
      flashOn = false;
    }

    if(flashStart && keyState != 3) {
      digitalWrite(indRpin, LOW);
      digitalWrite(indGpin, HIGH);
      digitalWrite(indBpin, LOW);
    } else {
      if(keyState == 0) {
        digitalWrite(indRpin, LOW);
        digitalWrite(indGpin, LOW);
        digitalWrite(indBpin, LOW);
      } else if(keyState == 3) {
        digitalWrite(indRpin, HIGH);
        digitalWrite(indGpin, LOW);
        digitalWrite(indBpin, LOW);
      } else {
        if(flashOn || autoStart) {
          digitalWrite(indRpin, LOW);
          digitalWrite(indGpin, HIGH);
          digitalWrite(indBpin, LOW);
        } else {
          digitalWrite(indRpin, LOW);
          digitalWrite(indGpin, LOW);
          digitalWrite(indBpin, LOW);
        }
      }
    }
  } else {
    if(hBrakeEngaged && timerRun >= runMinTime) {
      digitalWrite(indRpin, LOW);
      digitalWrite(indGpin, LOW);
      digitalWrite(indBpin, HIGH);
    } else {
      digitalWrite(indRpin, LOW);
      digitalWrite(indGpin, HIGH);
      digitalWrite(indBpin, LOW);
    }
  }
}

void cycleIgnition(int demandState = -1) {
  // 0 = OFF
  // 1 = ACC
  // 2 = ON
  // 3 = START

  if(demandState >= 0) {
    // Serial.println("Demand: " + demandState);
    keyState = demandState;
  }

  // Set State
  if(keyState == 0) {
    if(brakePressed) {
      Serial.println("Starting... (State Off)");
      autoStart = true;
      keyState++;
    } else {
      Serial.println("ACC");
      keyState++;
    }
  } else if(keyState == 1) {
    if(brakePressed) {
      Serial.println("Starting... (State ACC)");
      autoStart = true;
      keyState++;
    } else {
      Serial.println("ON");
      keyState++;
    }
  } else if(keyState == 2) {
    if(!isRunning) {
      if(brakePressed) {
        Serial.println("Starting... (State ON)");
        autoStart = true;
      } else if(lastKeyState == keyState) {
        Serial.println("OFF after ON (Not running)");
        keyState = 0;
        resetOff();
      }
    } else {
      if(hBrakeEngaged && lastKeyState == keyState) {
        Serial.println("OFF after ON");
        keyState = 0;
        resetOff();
      } else if(!hBrakeEngaged) {
        Serial.println("Pull handbrake to turn off");
        notifyOLED("hbToOff");
      }
    }
  } else if(keyState == 3) {
    // No use for now
  }

  // Set Relay
  if(keyState == 0) {
    digitalWrite(relayAccPin, LOW);
    digitalWrite(relayOn1Pin, LOW);
    digitalWrite(relayOn2Pin, LOW);
    digitalWrite(relayStartPin, LOW);
  } else if(keyState == 1) {
    digitalWrite(relayAccPin, HIGH);
    digitalWrite(relayOn1Pin, LOW);
    digitalWrite(relayOn2Pin, LOW);
    digitalWrite(relayStartPin, LOW);
  } else if(keyState == 2) {
    digitalWrite(relayAccPin, HIGH);
    digitalWrite(relayOn1Pin, HIGH);
    digitalWrite(relayOn2Pin, HIGH);
    digitalWrite(relayStartPin, LOW);
  } else if(keyState == 3) {
    digitalWrite(relayAccPin, LOW);
    digitalWrite(relayOn1Pin, LOW);
    digitalWrite(relayOn2Pin, HIGH);
    digitalWrite(relayStartPin, HIGH);
  }
}

void resetOff() {
  isPrimed = false;
  isRunning = false;
}

void updateKeyState() {
  bool key = digitalRead(senseKeyPin);
  if(!key && keyDetect)
    keyDetect = false;
  else if(key && !keyDetect)
    keyDetect = true;
  // Door
  doorOpen = false;
  if(doorWasOpen && !doorOpen)
    doorWasOpen = false;
  else if(!doorWasOpen && doorOpen)
    doorWasOpen = true;
}

void updateNotOff(bool btnPowerState, bool brakeState, bool hBrakeState) {
  // if engine is not running and canStart is TRUE, start engine
  if(autoStart) {
    // 1. Check if keyState is is 2, if not, set to 2
    // 2. If primed, start engine
    if(keyState < 2) {
      cycleIgnition();
      // notify tone
      // tone(buzzerPin, 1500, beepNotifyTime);
    } else if(keyState == 2 && isPrimed) {
      if(timerPrime >= primeTime && (brakePressed || crankOnBrakeRelease)) {
        Serial.println("Cranking...");
        cycleIgnition(3);
        // notify tone
        // tone(buzzerPin, 1500, beepNotifyTime);
      } else if(!brakePressed) {
        Serial.println("Cranking Stopped");
        notifyOLED("brakeToStart");
        autoStart = false;
        isPrimed = false;
        // canStart = false;
      }
    } else if(keyState == 3) {
      if((timerStart >= startMinTime && btnPowerState) || (!brakePressed && !crankOnBrakeRelease)) {
        Serial.println("Running");
        autoStart = false;
        canStart = false;
        hasStarted = true;
        isRunning = true;
        cycleIgnition(2);
      }
    }
  }

  // Timers
  // Timer Prime
  if(keyState == 2) {
    if(timerPrime < primeTime) {
      if(smPrime == 0) {
        Serial.println("Priming");
        smPrime = millis();
      }
      timerPrime = millis() - smPrime;
    } else if(!isPrimed) {
      Serial.println("Primed");
      isPrimed = true;
    }
  } else if(smPrime > 0 || timerPrime) {
    smPrime = 0;
    timerPrime = 0;
  }
  // Timer Start
  if(keyState == 3) {
    if(smStart == 0)
      smStart = millis();
    timerStart = millis() - smStart;
  } else if(smStart > 0 || timerStart > 0) {
    smStart = 0;
    timerStart = 0;
  }
  // Running Timer
  if(isRunning) {
    if(smRun == 0)
      smRun = millis();
    timerRun = millis() - smRun;
  } else if(smRun > 0 || timerRun > 0) {
    tripTime = timerRun;
    smRun = 0;
    timerRun = 0;
  }
}

void loop() {
  // put your main code here, to run repeatedly:

  // BUTTONS (sense)
  bool snsBrake = digitalRead(senseBrkPin);
  bool snsHBrake = digitalRead(senseHBrkPin);
  bool snsPower = digitalRead(senseBtnPin);

  bool brakeState = snsBrake;
  if (brakeLastState != brakeState) {
    brakeLastState = brakeState;
    if (brakeState == HIGH) {
      // Serial.println(" released");
      brakePressed = false;
    }
    if (brakeState == LOW) {
      // Serial.println(" pressed");
      brakePressed = true;
      notifyOLED();
    }
  }

  bool hBrakeState = snsHBrake;
  if (hBrakeLastState != hBrakeState) {
    hBrakeLastState = hBrakeState;
    if (hBrakeState == HIGH) {
      Serial.println("Released");
      hBrakeEngaged = false;
    }
    if (hBrakeState == LOW) {
      Serial.println("Engaged");
      hBrakeEngaged = true;
    }
  }

  bool btnPowerState = snsPower;
  if (btnPowerLastState != btnPowerState) {
    btnPowerLastState = btnPowerState;
    if (btnPowerState == HIGH) {
      // Serial.println(" released");
      btnPowerIsPressed = false;
    }
    if (btnPowerState == LOW) {
      // Serial.println(" pressed");
      btnPowerIsPressed = true;
      notifyOLED();
      cycleIgnition();
    }
  }

  if(keyState > 0 || brakePressed || autoStart)
    updateNotOff(btnPowerState, brakeState, hBrakeState);

  // Timers
  // ON Timers
  if(keyState == 0) {
    smPrime = 0;
    timerPrime = 0;
    smStart = 0;
    timerStart = 0;
    smRun = 0;
    timerRun = 0;
  }

  updateLED();
  updateKeyState();
  // delay(1);

  if(!displayBlank)
    updateOLED();

  // LAST
  lastKeyState = keyState;
  lastFlashStart = flashStart;
  lastFlashOn = flashOn;
}