/*
Features:
- Key State Control
- Turn Signal Control (including Hazard Lights)
- Light Control (with Auto Mode)
- Wiper Control (with Auto Mode)
*/
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo steeringLock;

// LCD Custom Characters
byte keyChar[8] = {
  0b00110,
  0b00100,
  0b00110,
  0b00100,
  0b01110,
  0b10001,
  0b10001,
  0b01110
};
byte signalLeftChar[8] =
{
  0b00001,
  0b00011,
  0b00111,
  0b01111,
  0b00111,
  0b00011,
  0b00001,
  0b00000
};
byte signalRightChar[8] =
{
  0b10000,
  0b11000,
  0b11100,
  0b11110,
  0b11100,
  0b11000,
  0b10000,
  0b00000
};
byte signalMidChar[8] =
{
  0b00000,
  0b00000,
  0b11111,
  0b11111,
  0b11111,
  0b00000,
  0b00000,
  0b00000
};
byte lightChar[8] =
{
  0b11100,
  0b10010,
  0b10001,
  0b10001,
  0b10001,
  0b10010,
  0b11100,
  0b00000
};
// byte autoLightChar[8] = 
// {
// 	0b11100,
// 	0b10010,
// 	0b10101,
// 	0b11011,
// 	0b11111,
// 	0b11011,
// 	0b10010,
// 	0b11100
// };
byte parkLightChar[8] =
{
  0b00100,
  0b00010,
  0b00000,
  0b01110,
  0b00000,
  0b00010,
  0b00100,
  0b00000
};
byte lowLightChar[8] =
{
  0b00001,
  0b00010,
  0b00100,
  0b01001,
  0b10010,
  0b00100,
  0b01000,
  0b00000
};
byte highLightChar[8] =
{
  0b00000,
  0b11111,
  0b00000,
  0b11111,
  0b00000,
  0b11111,
  0b00000,
  0b00000
};
// END

// CANBUS Pins
int pinBuzzer = 13;

// Ignition Pins
int pinRelayACC = 2;
int pinRelayON1 = 3;
int pinRelayON2 = 4; // Off when starting
int pinRelayStart = 5;
int pinServoLock = 8;
int pinBtnStart = 6;
int pinBtnBrake = 7;

// Ignition Variables
/*
keyState:
0. Off
1. ACC
2. On
3. Start
*/
int keyState = 0;
int lastKeyState = 0;
int keyStarted = 0;
int fuelPrimed = 0;
char *keyStateName[] = {"OFF  ", "ACC  ", "RUN  ", "START"};
int btnStart = 1;
int lastBtnStart = 1;
int btnBrake = 1;
//int lastBtnBrake = 1;

// Turn Signal Pins
int pinSwitchTurnLeft = 48;
int pinSwitchTurnRight = 50;
int pinBtnHazard = 52;
int pinRelayTurnLeft = 49;
int pinRelayTurnRight = 51;

// Turn Signal Variables
int btnTurnLeft = 1;
int stateTurnLeft = 0;
int btnTurnRight = 1;
int stateTurnRight = 0;
int stateCancel = 0;
int btnHazard = 1;
int stateHazard = 0;
int turnSignalBlink = 0;
int laneChangeBlinkTimes = 5;
int delayTurnSignal = 40;

// Light Pins
int pinParkLight = 39;
int pinLowLight = 41;
int pinAutoLight = 43;
int pinHighLight = 45;
int pinFlashLight = 47;
int pinRelayRunLight = 23;
int pinRelayParkLight = 25;
int pinRelayLowLight = 27;
int pinRelayHighLight = 29;

// Light Variables
/*
0. Off
1. Park
2. Beam
*/
int lightMode = 0; // 0 = Off, 1 = Park, 2 = Beam
int lightAuto = 0;
int lastLightMode = 0;
int lightHighBeam = 0;
int swAuto = 1;
int swPark = 1;
int swLow = 1;
int swHigh = 1;
int swFlash = 1;

char lcdNotify[] = {};

float lockPos = 0.0;

int timerLcdRefresh = 0;
int timerStarter = 0;
int timerLock = 0;
int timerTurnSignal = 0;

void btnWithBrake() {
  if (keyStarted == 0) {
    keyState = 3;
  } else {
    keyState = 0;
  }
}

void btnWithoutBrake() {
  if (keyState < 2) {
    keyState++;
  } else {
    keyState = 0;
  }
}

void lcdDisplay() {
  if (timerLcdRefresh == 100) {
    if (keyState == 0) {
      lcd.noDisplay();
      lcd.noBacklight();
    } else {
      if (lastKeyState == 0) {
        lcd.display();
        lcd.backlight();
      }

      lcd.setCursor(8,3);
      lcd.write(0);
      lcd.print(keyStateName[keyState]);

      if (digitalRead(pinRelayTurnLeft) == 1) {
        lcd.setCursor(0,0);
        lcd.write(2);
        lcd.write(1);
      } else if (digitalRead(pinRelayTurnLeft) == 0) {
        lcd.setCursor(0,0);
        lcd.print("  ");
      }

      if (digitalRead(pinRelayTurnRight) == 1) {
        lcd.setCursor(18,0);
        lcd.write(1);
        lcd.write(3);
      } else if (digitalRead(pinRelayTurnRight) == 0) {
        lcd.setCursor(18,0);
        lcd.print("  ");
      }
      
      lcd.setCursor(8,0);
      if (lightMode == 2) {
        lcd.write(6);
        // lcd.write(4);
      } else if (lightMode == 1) {
        lcd.write(5);
        // lcd.write(4);
      } else {
        lcd.print(" ");
        // lcd.print(" ");
      }

      lcd.setCursor(9,0);
      if (swAuto == 0) {
        lcd.print("A");
      } else if(lightMode > 0) {
        lcd.write(4);
      } else {
        lcd.print(" ");
      }

      lcd.setCursor(10,0);
      if(lightHighBeam == 1) {
        lcd.write(7);
        lcd.write(4);
      } else {
        lcd.print(" ");
        lcd.print(" ");
      }
      
      // if (lcdNotify[0] != "") {
      //   lcd.setCursor(10,0);
      // } else {
      //   for (int i = 0; i < 20; i++) {
      //     lcd.print(" ");
      //   }
      // }
    }
  } else {
    timerLcdRefresh++;
  }
  lastKeyState = keyState;
}

void ignitionControl() {
  // Key State
  if (keyState == 0) {
    digitalWrite(pinRelayACC, LOW);
    digitalWrite(pinRelayON1, LOW);
    digitalWrite(pinRelayON2, LOW);
    digitalWrite(pinRelayStart, LOW);

    keyStarted = 0;
    fuelPrimed = 0;
  } else if (keyState == 1) {
    digitalWrite(pinRelayACC, HIGH);
    digitalWrite(pinRelayON1, LOW);
    digitalWrite(pinRelayON2, LOW);
    digitalWrite(pinRelayStart, LOW);
  } else if (keyState == 2) {
    digitalWrite(pinRelayACC, HIGH);
    digitalWrite(pinRelayON1, HIGH);
    digitalWrite(pinRelayON2, HIGH);
    digitalWrite(pinRelayStart, LOW);

    if (fuelPrimed == 0) {
      for (int i = 0; i < 101; i++) {
        if (i == 100) {
          fuelPrimed = 1;
        }
      }
    }
  } else if (keyState == 3) {
    if (fuelPrimed == 0) {
      digitalWrite(pinRelayACC, HIGH);
      digitalWrite(pinRelayON1, HIGH);
      digitalWrite(pinRelayON2, HIGH);
      digitalWrite(pinRelayStart, LOW);

      if (timerStarter < 100) {
        timerStarter++;
      } else {
        timerStarter = 0;
        fuelPrimed = 1;
      }
    } else {
      digitalWrite(pinRelayACC, HIGH);
      digitalWrite(pinRelayON1, HIGH);
      digitalWrite(pinRelayON2, LOW);
      digitalWrite(pinRelayStart, HIGH);

      if (timerStarter < 100) {
        timerStarter++;
      } else {
        timerStarter = 0;
        keyStarted = 1;
        keyState = 2;
      }
    }
  }

  if (keyState >= 2) {
    lockPos = 180;
  } else {
    lockPos = 0;
  }
}

void turnSignalControl() {
  if (btnTurnLeft == 0 && stateTurnLeft == 0 && stateHazard == 0) {
    stateTurnRight = 0;
    stateTurnLeft = 1;
    // turnSignalBlink = 0;
    timerTurnSignal = delayTurnSignal;
  }

  if (btnTurnRight == 0 && stateTurnRight == 0 && stateHazard == 0) {
    stateTurnLeft = 0;
    stateTurnRight = 1;
    // turnSignalBlink = 0;
    timerTurnSignal = delayTurnSignal;
  }

  if (btnHazard == 0 && stateHazard == 0) {
    stateTurnLeft = 0;
    stateTurnRight = 0;
    stateHazard = 1;
    // turnSignalBlink = 0;
    timerTurnSignal = delayTurnSignal;
  }

  if (stateTurnLeft == 1) {
    if (timerTurnSignal == delayTurnSignal) {
      if (digitalRead(pinRelayTurnLeft) == 0) {
        digitalWrite(pinRelayTurnLeft, HIGH);
        digitalWrite(pinRelayTurnRight, LOW);
        tone(pinBuzzer, 500, 5);
      } else {
        turnSignalBlink++;
        digitalWrite(pinRelayTurnLeft, LOW);
        tone(pinBuzzer, 500, 5);
      }
      timerTurnSignal--;
    } else if (timerTurnSignal > 0) {
      timerTurnSignal--;
    } else {
      timerTurnSignal = delayTurnSignal;
    }

    if (turnSignalBlink >= laneChangeBlinkTimes && btnTurnLeft == 1) {
      stateTurnLeft = 0;
      turnSignalBlink = 0;
      digitalWrite(pinRelayTurnLeft, LOW);
    }
  }

  if (stateTurnRight == 1) {
    if (timerTurnSignal == delayTurnSignal) {
      if (digitalRead(pinRelayTurnRight) == 0) {
        digitalWrite(pinRelayTurnRight, HIGH);
        digitalWrite(pinRelayTurnLeft, LOW);
        tone(pinBuzzer, 500, 5);
      } else {
        turnSignalBlink++;
        digitalWrite(pinRelayTurnRight, LOW);
        tone(pinBuzzer, 500, 5);
      }
      timerTurnSignal--;
    } else if (timerTurnSignal > 0) {
      timerTurnSignal--;
    } else {
      timerTurnSignal = delayTurnSignal;
    }

    if (turnSignalBlink >= laneChangeBlinkTimes && btnTurnRight == 1) {
      stateTurnRight = 0;
      turnSignalBlink = 0;
      digitalWrite(pinRelayTurnRight, LOW);
    }
  }

  if (stateHazard == 1) {
    if (timerTurnSignal == delayTurnSignal) {
      if (digitalRead(pinRelayTurnLeft) == 0 || digitalRead(pinRelayTurnRight) == 0) {
        digitalWrite(pinRelayTurnLeft, HIGH);
        digitalWrite(pinRelayTurnRight, HIGH);
        tone(pinBuzzer, 500, 5);
      } else {
        turnSignalBlink++;
        digitalWrite(pinRelayTurnLeft, LOW);
        digitalWrite(pinRelayTurnRight, LOW);
        tone(pinBuzzer, 500, 5);
      }
      timerTurnSignal--;
    } else if (timerTurnSignal > 0) {
      timerTurnSignal--;
    } else {
      timerTurnSignal = delayTurnSignal;
    }

    if (btnHazard == 1) {
      stateHazard = 0;
      turnSignalBlink = 0;
      digitalWrite(pinRelayTurnLeft, LOW);
      digitalWrite(pinRelayTurnRight, LOW);
    }
  }
}

void lightControl() {
  if (keyState == 2) {
    // If key is ON
    if (swAuto == 0) {
      // TEMPORARY
      if(lightMode > 0) {
        lightMode--;
      }
    } else if(swLow == 0) {
      if(lightMode < 2) {
        lightMode++;
      }
    } else if(swPark == 0) {
      if(lightMode < 1) {
        lightMode++;
      } else if(lightMode > 1) {
        lightMode--;
      }
    } else {
      if(lightMode > 0) {
        lightMode--;
      }
    }
  } else {
    // If key is not ON
    if(swAuto == 0) {
      // TEMPORARY
      if (lightMode != 0) {
        digitalWrite(pinRelayParkLight, LOW);
        digitalWrite(pinRelayLowLight, LOW);
        lightMode = 0;
      }
    } else {
      if (lightMode != 0) {
        lightMode = 0;
      }
    }
  }

  // Set Relay
  if(lightMode == 2) {
    digitalWrite(pinRelayParkLight, HIGH);
    digitalWrite(pinRelayLowLight, HIGH);
  } else if(lightMode == 1) {
    digitalWrite(pinRelayParkLight, HIGH);
    digitalWrite(pinRelayLowLight, LOW);
  } else if(lightMode == 0) {
    digitalWrite(pinRelayParkLight, LOW);
    digitalWrite(pinRelayLowLight, LOW);
  }

  // High-beam control
  if((keyState == 2 && swHigh == 0 && swLow == 0) || swFlash == 0) {
    digitalWrite(pinRelayHighLight, HIGH);
    lightHighBeam = 1;
  } else if((keyState < 2 || swHigh == 1 || swLow == 1) && swFlash == 1) {
    digitalWrite(pinRelayHighLight, LOW);
    lightHighBeam = 0;
  }
}

void setup() {
  // put your setup code here, to run once:
  lcd.begin(20, 4);
  lcd.createChar(0, keyChar);
  lcd.createChar(1, signalMidChar);
  lcd.createChar(2, signalLeftChar);
  lcd.createChar(3, signalRightChar);
  lcd.createChar(4, lightChar);
  lcd.createChar(5, parkLightChar);
  lcd.createChar(6, lowLightChar);
  lcd.createChar(7, highLightChar);
  // 8 chars only... :(

  lcd.display();
  lcd.backlight();
  lcd.clear();
  timerLcdRefresh = 0;

  lockPos = 0;
  steeringLock.attach(pinServoLock);
  steeringLock.write(0);

  // Ignition Pins & Relays
  pinMode(pinBtnStart, INPUT_PULLUP);
  pinMode(pinBtnBrake, INPUT_PULLUP);
  pinMode(pinRelayACC, OUTPUT);
  pinMode(pinRelayON1, OUTPUT);
  pinMode(pinRelayON2, OUTPUT);
  pinMode(pinRelayStart, OUTPUT);
  pinMode(pinServoLock, OUTPUT);

  //Turn Signal Pins & Relays
  pinMode(pinSwitchTurnLeft, INPUT_PULLUP);
  pinMode(pinSwitchTurnRight, INPUT_PULLUP);
  pinMode(pinBtnHazard, INPUT_PULLUP);
  pinMode(pinRelayTurnLeft, OUTPUT);
  pinMode(pinRelayTurnRight, OUTPUT);

  // Light Pins & Relays
  pinMode(pinAutoLight, INPUT_PULLUP);
  pinMode(pinParkLight, INPUT_PULLUP);
  pinMode(pinLowLight, INPUT_PULLUP);
  pinMode(pinHighLight, INPUT_PULLUP);
  pinMode(pinFlashLight, INPUT_PULLUP);
  pinMode(pinRelayRunLight, OUTPUT);
  pinMode(pinRelayParkLight, OUTPUT);
  pinMode(pinRelayParkLight, OUTPUT);
  pinMode(pinRelayParkLight, OUTPUT);

  keyState = 0;
  lastKeyState = 0;
  btnStart = 1;
  lastBtnStart = 1;
  btnBrake = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  //delay(1);
  // Buttons for Ignition
  btnStart = digitalRead(pinBtnStart);
  btnBrake = digitalRead(pinBtnBrake);
  if (btnStart > lastBtnStart) {
    if (btnBrake == 0) {
      btnWithBrake();
    } else {
      btnWithoutBrake();
    }
  }
  lastBtnStart = btnStart;

  // Steering Lock
  steeringLock.write(lockPos);

  // Turn Signal Buttons
  btnTurnLeft = digitalRead(pinSwitchTurnLeft);
  btnTurnRight = digitalRead(pinSwitchTurnRight);
  btnHazard = digitalRead(pinBtnHazard);
  if (btnTurnLeft == 0 && btnTurnRight == 0 || keyState < 2) {
    btnTurnLeft = 1;
    btnTurnRight = 1;
  }
  if ((btnTurnRight == 0 && stateTurnLeft == 1 && stateCancel == 0) || (btnTurnLeft == 0 && stateTurnRight == 1 && stateCancel == 0)) {
    stateCancel = 1;
    turnSignalBlink = 3;
  } else {
    stateCancel = 0;
  }

  // Headlight Buttons
  swAuto = digitalRead(pinAutoLight);
  swPark = digitalRead(pinParkLight);
  swLow = digitalRead(pinLowLight);
  swHigh = digitalRead(pinHighLight);
  swFlash = digitalRead(pinFlashLight);

  // Run stuff
  ignitionControl();
  turnSignalControl();
  lightControl();
  // wiperControl();

  lcdDisplay();
}