// obdii.c
// for NerdKits with ATmega168
// mrobbins@mit.edu

// #define DEBUG

#define F_CPU 14745600

#include <stdio.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <inttypes.h>

#include "../libnerdkits/delay.h"
#include "../libnerdkits/uart.h"
#include "../libnerdkits/lcd.h"

// PIN DEFINITIONS:
//
// PC5 -- input (use pull-up; inverted; a HIGH on the VPW bus looks LOW here)
//	PCINT13
// PC4 -- output (a HIGH pulls the VPW bus HIGH)

// for CLK/64:
//#define T_64US 15
//#define T_128US 29
//#define T_200US 46
// for CLK/8:
#define T_64US 118
#define T_128US 236
#define T_200US 368
#define T_200US_2 (T_200US - 256)

volatile uint8_t writer_started = 0;
volatile uint8_t writer_extra = 0;
volatile uint8_t writer_size = 0;
unsigned char writer_data[15];
volatile uint8_t writer_counter;
volatile uint8_t writer_bit_counter;
volatile uint8_t writer_collision;

volatile uint8_t reader_size = 0;
volatile unsigned char reader_data[15];
volatile uint8_t reader_pin_last=0;
volatile char reader_byte=0;
volatile uint8_t reader_byte_counter=0;
volatile uint8_t reader_started = 0;

void hex_print(unsigned char *buf, uint8_t len) {
  uint8_t i;
  uint8_t x;
  for(i=0; i<len; i++) {
    x = buf[i];
    printf_P(PSTR("%02x "), x);
  }
}

// This checksum code originally from Bruce Lightner,
// Circuit Cellar Issue 183, October 2005
// ftp://ftp.circuitcellar.com/pub/Circuit_Cellar/2005/183/Lightner-183.zip
unsigned char crc8buf(unsigned char *buf, uint8_t len) {
    unsigned char val;
    unsigned char i;
    unsigned char chksum;

    chksum = 0xff;  // start with all one's
    while (len--) {
        i = 8;
        val = *buf++;
        while (i--) {
            if (((val ^ chksum) & 0x80) != 0) {
                chksum ^= 0x0e;
                chksum = (chksum << 1) | 1;
            } else {
                chksum = chksum << 1;
            }
            val = val << 1;
        }
    }

    return ~chksum;
}


SIGNAL(SIG_OUTPUT_COMPARE0A) {
  if(!writer_started)
	return;

  // to extend period more than 256
  if(writer_extra > 0) {
    OCR0A = writer_extra;
    writer_extra = 0;
    return;
  }
  
  // check for collision
  if(writer_collision) {
    // stop writing!  de-assert.
    PORTC &= ~(1<<PC4);	// should be de-asserted anyway.
    writer_started = 0;
    TIMSK0 &= ~(1<<OCIE0A);

#ifdef DEBUG    
    printf_P("WCOL\r\n");
#endif
    return;
  }

  uint8_t this_bit;
  this_bit = writer_data[writer_counter] >> (7-writer_bit_counter);
  this_bit = this_bit & 0x01;
  
  // toggle bit and set timer
  if((writer_bit_counter & 0x01) == 0) {
    // bits 0,2,4,6
    PORTC &= ~(1<<PC4);
    if(this_bit) {
      OCR0A = T_128US;
    } else {
      OCR0A = T_64US;
    }
  } else {
    // bits 1,3,5,7
    PORTC |= (1<<PC4);
    if(this_bit) {
      OCR0A = T_64US;
    } else {
      OCR0A = T_128US;
    }
  }

  // override in case of end-of-frame
  if(writer_counter == writer_size) {
    OCR0A = 255;
    writer_extra = T_200US_2;
  }
  
  // update counters
  writer_bit_counter++;
  if(writer_bit_counter == 8) {
    writer_counter++;
    writer_bit_counter = 0;
  }
  
  // when done, disable our own interrupt from firing.
  if((writer_counter == writer_size) && (writer_bit_counter == 1)) {
    TIMSK0 &= ~(1<<OCIE0A);
#ifdef DEBUG
    //printf_P(PSTR("SEND SUCCESSFUL\r\n"));
#endif
    writer_started = 0;
  }
}

void send_break() {
  // hold PC4 high for 800us
  PORTC |= (1<<PC4);
  delay_us(800);
  PORTC &= ~(1<<PC4);
}

void writer_send(unsigned char *buf, uint8_t len) {
  // block out receiver
  writer_started = 1;

  // FIXME: should check if we're already busy sending stuff
  uint8_t i;
  for(i=0; i<len; i++) {
    writer_data[i] = buf[i];
  }
  // add the checksum
  writer_data[len] = crc8buf(buf, len);
  writer_data[len+1] = 0;	// For EOF bit to be low
  writer_size = len + 1; // plus one is for checksum

#ifdef DEBUG
//  printf_P(PSTR("SEND "));
//  hex_print(writer_data, writer_size);
//  printf_P(PSTR("\r\n"));
#endif
  
  writer_counter = 0;
  writer_bit_counter = 0;
  writer_collision = 0;
  
  // wait for reader to finish
  while(reader_started) {
  }
  
  // set SOF period, start timer, and enable its interrupt
  OCR0A = 255;
  writer_extra = T_200US_2;
  TCNT0 = 0;
  TIFR0 |= (1<<OCF0A);	// clear interrupt flag in case it was already set
  TIMSK0 |= (1<<OCIE0A); // enable it
  PORTC |= (1<<PC4);

  //printf_P(PSTR("LEAVING writer_send\r\n"));
}

void writer_init() {
  // PC4 output
  DDRC |= (1<<PC4);
  
  // Timer0 setup CLK/8, CTC on OCR0A match
  TCCR0A |= (1<<WGM01);
  TCCR0B |= (1<<CS01);
  // Timer0 Output Compare Interrupt A enable
  TIMSK0 |= (1<<OCIE0A);  
}


SIGNAL(SIG_PIN_CHANGE1) {
  // record timer value
  uint16_t timer_val = TCNT1;
  TCNT1 = 0;
  uint8_t cur_pin_value = (PINC>>PC5) & 0x01;

#ifdef DEBUG
  // copy interpreted bus voltage signal onto PC3 for debugging fun
  if(!writer_started) {
    if(cur_pin_value) {
      PORTC &= ~(1<<PC3);
    } else {
      PORTC |= (1<<PC3);
    }
  }
#endif

  
  // do collision checking magic if writer is active
  if(writer_started) {
    // check for collision
    if((PORTC & (1<<PC4)) == 0) {
      // we're not dominant.
      if(cur_pin_value == 0) {
        // but somebody is driving the bus!!!
        // collision has occured
        writer_collision = 1;
      }
    }
    
    return;
  }


  //verify that a transition has happened
  if(cur_pin_value == reader_pin_last || timer_val < 8)
    return;	// glitched transition; ignore
  reader_pin_last = cur_pin_value;
  
  
  // restart timer and clear match flag (for next time)
  //if((TIFR1 & (1<<OCF1A)) != 0)
  //  TIFR1 |= (1<<OCF1A);

#ifdef DEBUG
  //printf_P(PSTR("PC1 %d %d\r\n"), cur_pin_value, timer_val);
  //return;
#endif  
  
  // discriminate based on timer_val
  uint8_t bit_choice;
  if(timer_val < 37) {	// 160.6us
    if(!reader_started)
      return;
      
    if(timer_val < 22) {		// 95.4us
      // small period (64us)
      bit_choice = cur_pin_value ? 1 : 0;
    } else {
      // large period (128us)
      bit_choice = cur_pin_value ? 0 : 1;
    }
    
    // YAY! we can haz new bit 
    reader_byte = (reader_byte << 1);
    reader_byte |= bit_choice;
    reader_byte_counter++;
    if(reader_byte_counter == 8) {
      // DOUBLE YAY! we can has new byte
      reader_data[reader_size] = reader_byte;
      reader_byte_counter = 0;
      reader_byte = 0;
      reader_size++;
      
      // check for overflow
      if(reader_size == 12) {
        // FIXME: probably nothing required here
      }
    }
  } else if(timer_val < 61) {	// 264.8us
    // Start Of Frame (200us)
    if(cur_pin_value == 1 && !reader_started) {
      reader_started = 1;
      reader_byte_counter = 0;
      reader_byte = 0;
      reader_size = 0;
      // change timer overflow to 200us
      OCR1A = 46; // was 46
#ifdef DEBUG
      //printf_P(PSTR("RSOF\r\n"));
#endif
    }
    return;
  }
  
  return;
}

volatile unsigned char packet_data[15];
volatile uint8_t packet_waiting;
volatile uint8_t packet_size;

SIGNAL(SIG_OUTPUT_COMPARE1A) {
  if(reader_started) {
    // indicates End Of Frame OR reader timeout
    if(OCR1A==23040) {
      // was timeout
      printf_P(PSTR("RDTO\r\n"));
      reader_started = 0;
    } else {
      // was end of frame
      // change timer overflow to 100ms
      OCR1A = 23040;

#ifdef DEBUG
      printf_P(PSTR("REOF %d.%d\r\n"), reader_size, reader_byte_counter);
#endif
    

    
      // handle the bytes
      // verify checksum
      unsigned char cksum = crc8buf(reader_data, (reader_size - 1));
      if(cksum != reader_data[reader_size-1]) {
        // checksum error!
#ifdef DEBUG
        printf_P(PSTR("CKRR "));
        hex_print(reader_data, reader_size);
        printf_P(PSTR("\r\n"));
#endif
        return;
      }
      // copy all but checksum byte to the packet buffer
      uint8_t i;
      for(i=0; i<15; i++) {
        packet_data[i] = reader_data[i];
      } 
      packet_size = (reader_size > 0) ? (reader_size - 1) : 0;
      packet_waiting = (reader_size > 0) ? 1 : 0;

#ifdef DEBUG
      printf_P(PSTR("RECV "));
      hex_print(reader_data, reader_size);
      printf_P(PSTR("\r\n"));
#endif

      // stop the reader
      reader_started = 0;
    }
  } else {
    // FIXME: probably nothing required here
  }
}

void reader_init() {
  // For reader:
  // enable pin change interrupt PCINT13
  PCICR |= (1<<PCIE1);
  PCMSK1 |= (1<<PCINT13);
  reader_pin_last = (PINC>>PC5) & 0x01;
  
#ifdef DEBUG
  DDRC |= (1<<PC3);
#endif
  
  // Timer1 setup CLK/64
  OCR1A = 23040;
  TCCR1B |= (1<<CS11) | (1<<CS10);
  // Timer1 enable Output Compare 1 match interrupt
  TIMSK1 |= (1<<OCIE1A);
  // notes:
  //	100,000us = 23040 (CTC timer max)
  //	     64us = 15 (small bit period)
  //	    128us = 29 (large bit period)
  //	    200us = 46 (Start Of Frame period)
  //	    768us = 177 (Break period)
}

void request_ID(uint8_t id) {
  unsigned char msg[5];
  msg[0] = 0x68;
  msg[1] = 0x6a;
  msg[2] = 0xf0;
  msg[3] = 0x01;
  msg[4] = id;

  writer_send(msg, 5);
}

#define READ_TIMEOUT 65535

uint16_t read_ID(uint8_t id){
  uint16_t veh_RPM;

  uint16_t loopy = 0;
  for(loopy=0; loopy<1000; loopy++) {
      delay_us(100);
      if(packet_waiting) {
        // see if it's the RPM return packet
        if(packet_data[0]==0x48 && packet_data[1]==0x6b && packet_data[2]==0x10 && packet_data[3]==0x41 && packet_data[4]==id) {
          // got RPM data
          veh_RPM = packet_data[5];
          veh_RPM = veh_RPM << 8;
          veh_RPM = veh_RPM + packet_data[6];
//          delay_ms(30);
	  printf_P(PSTR("CODE %d = %d\r\n"),id,veh_RPM);
	  return veh_RPM; 
	}
      }
    }
   return READ_TIMEOUT;
}

#define KM_PER_MILE ((float) (5280.0*12.0*0.0254*0.001))

int main() {
  // PC5 input, pull-up
  PORTC |= (1<<PC5);  
  
  // init serial port
  uart_init();
  FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
  stdin = stdout = &uart_stream;

  // init lcd
  lcd_init();
  FILE lcd_stream = FDEV_SETUP_STREAM(lcd_putchar, 0, _FDEV_SETUP_WRITE);
  lcd_home();
  //			 123456789012345678901234
  lcd_write_string(PSTR("NerdKits OBD-II Scanner "));
  lcd_line_two();
  lcd_write_string(PSTR("Waiting..."));

  // start up the VPW stuff
  writer_init();
  reader_init();

  // enable all interrupts
  sei();
  
  uint16_t veh_RPM_raw = READ_TIMEOUT;
  uint16_t veh_speed_raw = READ_TIMEOUT;
  uint16_t veh_throttle_raw = READ_TIMEOUT;
  uint16_t veh_temp_raw = READ_TIMEOUT;

  float veh_RPM;
  float veh_speed;	// mph
  float veh_throttle;	// %
  float veh_temp;	// F

  // get initial speed
  
  while(1) {   
    request_ID(0x0c); // RPM
    if(veh_speed_raw != READ_TIMEOUT) {
      veh_speed = ((float) (veh_speed_raw >> 8)) / KM_PER_MILE;
      lcd_home();
      fprintf_P(&lcd_stream, PSTR("%3.0f mph     "), veh_speed);
    }
    veh_RPM_raw = read_ID(0x0c);	// 4 LSB per RPM

    request_ID(0x11); // throttle position
    if(veh_RPM_raw != READ_TIMEOUT) {
      veh_RPM = ((float) veh_RPM_raw) / 4.0;
      lcd_set_type_command();
      lcd_write_byte(0x80 + 12);	// 13th column on 1st row
      fprintf_P(&lcd_stream, PSTR("%8.2f rpm"), veh_RPM);
    }
    veh_throttle_raw = read_ID(0x11);


    request_ID(0x05); // temp
    if(veh_throttle_raw != READ_TIMEOUT) {
      veh_throttle = ((float) (veh_throttle_raw >> 8)) * 100.0 / 255.0;
      lcd_line_two();
      fprintf_P(&lcd_stream, PSTR("%6.2f%% throttle"), veh_throttle);
    }
    veh_temp_raw = read_ID(0x05); // deg C with offset


    request_ID(0x0d); // speed
    if(veh_temp_raw != READ_TIMEOUT) {
      veh_temp = ((float) (veh_temp_raw >> 8)) - 40;	// celcius
      veh_temp = (veh_temp * 9.0/5.0) + 32.0;	// fahrenheit
      
      lcd_set_type_command();
      lcd_write_byte(0x80 + 0x40 + 16);	// 17th column on 2nd row
      fprintf_P(&lcd_stream, PSTR("   %3.0f%cF"), veh_temp, 0xdf);
    }
    veh_speed_raw = read_ID(0x0d); // kph
  } 

  return 0;
}