#include "EEPROM.h"
#include <CAN.h>
#include <OBD2.h>

#define BUTTON_GPIO 5
#define SERIAL_TX 17
#define SERIAL_RX 16

#define EEPROM_SIZE 2
#define EEPROM_ADR_DISPLAYMODE 0
#define RESETDELAY 4000

uint8_t displaymode;
char tempString[5];

HardwareSerial SerialLED(1);

// ---------------------------------------------------------------------------------
void setup() {

  pinMode(BUTTON_GPIO, INPUT_PULLUP);

  SerialLED.begin(9600, SERIAL_8N1, SERIAL_TX, SERIAL_RX);
  SerialLED.write(0x76);      // Clear display command
  SerialLED.write(0x7A);      // Set brightness command byte
  SerialLED.write((byte)255); // 0-255 brightness data byte
  SerialLED.print("8888");

  EEPROM.begin(EEPROM_SIZE);
  displaymode = EEPROM.read(EEPROM_ADR_DISPLAYMODE);
  if (displaymode>3) {
    displaymode = 0;
  }
  CAN.setPins(22, 21);
  while (!OBD2.begin()) {
    delay(500);
  }
}


// ---------------------------------------------------------------------------------
void loop() {

  static uint8_t button = 0;
 
  SerialLED.write(0x79); // Send the Move Cursor Command
  SerialLED.write(0x00); // Send the data byte for position
 
  int16_t kmh = (int16_t) OBD2.pidRead(VEHICLE_SPEED);

  if (displaymode == 0) {
    uint8_t gear;
    if (kmh<1 || kmh>500) {
      gear = 0;
    }
    else {   
      int16_t ratio = (int16_t) (OBD2.pidRead(ENGINE_RPM)/kmh);
      if (ratio>100) {
        gear = 1;
      } else if (ratio>58) {
        gear = 2;
      } else if (ratio>42) {
        gear = 3;
      } else if (ratio>31) {
        gear = 4;
      } else if (ratio>25) {
        gear = 5;
      } else if (ratio>21) {
        gear = 6;
      } else {
        gear = 7;
      } 
    }
    sprintf(tempString, "%3d ", gear);
    SerialLED.print(tempString);
  }
 
  if (displaymode == 1) {
    if (kmh<0 || kmh>500) {
      SerialLED.print(" -- ");
    }
    else {
      sprintf(tempString, "%3d ", kmh);
      SerialLED.print(tempString);
    }
  }
 
  if (displaymode == 2) {
    int16_t oil = (int16_t) (OBD2.pidRead(ENGINE_OIL_TEMPERATURE));
    if (oil<0 ||oil>200) {
      SerialLED.print(" --");
    }
    else {
      sprintf(tempString, "%3d", oil);
      SerialLED.print(tempString);
    }
    SerialLED.write(0x7E);        // Digit #4 control
    SerialLED.write(0b01100011);  // set segments     
  }
 
  if (displaymode == 3) {
    int16_t h2o = (int16_t) (OBD2.pidRead(ENGINE_COOLANT_TEMPERATURE));
    if (h2o<0 || h2o>200) {
      SerialLED.print(" --c");
    }
    else {
      sprintf(tempString, "%3dc", h2o);
      SerialLED.print(tempString);
    }
  }

 
  // BUTTON TO CONTROL DISPLAY MODE
  if (!digitalRead(BUTTON_GPIO)) {
   if (!button) {
      timer = millis();
    }
    if (button && millis() - timer > RESETDELAY) {
    SerialLED.print(" Go ");
      delay(2000);
      // RESET GEAR RATIOS HERE... TODO
    }
    else {
      button = 1;
    }
  }
  else {
    if (button) {
      switchDisplayMode();
    }
    button = 0;
  }

  delay(50);
}


// ---------------------------------------------------------------------------------
void switchDisplayMode(void) {
  displaymode++;
  if (displaymode>3) {
    displaymode = 0;
  }
  EEPROM.write(EEPROM_ADR_DISPLAYMODE, displaymode);
  EEPROM.commit();
}


// ---------------------------------------------------------------------------------

#include <CAN.h>
#include <OBD2.h>

CAN.setPins(22, 21);

while (!OBD2.begin()) {
  delay(500);
}
 
int16_t kmh = (int16_t) OBD2.pidRead(VEHICLE_SPEED);

12 st          clips           51489119216
1 st           panel V         41007294289
1 st           panel H         41007294290
1 st           ljuddämp V      51487334357
1 st           ljuddämp H      51487334358
1 st           ljuddämp V      51487296055
1 st           ljuddämp H      51487296056
12 st          clips           51471911992
1st            gummilist       51487300693
1st            gummilist       51487300694