#include <Adafruit_LiquidCrystal.h>
Adafruit_LiquidCrystal lcd(10, 2, 3, 6, 7, 8); // Creates an LC object. Parameters: (rs, enable, d4, d5, d6, d7)

// Engine data from FORD CanBUS
float engine_coolant_temperature = 0.0;
float engine_rpm = 0.0;
float engine_tps = 0.0;
float pedal_position = 0.0;
float engine_torque = 0.0;
float engine_manifold_pressure = 0.0;
int brake_switch = 0;



// Engine data to VW DSG CanBUS


/////////////////////////////////

// Engine data from VW DSG CanBUS


////////////////////////////////

// Setup Pins for Up/Down shift paddles //
const int upShiftPin = 5;    
const int downShiftPin = 4;
/////////////////////////////////////////

// Drive Modes //
const int neutralShiftPin = 9;
const int manualShiftPin = 13;
const int sportModeShiftPin = 14;
const int driveModeShiftPin = 16;
const int parkShiftPin = 15;

// Drive Mode LED Statuses //
const int upShiftLEDPin =  12;
const int downShiftLEDPin = 11;
const int neutralShiftLEDPin = 17;
const int manualShiftLEDPin = 18;
const int sportmodeShiftLEDPin = 19;
const int drivemodeShiftLEDPin = 20;
const int parkShiftLEDPin = 21;

/////////////////////////////////////

// States of Up/Down shift paddles //
int upShiftState = 0;
int downShiftState = 0;
int driveShiftState = 0;
int neutralShiftState = 0;
int parkShiftState = 0;
int sportModeShiftState = 0;
int reverseShiftState = 0;
int manualShiftState = 0;
int lastupShiftState = 0;
int lastdownShiftState = 0;
int lastdriveShiftState = 0;
int lastneutralShiftState = 0;
int lastparkShiftState = 0;
int lastreverseShiftState = 0;
int lastsportsModeshiftState = 0;
int lastmanualShiftState = 0;
int driveMode = 0;
bool manualSelected = false;
// Current Gear and Required gear //
int currentGear = 0;
int requiredGear = 0;
int max_gears = 6;
void setup() {
  Serial.begin(9600);
  // initialize the LED pin as an output:
  pinMode(upShiftLEDPin, OUTPUT);
  pinMode(downShiftLEDPin, OUTPUT);
  
  // initialize the pushbutton pin as an input:
  pinMode(upShiftPin, INPUT);
  pinMode(downShiftPin, INPUT);
  pinMode(driveModeShiftPin, INPUT);
  pinMode(neutralShiftPin, INPUT);
  pinMode(parkShiftPin, INPUT);
  pinMode(sportModeShiftPin, INPUT);
  pinMode(manualShiftPin, INPUT);
  lcd.begin(16,2);
  lcd.setCursor(0,1);
}

// Gear change loop //
void loop() {
  // read the state of the pushbutton values:
  upShiftState = digitalRead(upShiftPin);
  downShiftState = digitalRead(downShiftPin);
  neutralShiftState = digitalRead(neutralShiftPin);
  driveShiftState = digitalRead(driveModeShiftPin);
  parkShiftState = digitalRead(parkShiftPin);
  sportModeShiftState = digitalRead(sportModeShiftPin); 
  manualShiftState = digitalRead(manualShiftPin);
  String lcdText9 = "D";
  String lcdText3 = "N";
  String lcdText4 = "R";
  String lcdText5 = "P";
  String lcdText6 = "S";
  String lcdText7 = "+/-";
  String lcdText8 = "LC"; 

// Manual Mode Selected //
  if (manualShiftState != lastmanualShiftState) {
    if(manualShiftState == HIGH){
      lcd.clear();   
      manualSelected = true;               
      Serial.println(" Manual Mode ");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText7;
      lcd.print(lcdText2);
      delay(50);
    }
  } 

  
  // Manual Paddle Control Selected //
  // Paddles will only allowed to be used when in Manual Mode //
  if (manualSelected & upShiftState != lastupShiftState) {
     if (upShiftState == HIGH & manualShiftState == HIGH) { 
      lcd.clear();      
      digitalWrite(upShiftLEDPin, HIGH);
      digitalWrite(upShiftPin, LOW);
      digitalWrite(upShiftLEDPin, LOW);
      Serial.println(" Upshift ");
      requiredGear += 1;
      currentGear = constrain(requiredGear, 0, max_gears);
      Serial.println(currentGear);
      String lcdText = "Current Gear  ";      
      String lcdText2 = lcdText + currentGear;
      if(currentGear == 0){        
        lcdText2 = lcdText + lcdText3;
      } else if (currentGear == -1){        
        lcdText2 = lcdText + lcdText5;
      }
        lcd.print(lcdText2);      
        delay(150);
     } 
  }  
 else if (manualSelected & downShiftState != lastdownShiftState) {
     if (downShiftState == HIGH & manualShiftState == HIGH) {
      lcd.clear();
      digitalWrite(downShiftLEDPin, HIGH);
      digitalWrite(downShiftPin, LOW);
      digitalWrite(downShiftLEDPin, LOW);
      Serial.println(" Downshift ");
      requiredGear -= 1;
      currentGear = constrain(requiredGear, 0, max_gears);
      Serial.println(currentGear); 
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + currentGear;
      if(currentGear == 0){
        lcdText2 = lcdText + lcdText3;
      } else if (currentGear == -1){
        lcdText2 = lcdText + lcdText5;
      }   
      lcd.print(lcdText2);
      delay(150);  
    }    
  }
   // Drive Selected //
  else if (driveShiftState != lastdriveShiftState) {
    if(driveShiftState == HIGH){
      lcd.clear();       
      driveMode = 1;     
      Serial.println(" Drive ");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText9;
      lcd.print(lcdText2);
      delay(150);
    }
  } 
  // Neutral Selected //
  else if (neutralShiftState != lastneutralShiftState) {
    if(neutralShiftState == HIGH){
      lcd.clear();      
      driveMode = 0;
      requiredGear = 0;
      Serial.println(" Neutral ");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText3;
      lcd.print(lcdText2);
      delay(150);
    }
  }
  // Park Selected //
  else if (parkShiftState != lastparkShiftState) {
    if(parkShiftState == HIGH){
      lcd.clear();      
      driveMode = -1;
      Serial.println(" Park ");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText5;
      lcd.print(lcdText2);
      delay(150);
    }
  }
  // Reverse Selected //
   else if (reverseShiftState != lastreverseShiftState) {
    if(reverseShiftState == HIGH){
      lcd.clear();      
      driveMode = -2;
      Serial.println(" Reverse ");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText4;
      lcd.print(lcdText2);
      delay(150);
    }
  }
  // Sport Selected //
   else if (sportModeShiftState != lastsportsModeshiftState) {
    if(sportModeShiftState == HIGH){
      lcd.clear();      
      driveMode = -3;
      Serial.println(" Sport");
      String lcdText = "Current Gear  ";
      String lcdText2 = lcdText + lcdText6;
      lcd.print(lcdText2);
      delay(150);
    }
  }
  
  lastupShiftState = upShiftState;
  lastdownShiftState = downShiftState;
  lastdriveShiftState = driveShiftState;
  lastneutralShiftState = neutralShiftState;
  lastparkShiftState = parkShiftState;
  lastsportsModeshiftState = sportModeShiftState;
  lastmanualShiftState = manualShiftState;
}