/*************************************************************************
* Inpired from simple_obd_display.ino by Stanley Huang <stanley@freematics.com.au>
* (Simple OBD Data Display for Freematics OBD-II Adapter)
*************************************************************************/
#include <MyLCD.h>
#include "MyOBD.h"
MyLCD lcd;
MyOBD obd;

void setup()
{
  lcd.begin();
  obd.debug=1;
  lcd.println("obd.begin()");
  obd.begin();
  reconnect();
  showDTC();

  delay(20*1000); // Freeze for 20 seconds before displaying real-time PID values
  obd.debug=0;
  lcd.begin();
  initScreen();
}

void reconnect()
{
  lcd.println("obd.init()");
  while (!obd.init()) {
    lcd.println("obd.init() failed");
  }
}

void showDTC() {
  static char buf[10];
  static uint16_t codes[16];
  byte c, count;
  lcd.println("obd.readDTC()");
  count = obd.readDTC(codes, 16);
  lcd.print("DTC: ");
  if ( count==0 ) {
    lcd.println("none");
  } else {
    for (c=0; c<count; c++) {
        sprintf(buf, "P%04u ", codes[c]);
        lcd.println(buf);
    }
    //lcd.println();
  }
}

void loop()
{
  static byte pids[]= {PID_RPM, PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE};
  static byte index = 0, prev_err=obd.errors;
  static long prevRead = 0;
  long wait;
  byte pid = pids[index];
  int value;

  // Don't read on CAN bus to fast, it struggle and reset main car dashboard
  wait = millis() - prevRead + 25; // 40 Hz, 10 FPS for 4 pids
  if ( wait > 0 ) delay(wait);
  prevRead = millis();

  // send a query to OBD adapter for specified OBD-II pid
  if (obd.readPID(pid, value)) {
      showData(pid, value);
  }
  index = (index + 1) % sizeof(pids);

  if (obd.errors > prev_err) {
      showData(0, obd.errors);
  }
  if ( obd.errors > 10 ) {
    obd.errors=0;
    reconnect();
  }
}

void initScreen()
{
  lcd.setCursor(1,4);
  lcd.println("comm. errors:       ");
  lcd.println("         rpm:       ");
  lcd.println("       speed:        km/h");
  lcd.println("       throt:        %");
  lcd.println("        load:       ");
  showData(0, obd.errors);
}

void showData(byte pid, int value)
{
  static char buf[10];
  int res = sprintf(buf, "%6d", value);
  switch (pid) {
  case 0:               lcd.setCursor(15,4); lcd.print(buf); break;
  case PID_RPM:         lcd.setCursor(15,5); lcd.print(buf); break;
  case PID_SPEED:       lcd.setCursor(15,6); lcd.print(buf); break;
  case PID_THROTTLE:    lcd.setCursor(15,7); lcd.print(buf); break;
  case PID_ENGINE_LOAD: lcd.setCursor(15,8); lcd.print(buf); break;
  }
}