//--------------------------------------------------------------------------------
String vinRead = "014 0: 49 02 01 4D 50 42 1: 41 4D 46 30 36 30 4E 2: 58 34 34 39 33 31 34";
//convert ascii code to Charactor
char asciiTable[]= " !\"#$%&'()*+,-./0123456789:;<=>?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_`abcdefghijklmnopqrstuvwxyz{|}`";
char HextoChar(uint8_t asciiCode) {
  if ((asciiCode > 0x20) && (asciiCode < 0x80)) {
    return(asciiTable[asciiCode-32]);
  } 
  return '/0';
}
//--------------------------------------------------------------------------------
//function get VIN 
String getVIN(String elm_rsp) {
  String VIN = "";
  uint8_t byteCount = 0;
  elm_rsp.trim();
  while (elm_rsp.length() > 0) {//keep reading each char
    int index = elm_rsp.indexOf(' ');//check space
    String getByte =  elm_rsp.substring(0, index);//get first byte
    if (index == -1)  {//no space found
        byte ascii = strtol(getByte.c_str(), NULL, 16);  //byte 3 save to A
        VIN.concat(HextoChar(ascii));//last char
        //check correct VIN
        if (VIN.length() == byteCount) {
          VIN = VIN.substring(3,-1);
          return(VIN);//return VIN number
        } else {
          return("Cannot read VIN");
        }

    } else {//found space
      if (getByte.indexOf(':') == -1 ) {
        if (getByte.length() >= 3) {
          byteCount = strtol(getByte.c_str(), NULL, 16);//get byte count
        //  Serial.println(byteCount);
        } else {//skip ':' byte
          byte ascii = strtol(getByte.c_str(), NULL, 16);  //byte 3 save to A
          VIN.concat(HextoChar(ascii));
        }
      } 
      elm_rsp = elm_rsp.substring(index + 1);//copy the rest behind space to elm_rsp.
    }//else
  }  //while
  return("Cannot read VIN");
}//getVIN


//--------------------------------------------------------------------------------
String dtcRead = "00E 0: 43 06 00 7D C6 93 1: 01 08 C6 0F 02 E9 02 2: E0 CC CC CC CC CC CC 43 01 C4 01";   
//DTC prefex code mapping
const String dtcMap[16] = {"P0","P1","P2","P3","C0","C1","C2","C3","B0","B1","B2","B3","U0","U1","U2","U3"};
uint8_t error_cnt = 0;
uint8_t no_of_dtc = 0;
//--------------------------------------------------------------------------------
void getDTC(String elm_rsp) {
  // put your setup code here, to run once:
  // Clean and tokenize input
  dtcRead.trim();
  dtcRead.replace("\r", " ");
  dtcRead.replace("\n", " ");
  dtcRead.replace(":", "");

  String tokens[128];
  int tokenCount = 0;

  while (dtcRead.length() > 0 && tokenCount < 128) {
    int index = dtcRead.indexOf(' ');
    if (index == -1) index = dtcRead.length();
    String token = dtcRead.substring(0, index);
    dtcRead = dtcRead.substring(index + 1);
    token.trim();

    // Skip labels like "0", "1", "2"
    if (token.length() == 1 && isDigit(token[0])) continue;

    if (token.length() > 0) {
      tokens[tokenCount++] = token;
    }
  }

  // Parse DTCs
  String dtcList[16];
  byte no_of_dtc = 0;

  for (int i = 0; i < tokenCount - 2; i++) {
    if (tokens[i] == "43") {
      int numDTC = strtol(tokens[i + 1].c_str(), NULL, 16);
      i += 2;

      for (int j = 0; j < numDTC && i + 1 < tokenCount; j++) {
        uint8_t b1 = strtol(tokens[i++].c_str(), NULL, 16);
        uint8_t b2 = strtol(tokens[i++].c_str(), NULL, 16);

        uint8_t firstNibble = (b1 & 0xF0) >> 4;
        uint8_t secondNibble = (b1 & 0x0F);

        String dtcCode = dtcMap[firstNibble];
        dtcCode += String(secondNibble, HEX);
        if (secondNibble < 0x10 && secondNibble < 0xA) dtcCode += "";

        if (b2 < 0x10) dtcCode += "0";
        dtcCode += String(b2, HEX);

        dtcCode.toUpperCase();
        dtcList[no_of_dtc++] = dtcCode;
      }
    }
  }
  Serial.println();            
  String txt = "MIL is ON - No of DTCs = "+String(no_of_dtc);
  Serial.println(txt);    
  for (int i =0; i < no_of_dtc;i++) {
    Serial.print(dtcList[i]+", ");
  }          
}     
//--------------------------------------------------------------------------------
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(100);
  Serial.println("-------------");
  Serial.println("GET DTC");
  Serial.println(dtcRead);
  getDTC(dtcRead);
    Serial.println(); 
  Serial.println("-------------");
  Serial.println("GET VIN");
  Serial.println(vinRead);
  Serial.print("VIN: ");
  Serial.println(getVIN(vinRead));
}

void loop() {
  delay(10);
}