#include <SoftwareSerial.h>

#define BUFFER_SIZE 133
uint8_t ecuBufferIn[BUFFER_SIZE];  //Incoming hex from ECU
uint8_t ecuBufferInCounter = 0;
uint8_t tstBufferIn[BUFFER_SIZE];  //Incoming hex from ECU
uint8_t tstBufferInCounter = 0;
uint8_t myAddress = 0xF1;
uint32_t lastKlineResponse = 0;
bool loggerActive = true;
bool debugMode = false;

SoftwareSerial FakeSerial(8, 7); //Rx, Tx

void setup() {
  Serial.begin(115200);
  FakeSerial.begin(115200);
  delay(50);
  randomSeed(analogRead(0));

  TestData();
}

void loop() {
  // put your main code here, to run repeatedly:
  FetchData();
}

uint8_t BitDelay = 40;
long lastBitReceived;

void FetchData() {
  bool logMessage = false;
  while (Serial.available()) {
    if (lastBitReceived > 0) {
      BitDelay = millis() - lastBitReceived;
      BitDelay += 5;
    }
    lastBitReceived = millis();
    ecuBufferIn[ecuBufferInCounter++] = Serial.read();

    //Init sometimes start with zeros (from fast init?!)
    if (ecuBufferInCounter == 1 && ecuBufferIn[0] == 0x00) {
      ecuBufferInCounter--;
      break;
    }
    //Buffer exeeded
    if (ecuBufferInCounter >= BUFFER_SIZE) {
      Serial.println("Buffer exeeded!");
      logMessage = true;
      break;
    }
    //Message length and checksum complete received (known protocol):
    if (MessageComplete()) {
      logMessage = true;
      break;
    }
  }

  //Time between bits went over threshold. Probably new message
  if (lastBitReceived > 0 && BitDelay < millis() - lastBitReceived) {
    Serial.println("Timeout! (" + String(BitDelay) + ")");
    lastBitReceived = 0;
    //BitDelay = 40;
    BitDelay += 10;
    logMessage = true;
  }

  if (!logMessage || ecuBufferInCounter < 1)
    return;
  if (loggerActive)
    LogPermanent(MessageDirectionReceived() ? "<" : ">", ecuBufferIn, 0, ecuBufferInCounter, true);

  ClearKlineBuffer();
  lastKlineResponse = millis();
  lastBitReceived = 0;
  //BitDelay = 40;
  BitDelay += 10;
}

bool MessageDirectionReceived(){
  if(ecuBufferInCounter < 1)
    return false;

  switch(ecuBufferIn[0]){
    case 0x48:
    case 0x49:
    case 0x68:
    case 0x69://CARB
      if(ecuBufferInCounter > 2 && (ecuBufferIn[2] == myAddress))
        return true;
      break;
    case 0x02:
      return true;
    case 0x72:
      return false;
    default:
      //Tester F0 / F1
      if(ecuBufferInCounter > 1 && (ecuBufferIn[1] == myAddress || ecuBufferIn[1] == myAddress -1))
        return true;
      if(ecuBufferInCounter > 1 && (ecuBufferIn[2] == myAddress || ecuBufferIn[2] == myAddress -1))
        return false;
  }
  return false; 
}

bool MessageComplete() {
  uint8_t expectedLength = 0;
  switch(ecuBufferIn[0]){
    case 0x01:
    case 0x71:
      if (ecuBufferInCounter >= 1)
        expectedLength = ecuBufferIn[3] + 4;
      break;
    case 0x02: //Honda
    case 0x72:
    case 0xFE:
      if (ecuBufferInCounter >= 1)
        expectedLength = ecuBufferIn[1];
      break;
    case 0x10:
      if (ecuBufferInCounter >= 3)
        expectedLength = ecuBufferIn[2] + 4;
      break;
    default: //0x80, 0x81, 0xC0,1,2,X
      if (bitRead(ecuBufferIn[0], 7)) {
        if (ecuBufferInCounter < 4)
          return false;
        bool functional = bitRead(ecuBufferIn[0], 6);
        //Serial.println("With Adress" + String(functional ? " functional" : " physical"));
        if (functional)
          expectedLength = 4;
        else
          expectedLength = 4;
        uint8_t len = ecuBufferIn[0] << 2;
        len = len >> 2;
        if (len == 0) //No length in Format: Is included in data
          expectedLength += ecuBufferIn[3] + 1;//Add length itself
        else
          expectedLength += len;
        //CARB base on timing. No length provided
        //Serial.println("ExpectedLength: " + String(expectedLength));
      }
  }
    
  if (expectedLength > 0) {
    if (expectedLength == ecuBufferInCounter && debugMode)
      Serial.println("complete");
    return expectedLength == ecuBufferInCounter;
  }
  if (debugMode)
    Serial.println("incomplete");
  return false;
}