#include <SoftwareSerial.h>

#define rxPin 8
#define txPin 7
#define K_IN  0
#define K_OUT 1

#define READ_ATTEMPTS 125

SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);
char command; 												//Terminal Commands

char pid_reslen[] = 										//PID Lengths
{
  // pid 0x00 to 0x1F
  4,4,2,2,1,1,1,1,1,1,1,1,2,1,1,1,
  2,1,1,1,2,2,2,2,2,2,2,2,1,1,1,4,

  // pid 0x20 to 0x3F
  4,2,2,2,4,4,4,4,4,4,4,4,1,1,1,1,
  1,2,2,1,4,4,4,4,4,4,4,4,2,2,2,2,

  // pid 0x40 to 0x4E
  4,8,2,2,2,1,1,1,1,1,1,1,1,2,2
};

long tempLong;
char str[40];

void setup()  {
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  digitalWrite(txPin, HIGH);
  delay(500);
  pinMode(K_IN, INPUT);
  pinMode(K_OUT, OUTPUT);
  pinMode(13, OUTPUT);
  mySerial.begin(9600);
  mySerial.println("-=Arduino OBD2 Terminal=-");
}

void loop() {
  command = mySerial.read();
  switch (command) {
  case '1':
    obdConnect();
    break;
  case '2':
    mySerial.println("MIL Selected");
	get_pid(0x01, str, &tempLong);
    break;
  case '3':
    mySerial.println("RPM Selected");
	get_pid(0x03, str, &tempLong);
    break;
  case '4':
    mySerial.println("MAF Selected");
	get_pid(0x10, str, &tempLong);
    break;
  }
}


int obdConnect(){
  mySerial.println("Attempting ECU initialization...");
  if (iso_init() == 0){
    mySerial.println("PASS");
    return 0;
  }
  else{
    mySerial.println("FAIL");
    return 1;
  }
}


byte iso_init()
{
  byte b;

  serial_tx_off(); 							//disable UART so we can "bit-Bang" the slow init.
  serial_rx_off();
  delay(3000); 								//k line should be free of traffic for at least two secconds.
  
  
  digitalWrite(K_OUT, HIGH);				// drive K line high for 300ms
  delay(300);

  // send 0x33 at 5 bauds
  
  digitalWrite(K_OUT, LOW); 				 // start bit
  delay(200);

  b=0x33;
  for (byte mask = 0x01; mask != 0; mask <<= 1)
  {
    if (b & mask) 
      digitalWrite(K_OUT, HIGH); 
    else
      digitalWrite(K_OUT, LOW);
    delay(200);
  }

  digitalWrite(K_OUT, HIGH);                // stop bit + 60 ms delay
  delay(260);

  
  Serial.begin(10400);						// switch now to 10400 bauds

  
  byte i=0;									// wait for 0x55 from the ECU (up to 300ms)
  while(i<3 && !iso_read_byte(&b)){
    	i++;
  }
  mySerial.println(b, BYTE);
  if(b != 0x55){
        return 1;
  }
  
  iso_read_byte(&b);
  mySerial.println(b, BYTE);					// wait for kw1 and kw2
  iso_read_byte(&b);
  mySerial.println(b, BYTE);
  iso_write_byte(~b);           		  	// send ~kw2 (invert of last keyword)
  iso_read_byte(&b);
  mySerial.println(b, BYTE);  	    // ECU answer by 0xCC (~0x33)
  if(b!=0xCC)
    return 1;
  
  return 0;
}



void serial_rx_off() {
  UCSR0B &= ~(_BV(RXEN0));  
}

void serial_tx_off() {
   UCSR0B &= ~(_BV(TXEN0));  
   delay(20);                 				//allow time for buffers to flush
}

void serial_rx_on() {
  Serial.begin(10400);
}


boolean iso_read_byte(byte * b)
{
  int readData;
  boolean success = true;
  byte t=0;

  while(t != READ_ATTEMPTS  && (readData=Serial.read())==-1) {
    delay(1);
    t++;
  }
  if (t>=READ_ATTEMPTS) {
    success = false;
  }
  if (success)
  {
    *b = (byte) readData;
  }

  return success;
}

void iso_write_byte(byte b)
{
  serial_rx_off();
  Serial.print(b);
  delay(10);	
  serial_rx_on();
}


byte iso_checksum(byte *data, byte len)
{
  byte i;
  byte crc;

  crc=0;
  for(i=0; i<len; i++)
    crc=crc+data[i];

  return crc;
}

byte iso_write_data(byte *data, byte len)
{
  byte i, n;
  byte buf[20];

  					// ISO header
  buf[0]=0x68;
  buf[1]=0x6A;		// 0x68 0x6A is an OBD-II request
  buf[2]=0xF1;		// our requesterÕs address (off-board tool)


  // append message
  for(i=0; i<len; i++)
    buf[i+3]=data[i];

  // calculate checksum
  i+=3;
  buf[i]=iso_checksum(buf, i);

  // send char one by one
  n=i+1;
  for(i=0; i<n; i++)
  {
    iso_write_byte(buf[i]);
  }

  return 0;
}

// read n byte(s) of data (+ header + cmd and crc)
// return the count of bytes of message (includes all data in message)
byte iso_read_data(byte *data, byte len)
{
  byte i;
  byte buf[20];
  byte dataSize = 0;

  for(i=0; i<len+6; i++)
  {
    if (iso_read_byte(buf+i))
    {
      dataSize++;
    }
  }
  
  memcpy(data, buf+5, len);
  delay(55);    //guarantee 55 ms pause between requests
  return dataSize;
}

boolean get_pid(byte pid, char *retbuf, long *ret)
{
  byte cmd[2];    												// to send the command
  byte buf[10];   												// to receive the result
  
  byte reslen = pid_reslen[pid]; 						    	// receive length depends on pid
 
  cmd[0]=0x01;    												// ISO cmd 1, get PID
  cmd[1]=pid;
  
  iso_write_data(cmd, 2); 										// send command, length 2
  
  if (!iso_read_data(buf, reslen)) 								// read requested length, n bytes received in buf
  {
      mySerial.println("ISO Read Data Error.");
      return false; 
  }

  *ret=buf[0]*256U+buf[1]; 										// a lot of formulas are the same so calculate a default return value here
  
  mySerial.print("Return Value ");
  mySerial.print(pid, DEC);
  mySerial.print(" : ");
  mySerial.println(*ret);
  
  return true;
}