/*************************************************************************
* 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();
  lcd.print("obd.begin()");
  obd.begin();
  lcd.println(" done");
  reconnect();
  showDTC();
  initScreen();
}

void reconnect()
{
  lcd.setCursor(1,2);
  lcd.print("obd.init()                       ");
  lcd.setCursor(11,2);
  while (!obd.init()) {
    lcd.print(".");
  }
  lcd.println(" done");
}

void showDTC() {
  static char buf[10];
  static uint16_t codes[16];
  byte c, 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.print(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;
  byte pid = pids[index];
  int value;
  // 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;
  }
}