#include <Keypad.h>
#include <Wire.h>
#include <LiquidTWI.h>

LiquidTWI lcd(0);

#define BACKLIGHT_RED 3
#define BACKLIGHT_GREEN 5
#define BACKLIGHT_BLUE 6

#define MLTIPLX_S0 2
#define MLTIPLX_S1 4
#define MLTIPLX_S2 7

#define ANALOG_SENSOR_INPUT 3

//const byte ROWS = 4; //four rows
//const byte COLS = 3; //three columns
//char keys[ROWS][COLS] = {
//  {'1', '2', '3'},
//  {'4', '5', '6'},
//  {'7', '8', '9'},
//  {'*', '0', '#'}
//};
//byte rowPins[ROWS] = {8, 9, 10, 11}; //connect to the row pinouts of the keypad
//byte colPins[COLS] = {2, 4, 7}; //connect to the column pinouts of the keypad

//Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );

// Declare multiplexer input variables;
int mltiplxInputCount = 0;
int mltiplxS0 = 0;
int mltiplxS1 = 0;
int mltiplxS2 = 0;
int rawAnalogValue;

// Declare display values
int waterTemp;
int transTemp;
int vacuumPressure;

byte oil[8] = {
  B00100,
  B00100,
  B01110,
  B01110,
  B11111,
  B11111,
  B01110,
  B00000
};

byte temp[8] = {
  B00110,
  B00100,
  B00110,
  B00100,
  B00110,
  B00100,
  B01110,
  B01110
};

byte volt[8] = {
  B00010,
  B00110,
  B01100,
  B11110,
  B01111,
  B00110,
  B01100,
  B01000
};

byte one_fifth[8] = {
  0b10000,
  0b10000,
  0b10000,
  0b10000,
  0b10000,
  0b10000,
  0b10000,
  0b10000
};

byte two_fifth[8] = {
  0b11000,
  0b11000,
  0b11000,
  0b11000,
  0b11000,
  0b11000,
  0b11000,
  0b11000
};

byte three_fifth[8] = {
  0b11100,
  0b11100,
  0b11100,
  0b11100,
  0b11100,
  0b11100,
  0b11100,
  0b11100
};

byte four_fifth[8] = {
  0b11110,
  0b11110,
  0b11110,
  0b11110,
  0b11110,
  0b11110,
  0b11110,
  0b11110
};

byte five_fifth[8] = {
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111
};
void initializeLCD() {
  lcd.begin(20, 4);
  lcd.createChar(0, oil);
  lcd.createChar(1, temp);
  lcd.createChar(2, volt);
  lcd.createChar(3, one_fifth);
  lcd.createChar(4, two_fifth);
  lcd.createChar(5, three_fifth);
  lcd.createChar(6, four_fifth);
  lcd.createChar(7, five_fifth);
}

void displayDashStaticText() {
  // First Line
  lcd.setCursor(0, 0);
  lcd.print("****          ***mph");

  // Second Line
  lcd.setCursor(0, 1);
  lcd.print("E         F  ****mpg");

  //Third Line
  lcd.setCursor(0, 2);
  lcd.print(" **psi  *** F  ****V");
  lcd.setCursor(0, 2);
  lcd.write(byte(0));
  lcd.setCursor(7, 2);
  lcd.write(byte(1));
  lcd.setCursor(11, 2);
  lcd.print((char)223);
  lcd.setCursor(14, 2);
  lcd.write(byte(2));

  //Fourth Line
  lcd.setCursor(0, 3);
  lcd.print("TF *** F  Vac **inHg");
  lcd.setCursor(2, 3);
  lcd.write(byte(1));
  lcd.setCursor(6, 3);
  lcd.print((char)223);
}

void setup() {
  initializeLCD();
  displayDashStaticText();

  //Set Pin Modes
  pinMode(BACKLIGHT_RED, OUTPUT);
  pinMode(BACKLIGHT_GREEN, OUTPUT);
  pinMode(BACKLIGHT_BLUE, OUTPUT);
  pinMode(MLTIPLX_S0, OUTPUT);
  pinMode(MLTIPLX_S1, OUTPUT);
  pinMode(MLTIPLX_S2, OUTPUT);

  setBacklightStandard();

  Serial.begin(9600);
}

void loop() {

  for (int mltiplxInputCount = 0; mltiplxInputCount <= 7; mltiplxInputCount++) {
    mltiplxS0 = bitRead(mltiplxInputCount, 0);
    mltiplxS1 = bitRead(mltiplxInputCount, 1);
    mltiplxS2 = bitRead(mltiplxInputCount, 2);

    digitalWrite(MLTIPLX_S0, mltiplxS0);
    digitalWrite(MLTIPLX_S1, mltiplxS1);
    digitalWrite(MLTIPLX_S2, mltiplxS2);

    rawAnalogValue = analogRead(ANALOG_SENSOR_INPUT);

    switch (mltiplxInputCount) {
      case 1: //trans temp sensor
        transTemp = map(rawAnalogValue, 0, 1023, 207, -1);
        setTransTempTo(transTemp);
        break;
      case 2: //coolant temp sensor
        waterTemp = map(rawAnalogValue, 0, 1023, 207, -1);
        setWaterTempTo(waterTemp);
        break;
      case 5: //vacuum sensor
        vacuumPressure = map(rawAnalogValue, 0, 940, 15, 0);
        setVacTo(vacuumPressure);
        break;
    }
  }


  //  int test_value = analogRead(2);
  //  int rpm = map(test_value, 0, 1023, 0, 6000);
  //  setTachTo(rpm);
  //
  //  int mph = map(test_value, 0, 1023, 0, 120);
  //  setSpeedoTo(mph);
  //
  //  int fuel = map(test_value, 0, 1023, 0, 100);
  //  setFuelTo(fuel);
  //
  //  float mpg = map(test_value, 0, 1023, 0, 200) / 10.0;
  //  setMilageTo(mpg);
  //
  //  int psi = map(test_value, 0, 1023, 0, 99);
  //  setOilPressureTo(psi);
  //
  //  int waterTemp = map(test_value, 0, 1023, 0, 240);
  //  setWaterTempTo(waterTemp);
  //
  //  float volts = map(test_value, 0, 1023, 0, 180) / 10.0;
  //  setVoltageTo(volts);
  //
  //  int transTemp = map(test_value, 0, 1023, 0, 300);
  //  setTransTempTo(transTemp);
  //
  //  int vac = map(test_value, 0, 1023, 0, 16);
  //  setVacTo(vac);



}

void setBacklight(uint8_t r, uint8_t g, uint8_t b) {
  // common anode so invert!
  r = map(r, 0, 255, 255, 0);
  g = map(g, 0, 255, 255, 0);
  b = map(b, 0, 255, 255, 0);
  analogWrite(BACKLIGHT_RED, r);
  analogWrite(BACKLIGHT_GREEN, g);
  analogWrite(BACKLIGHT_BLUE, b);
}

void setBacklightStandard() {
  setBacklight(255, 255, 30);
}

void printPaddedValue(int value, int startCol, int startRow, int maxWidth) {
  lcd.setCursor(startCol, startRow);
  switch (maxWidth) {
    case 2:
      char buffer2[2];
      sprintf(buffer2, "%2d", value);
      lcd.print(buffer2);
      break;
    case 3:
      char buffer3[3];
      sprintf(buffer3, "%3d", value);
      lcd.print(buffer3);
      break;
    case 4:
      char buffer4[4];
      sprintf(buffer4, "%3d", value);
      lcd.print(buffer4);
      break;
  }
}

void printFractionalValue(float value, int startCol, int startRow) {
  char buffer[4];
  dtostrf(value, 4, 1, buffer);
  lcd.setCursor(startCol, startRow);
  lcd.print(buffer);
}


void setGaugeValue(int percent, int startCol, int startRow, int width) {
  lcd.setCursor(startCol, startRow);

  int totalSegments = width * 5;
  int usedSegments = totalSegments * (percent / 100.0);
  int blankCharactars = width;
  while (usedSegments > 0) {
    if (usedSegments > 4) {
      lcd.write(byte(7));
      usedSegments -= 5;
      blankCharactars -= 1;
    } else {
      switch (usedSegments) {
        case 0:
          break;
        case 1:
          lcd.write(byte(3));
          break;
        case 2:
          lcd.write(byte(4));
          break;
        case 3:
          lcd.write(byte(5));
          break;
        case 4:
          lcd.write(byte(6));
          break;

      }
      usedSegments = 0;
      blankCharactars -= 1;
    }
  }

  for (int i = 0; i < blankCharactars; i++) {
    lcd.print(" ");
  }


}

void extract(int value) {
  lcd.setCursor(0, 0);
  char buffer[4];
  sprintf(buffer, "%4d", value);
  Serial.print(buffer);
  Serial.print("\n");
  lcd.print(buffer);
}

void setTachTo(int rpm) {
  float rpmPercent = (rpm / 6000.0) * 100;
  setGaugeValue(rpmPercent, 4, 0, 9);
  printPaddedValue(rpm, 0, 0, 4);
}

void setSpeedoTo(int mph) {
  printPaddedValue(mph, 14, 0, 3);
}

void setMilageTo(float mpg) {
  printFractionalValue(mpg, 13, 1);
}

void setFuelTo(int percent) {
  float fuelPercent = percent / 100.0;
  setGaugeValue(percent, 2, 1, 7);
}

void setOilPressureTo(int psi) {
  printPaddedValue(psi, 1, 2, 2);
}

void setWaterTempTo(int temp) {
  printPaddedValue(temp, 8, 2, 3);
}

void setVoltageTo(float volts) {
  printFractionalValue(volts, 15, 2);
}

void setTransTempTo(int temp) {
  printPaddedValue(temp, 3, 3, 3);
}

void setVacTo(int vac) {
  printPaddedValue(vac, 14, 3, 2);
}

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

//  char key = keypad.getKey();
//
//  if (key != NO_KEY) {
//    Serial.println(key);
//
//    value = key - '0';
//    Serial.println(double(key));
//
//    test = ((double(key) - 48) * 10) / 100.0;
//    //    Serial.println(test);
//
//  }


}