/* BMW KWP & Zeitronix serial data logger project.
by Carl Farrington carlfarrington@gmail.com
Not finished yet

The Zeitronix bit uses a MAX3232.
The K-Line bit uses an ST L9637 with just the 510Ohm reistor. Line echos are received and discarded from the input buffer.
The code originally disabled UART RX side, and enabled again after TX, but this was found to be less reliable than simply allowing
the serial input buffer to capture the echo, then read/discard it.

A switch can be connected between A14 and VCC (pull up switch) to set debug=1 while the code is running. This will output lots
more data to the serial console, including lots of dots (.) showing where other code/functions could be running, i.e. free time.
*/



#include <Arduino.h>
#include <SPI.h>
#include "SdFat.h"
#include <Wire.h>
#include "wiring_private.h"
#include <sam.h>

#define K_OUT 1 // K Output Line - TX on Arduino
#define K_IN 0 // K Input Line - RX on Arduino
#define PARTIALLY_RECEIVED 2
#define REQUEST_SENT 1
#define CLEAR 0
#define InterByteDelay_NORMAL 0 // spec says 5ms, but I seem to be ok with 0, and this is blocking during the request phase, before accesstiming is changed to the FASTMODE values.
#define InterByteDelay_FASTMODE 0 // we switch to these values in the code loop after the AccessTimingParameter request is accepted. Tweak these values..
#define kwp_wait_time_NORMAL 49
#define kwp_wait_time_FASTMODE 10 // we switch to these values in the code loop if our AccessTimingParameter request is accepted. Tweak these values..


uint8_t ECUAddr = 0x12;
uint8_t myAddr = 0xF1;
uint8_t ECUConnected = 0;
uint8_t GotPacket = 0;
unsigned long kwp_time_last_sent_or_received = 0;
unsigned long startKWP_receiveTime = 0;
unsigned long kwp_partial_part_a_time = 0; // for measuring blocking program time used during split/partial receive functions.. i.e. program time used/blocking time.
unsigned long kwp_partial_part_b_time = 0;
unsigned long kwp_last_dump = 0;
unsigned long startTime = millis(); // start of the program, and then start of the main ecu connect loop
char kwpdata_in[255]; // where we will store the data part of the latest packet. This may be partially filled sometimes, so we don't log this array directly.
uint8_t accesstimingdone = 0; // have we set the accesstiming to the extremes allowed?
uint8_t kwp_status = CLEAR; // CLEAR, REQUEST_SENT, OR PARTIALLY_RECEIVED (see kwp_size_to_split below)
uint8_t minimum_kwp_response_size = 5; // wait 'til this many bytes in incoming buffer, full packet including fmt,tgt,src,data,chksum.
uint8_t kwp_timeout = 150; // timeout at which we say ECU not connected and retry FastInit. This is non-blocking.
uint8_t tester_interByte_delay = InterByteDelay_NORMAL; // specs says 5ms between each byte, at least until changed via AccessTimingParameter
uint8_t kwp_wait_time = kwp_wait_time_NORMAL;// time after sending request that we wait to try to get response. also wait to send next request.
uint8_t kwp_wt_modifier = 0; // used to modify wait time here and there. We are adding it.
uint16_t FastInitRetryTimer = 5000; // frequency with which we retry FastInit. 
uint8_t debug = 0; // pull up pin 14 (A0 on Feather M0 PCB) if you like more info on the serial terminal. That will set debug=1
uint8_t kwp_size_to_split = 3; // if the data size is bigger than this, do a partial receive, let serial buffer collect the remainder (up to max of serial buffer size)
int echoBytes = 0; // track bytes sent over K-Line, so we can read back (and ignore) our echo. Let the buffer capture it though to save program time.
int kwp_data_to_buffer; //this says how many bytes we want to buffer to Serial.available(), on bigger responses. It's calculated from the data length that is in the packet header bytes.
int requestId = 1; //used to store the request number that's been sent, when sending multiple alternating requests
int FastInitStage = 0;  // This stores the state of the fastinit function
int gotZtPacket = 0; // finally incorporated the Zeitronix code into this KWP code. seems to co-exist quite well.
char ZtPacket[11]; // finally incorporated the Zeitronix code into this KWP code. seems to co-exist quite well.
char ZtDataString[80]; // string to output to serial
char logline[512] = {0x0};


struct kwpPacket {
	int fmt; // store in a 16 bit int, so that we can get 'valid data 0 - 255', and 'no data =  -1 from serial.read'
	uint8_t target;
	uint8_t source;
  uint8_t length;
	char *data;
	uint8_t checksum;
};

kwpPacket packet;

Uart Serial2 (&sercom1, 11, 10, SERCOM_RX_PAD_0, UART_TX_PAD_2);
 
void SERCOM1_Handler()
{
  Serial2.IrqHandler();
}

void setup() {
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);
  pinMode(14, INPUT_PULLDOWN);
    // Open serial communications and wait for port to open:
  Serial.begin(115200);
  Serial1.begin(10400);
  Serial2.begin(9600);
  pinPeripheral(10, PIO_SERCOM);
  pinPeripheral(11, PIO_SERCOM);
  Serial1.setTimeout(80);
  while (!Serial) {
    ; /* wait for serial port to connect. Needed for native USB port only.
      This does mean the code doesn't start to execute until we connect to the console. That's handy while coding
      */
  }
}

void getZeitronixPacket(char* packetpointer){
gotZtPacket = 0;
char buffer[32] = {0xF}; // buffer 32 bytes to make sure we find the 3 byte start sequence.
  for (int i=0; i<32; i++){
    Serial2.readBytes(&buffer[i],1);
    if (i > 1 && buffer[i] == 2 && buffer[i-1] == 1 && buffer[i-2] == 0) {
          gotZtPacket = 1;
          Serial2.readBytes(packetpointer,11);
          return;
    }
  }
}

void clearSerial1input(){
    while(Serial1.available()){
      Serial1.read();
    }
}

kwpPacket getResponse(){
  kwp_wt_modifier = 0;
  packet.fmt = -1;
  packet.data = kwpdata_in;

  startKWP_receiveTime = millis();

if(debug) { 
  Serial.println();
  Serial.println("Getting fmt");
  }

  packet.fmt = Serial1.read();

if(debug) { Serial.print("waited ");
  Serial.print(millis() - startKWP_receiveTime);
  Serial.println("ms to get fmt byte");
  }

if (packet.fmt < 0) {
  Serial.print("Didn't get valid Fmt byte\r\nGot: ");
  Serial.println(packet.fmt);
  packet.fmt = 0;
  GotPacket = 0;
  ECUConnected = 0;
  kwp_status = CLEAR;
  return(packet);
  }

if(debug){
  Serial.print("Did get it. It is: ");
  Serial.println(packet.fmt,HEX);
  }
  /* remember length does not include fmt or address bytes or checksum.
    checksum is all bytes except checksum itself.
    */

if (packet.fmt == 0x0) {
  if(debug){ Serial.println("Got Fmt 0x0 and processing"); }
	// no address bytes, but there is a length byte & checksum.
  Serial1.readBytes(&packet.length,1);// grab the length byte
	Serial1.readBytes(packet.data,packet.length); // grab data 
  Serial1.readBytes(&packet.checksum,1); // grab checksum
  kwp_status = CLEAR;
  GotPacket = 1;
  kwp_time_last_sent_or_received = millis();
	return(packet);
}

else if (packet.fmt >= 0x01 && packet.fmt <= 0x3F) {
    if(debug){ Serial.println("Got Fmt  >= 0x01 && <= 0x3F and processing");}
	// no address bytes, and we have the length here in the fmt byte.
	packet.length = packet.fmt;
	Serial1.readBytes(packet.data,packet.length);
  Serial1.readBytes(&packet.checksum,1);
  kwp_status = CLEAR;
  GotPacket = 1;
  kwp_time_last_sent_or_received = millis();
	return(packet);
}

else if (packet.fmt == 0xC0 || packet.fmt == 0x80) {
  if(debug){
    Serial.println("Got Fmt 0xC0 or 0x80 and processing");
    }
	
  // address bytes present and optional length byte present.
  Serial1.readBytes(&packet.target,1);
  Serial1.readBytes(&packet.source,1);
  Serial1.readBytes(&packet.length,1);
  // we should add in the partial_receive stuff like we've done for the 0x3F packet format.
	Serial1.readBytes(packet.data, packet.length);
  Serial1.readBytes(&packet.checksum,1);
  kwp_status = CLEAR;
  GotPacket = 1;
  kwp_time_last_sent_or_received = millis();
	return(packet);
}

else if (packet.fmt & 0x3F) { //bitmask of 00111111 AND Fmt.. will return the lower 6 bits i.e. length
  if(debug){
    Serial.println("Address bytes present, length included in Fmt.");
    }
	packet.length = packet.fmt & 0x3F;
  Serial1.readBytes(&packet.target,1);
  Serial1.readBytes(&packet.source,1);

  if (packet.length > kwp_size_to_split) { // we'll collect the data later and let the serial input buffer fill up first.
    if (packet.length >= 63) {kwp_data_to_buffer = 64;} // if expecting 63 or more, plus checksum, then buffer as much as we can
    else {
      kwp_data_to_buffer = packet.length;
      if(kwp_data_to_buffer < 10) {
        kwp_wt_modifier = 12; // on smaller packets, we are going too fast & getting dropouts.  workaround.
      }
    }
    if(debug) {
        Serial.println();
        Serial.print("Doing Partial Receive. kwp_data_to_buffer:");
        kwp_partial_part_a_time = millis() - startKWP_receiveTime;
        Serial.println(kwp_data_to_buffer);
    }
    kwp_status = PARTIALLY_RECEIVED;
    GotPacket = 0;
    return(packet);
  }
	Serial1.readBytes(packet.data, packet.length);
  Serial1.readBytes(&packet.checksum,1);
  if(debug){
    Serial.print("\r\ntook ");
    Serial.print(millis() - startKWP_receiveTime);
    Serial.println("ms of program time to get total packet");
    }
  kwp_status = CLEAR;
  GotPacket = 1;
  kwp_time_last_sent_or_received = millis();
	return(packet);
  }
  kwp_status = CLEAR;
  return(packet);
}


void getRemainingKWPdata(uint8_t length, uint8_t *checksum){
  kwp_partial_part_b_time = millis();
  Serial1.readBytes(kwpdata_in, length);
  Serial1.readBytes(checksum,1);
  if(debug){
    Serial.print("\r\ntook ");
    kwp_partial_part_b_time = (millis() - kwp_partial_part_b_time) + kwp_partial_part_a_time;
    Serial.print(kwp_partial_part_b_time);
    Serial.println("ms of program time to get total packet using partial receive/split function");
    }
  kwp_status = CLEAR;
  GotPacket = 1;
  kwp_time_last_sent_or_received = millis();
  return;
}

void SendKWPPacket(char *data, uint8_t datalength){

  // SERCOM0->USART.CTRLB.bit.RXEN = 0;
  echoBytes = 0;
	uint8_t fmt;
	uint8_t target = ECUAddr;
	uint8_t source = myAddr;
  uint8_t length;
	uint8_t checksum;

  if (datalength > 63) {
    fmt = 0x80;
    length = datalength;
  }
  else {
    fmt = 0x80 + datalength;
    length = 0;
  }

  if(debug) {
    Serial.println("\r\n\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%");
    Serial.print("Sending:");
    Serial.print(fmt,HEX);
    Serial.print(target,HEX);
    Serial.print(source,HEX);
    if (length > 0) {
      Serial.print(length,HEX);
      }
    checksum = fmt + target + source + length;
    for (int i = 0; i < datalength; i++){
      Serial.print(data[i],HEX);
      checksum += data[i];
      }
    Serial.println(checksum,HEX);
  }
  
  Serial1.write(fmt);
  ++echoBytes;
  delay(tester_interByte_delay);
  Serial1.write(target);
  ++echoBytes;
  delay(tester_interByte_delay);
  Serial1.write(source);
  ++echoBytes;
  delay(tester_interByte_delay);

  if (length > 0) {
    Serial1.write(length);
    ++echoBytes;
    delay(tester_interByte_delay);
  }
  checksum = fmt + target + source + length;
  for (int i = 0; i < datalength; i++){
    Serial1.write(data[i]);
    ++echoBytes; // change to echoBytes += datalength, and put it outside this for loop.
    checksum += data[i];
    delay(tester_interByte_delay);
  }
  Serial1.write(checksum);
  ++echoBytes;
  kwp_time_last_sent_or_received = millis();
  kwp_status = REQUEST_SENT;
}

void dumpKWP(kwpPacket packet){
    if (debug) {
      Serial.println();
      Serial.print("kwp_wait_time:");
      Serial.println(kwp_wait_time);
      Serial.print("Fmt:");
      Serial.print(packet.fmt,HEX);
      Serial.print(" target: ");
      Serial.print(packet.target,HEX);
      Serial.print(" source: ");
      Serial.print(packet.source,HEX);
      Serial.print(" length: ");
      Serial.println(packet.length,HEX);
      Serial.print("("); Serial.print(millis() - kwp_last_dump); Serial.print("ms since last packet) "); // keep track of frequency of receiving, for timing tweaks
      kwp_last_dump = millis(); // keep track of frequency of receiving, for timing tweaks
      Serial.print(millis());
      Serial.print(" Data: ");
    }
      uint8_t checksum = packet.fmt + packet.target + packet.source;
      if ((packet.fmt & 0x3F) == 0 ) {
        /* bitmask of 00111111 AND Fmt, to see if length was in separate byte
        (lower 6 bits of fmt were zero) and thus whether to add length byte to the checksum)
        kind of necessary because we are creating a packet.length byte even if it wasn't  there
        as a separate byte in the datagram to begin with */
        checksum+= packet.length;
      }
      Serial.print(millis());
      Serial.print(":");
      for (int i = 0; i < packet.length; i++){
        char byte[1];
        sprintf(byte,"%02x,",packet.data[i]);
        Serial.print(byte);
        checksum +=packet.data[i];
      }
    if (debug) {
      Serial.print(" Checksum: ");
      Serial.print(packet.checksum,HEX);
    }
      if (packet.checksum != checksum) { 
        Serial.print(" !!!ChkSum Mismatch!!! : ");
        Serial.print(" Calculated checksum: ");
        Serial.println(checksum,HEX);
        clearSerial1input();
        }
}

void setAccessTiming(uint8_t interByte_delay_value, uint8_t kwp_wait_time_value){
  tester_interByte_delay = interByte_delay_value;
  kwp_wait_time = kwp_wait_time_value;
  Serial.print("\r\nkwp_wait_time set to ");
  Serial.println(kwp_wait_time);
  Serial.print("tester_interByte_delay set to ");
  Serial.println(tester_interByte_delay);
  accesstimingdone = 1;
}

void setDefaultAccesstiming(){
  tester_interByte_delay = InterByteDelay_NORMAL;
  kwp_wait_time = kwp_wait_time_NORMAL;
  accesstimingdone = 0;
  Serial.print("\r\nResetting Access Timing to default interByte delay and ");
  Serial.print(kwp_wait_time_NORMAL);
  Serial.println("ms wait time\r\n");
}

void doFastInit() {
   if ((startTime + FastInitRetryTimer < millis()) || (startTime < 300)) { // no delay on first run
    digitalWrite(13, HIGH);
    startTime = millis();
    Serial.println();
    Serial.println("Connecting to ECU");
    clearSerial1input();
    Serial.println("\r\nTrying Wakeup (FastInit)\r\nSending init pulse, drop TX low for 25ms");
    pinPeripheral(K_OUT, PIO_OUTPUT); // need to drive tx pin manually for a moment..
    digitalWrite(K_OUT, LOW);
    FastInitStage = 1;
    }

    if (millis() == startTime + 25 && FastInitStage == 1) { // 
    digitalWrite(K_OUT, HIGH);
    Serial.println("Going high again");
    FastInitStage = 2;
    }

    if (millis() == startTime + 49 && FastInitStage == 2) {
      
      Serial.println("Sending StartCommunications (0x81) request"); 
      Serial1.begin(10400); // Re-initialise uart after all that malarky so that we can transmit
      Serial1.read(); // there's a spurious "0" on the line from the high/low FastInit toggle.. lose it!
      char request[] = {0x81};
      SendKWPPacket(request, 1);
      FastInitStage = 3;
    }

    if (FastInitStage == 3 && Serial1.available() >= minimum_kwp_response_size && echoBytes == 0) {
      Serial.println("Processing received data.."); 
      kwpPacket response = getResponse();
      if (GotPacket) {
        if (response.data[0] == 0xC1) {
        ECUConnected = 1;
        Serial.print("ECU Connected!\r\nkwp_wait_time:");
        Serial.println(kwp_wait_time);
        }
        else {
        Serial.println("Not good response");
        dumpKWP(response);
        ECUConnected = 0;
        FastInitStage = 0;
        }
      }
      else {
        GotPacket = 0;
        FastInitStage = 0;
      }
    }
}

void doAccessTimingChange(){
  if (kwp_status == CLEAR && (kwp_time_last_sent_or_received + kwp_wait_time < millis())) {
    if(debug) { 
       Serial.println("\r\nDoing Access Timing"); 
    }
    char request[] = {0x83, 0x03, 0x00, 0x02, 0x00, 0x3b, 0x00}; //set timing to what my ecu says are the limits..
      // char request[] = {0x83, 0x00}; // read limits.. we should do this automagically
        SendKWPPacket(request, 7);
    }
    if (kwp_status == REQUEST_SENT && (Serial1.available() >= minimum_kwp_response_size) && echoBytes == 0 && (kwp_time_last_sent_or_received + kwp_wait_time < millis())) {
      kwpPacket response = getResponse();
      if (response.data[0] == 0xC3) { // postive acceptance = first byte +0x40
        if(debug) { 
          Serial.println("\r\nTiming change request accepted");
        }
        setAccessTiming(InterByteDelay_FASTMODE,kwp_wait_time_FASTMODE); // alter our interbyte delay and also how frequently we send or receive (kwp_wait_time). Don't set it too low or there'll be less free time for other functions. Also the ECU doesn't like requests too soon after its replied.
        digitalWrite(13, LOW); // turn the red  LED off so we know all is well.
      }
    }
}

void loop() {

  while ((echoBytes) && (Serial1.available() == echoBytes)) {
    // clear our transmit echo from the rx line
    Serial1.read();
    --echoBytes;
  }

  debug = digitalRead(14);

  if (!ECUConnected) {
    doFastInit();
  }

/* Our kwp_wait_time is really the wait time between request & response (or response and next request.)
   If the response is big, it's not ideal to go and try to collect the response before the serial
   input buffer is full. a 63 byte response (e.g. for request $22 $40 $00) takes about 52ms to receive and process,
   if we try to grab it as soon as the first few bytes come in.
   We could be doing non KWP stuff in that time. (Zeitronix, or log_to_sd, etc.)
   A small KWP response (e.g. $22 $40 $04) requires only 2ms or so to receive and process, as we are already
   waiting until there's 5 bytes in the buffer before starting anything.

   So, we use a partial receive method if the data payload is above a certain length.
   The threshold for this is set in the variables up top. We have to receive the length information before
   we can make the decision though.
   
   This means instead of spending 50 - 60ms of blocking serial.readBytes program time to receive a 63 byte payload,
   it only takes 2ms of program time.
   Which leaves more cycles for other things. 
   */

  if (ECUConnected) {
    // the AccessTiming function doesn't have the code to cope with PARTIALLY_RECEIVED.
    // so don't reduce the partial threshold too much.
    if (!accesstimingdone) {
      doAccessTimingChange();
    }
    else { // This is the main request-response loop that runs after ecu connects, and after access timing is altered for high speed.
    if (kwp_status == REQUEST_SENT && Serial1.available() >= minimum_kwp_response_size && !echoBytes) {
       packet = getResponse();
       if(GotPacket){
         // there's an issue where 0s are returned on the serial input, even with nothing connected. Seems to be from a floating line on the breadboard.
        // so watch out for that if you see a lot of "Not good response" output.
        dumpKWP(packet);
        Serial.println();
        }
      }

    if (kwp_status == PARTIALLY_RECEIVED && Serial1.available() >= kwp_data_to_buffer && !echoBytes){
        getRemainingKWPdata(packet.length, &packet.checksum);
        if(GotPacket){
          dumpKWP(packet);
          Serial.println();
        }
      }
    if (kwp_time_last_sent_or_received + kwp_timeout < millis()) {
        if(debug) {
          Serial.print("\r\nStatus was ");
          Serial.print(kwp_status);
          Serial.print(" but timed out. Serial1.available() was ");
          Serial.println(Serial1.available());
          Serial.print("Time between last sent and now is ");
          Serial.print(millis() - kwp_time_last_sent_or_received);
          Serial.println("ms");
        }
        ECUConnected = 0;
        FastInitStage = 0;
        setDefaultAccesstiming();
        digitalWrite(13, HIGH);
        startTime = millis(); // make sure we wait before trying fastinit again.
        return;
      }

    if (kwp_status == CLEAR && (kwp_time_last_sent_or_received + kwp_wait_time + kwp_wt_modifier < millis())) {
      if (requestId == 1) {
        char request[] = {0x22, 0x40, 0x00};
        ++requestId; // for the next time around..
        SendKWPPacket(request, 3);
        }
      else {
        char request[] = {0x22, 0x40, 0x04};
        requestId = 1; // next time go back and send first request
        SendKWPPacket(request, 3);
      }
    }
    }
}


if (Serial2.available() > 32){ // wait til there's a nice chunk in the serial buffer
  getZeitronixPacket(ZtPacket);
  if (gotZtPacket == 1) {
    sprintf(ZtDataString, "\r\n%lu:%u,%u,%u,%u,%u,%u,%u,%u,%u,%u,%u\n", millis(),ZtPacket[0],ZtPacket[1],ZtPacket[2],ZtPacket[3],ZtPacket[4],ZtPacket[5],ZtPacket[6],ZtPacket[7],ZtPacket[8],ZtPacket[9],ZtPacket[10]);
    Serial.println(ZtDataString);
  }
}
 if(debug) { Serial.print("."); } // proper free time 
}

//OK the way we're going to work this log file format, is that we'll have a 'logline' array with various decoded parameters in it.
//we'll just update those parameters as quickly as we get new data for them, and log this to sd at 50Hz.
// So the log line will be kind of lying about some values, or rather they will be 'last read' values.
//Other people using this code can do their own alterations to that.