// Serielle Schnittstelle TCRT5000 zum ISKRA-MT681
#define RXD2 21                         // TCRT5000 (D0)
#define TXD2 13                         // ohne belegung
#define Baudrate 9600                   // vorgabe vom Stromzähler
#define Protocol SERIAL_8N1             // vorgabe vom Stromzähler
// Serielle Schnittstelle ESP32 zu PC
#define Baudrate_USB 115200             // 

unsigned long inByteIndex = 0;
unsigned long UndefiniertesEnde = 300;          // Pausenzeit eingeben
unsigned long myTime;
bool is_parsing = false;
int start_counter=0;
int start_2_counter=0;
bool sml_start=false;
byte inByte;

//#define DEBUG 1
#ifdef DEBUG
#define printdbg(x) Serial.print(x)
#define printdbgn(x) Serial.println(x)
#else
#define printdbg(x)
#define printdbgn(x)
#endif

const int maxBufferSize = 800;               // Maximale Größe des char-Arrays
byte sml[maxBufferSize];   

// verwendet Kanal 1
const byte sml_message[520] = {0x1B, 0x1B, 0x1B, 0x1B, 0x01, 0x01, 0x01, 0x01, 0x76, 0x05, 0xF2, 0x01, 0x32,
                       0x19, 0x62, 0x00, 0x62, 0x00, 0x72, 0x65, 0x00, 0x00, 0x01, 0x01, 0x76, 0x01,
                       0x01, 0x07, 0x45, 0x53, 0x59, 0x51, 0x33, 0x43, 0x0B, 0x06, 0x45, 0x53, 0x59,
                       0x01, 0x07, 0x1A, 0x33, 0xD0, 0xBA, 0x01, 0x01, 0x63, 0xFF, 0xF2, 0x00, 0x76,
                       0x05, 0xF2, 0x01, 0x32, 0x1A, 0x62, 0x00, 0x62, 0x00, 0x72, 0x65, 0x00, 0x00,
                       0x07, 0x01, 0x77, 0x01, 0x0B, 0x06, 0x45, 0x53, 0x59, 0x01, 0x07, 0x1A, 0x33,
                       0xD0, 0xBA, 0x01, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x7F, 0x77,
                       0x07, 0x81, 0x81, 0xC7, 0x82, 0x03, 0xFF, 0x01, 0x01, 0x01, 0x01, 0x04, 0x45,
                       0x53, 0x59, 0x01, 0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x00, 0xFF, 0x63, 0x01,
                       0x90, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0xFC,
                       0x69, 0x00, 0x00, 0x00, 0x67, 0x3A, 0x9C, 0x99, 0xB9, 0x01, 0x77, 0x07, 0x01,
                       0x01, 0x01, 0x08, 0x01, 0xFF, 0x63, 0x01, 0x90, 0x72, 0x62, 0x01, 0x65, 0x13,
                       0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01, 0x65, 0x00, 0x43, 0xA3, 0xEC, 0x01,
                       0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x02, 0xFF, 0x63, 0x01, 0x90, 0x72, 0x62,
                       0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01, 0x65, 0x00, 0x00,
                       0x00, 0x6E, 0x01, 0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x03, 0xFF, 0x63, 0x01,
                       0x90, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01,
                       0x65, 0x00, 0x00, 0x00, 0x6F, 0x01, 0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x04,
                       0xFF, 0x63, 0x01, 0x90, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62,
                       0x1E, 0x52, 0x01, 0x65, 0x00, 0x00, 0x00, 0x6E, 0x01, 0x77, 0x07, 0x01, 0x01,
                       0x01, 0x08, 0x05, 0xFF, 0x63, 0x01, 0x90, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14,
                       0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01, 0x65, 0x00, 0x00, 0x00, 0x6F, 0x01, 0x77,
                       0x07, 0x01, 0x01, 0x01, 0x08, 0x06, 0xFF, 0x63, 0x01, 0x90, 0x72, 0x62, 0x01,
                       0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01, 0x65, 0x00, 0x00, 0x00,
                       0x6E, 0x01, 0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x07, 0xFF, 0x63, 0x01, 0x90,
                       0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E, 0x52, 0x01, 0x65,
                       0x00, 0x00, 0x00, 0x6F, 0x01, 0x77, 0x07, 0x01, 0x01, 0x01, 0x08, 0x08, 0xFF,
                       0x63, 0x01, 0x90, 0x72, 0x62, 0x01, 0x65, 0x13, 0x14, 0xEE, 0x6C, 0x62, 0x1E,
                       0x52, 0x01, 0x65, 0x00, 0x00, 0x00, 0x6E, 0x01, 0x77, 0x07, 0x01, 0x00, 0x01,
                       0x07, 0x00, 0xFF, 0x01, 0x01, 0x62, 0x1B, 0x52, 0xFE, 0x55, 0x00, 0x01, 0x5F,
                       0x3B, 0x01, 0x77, 0x07, 0x01, 0x00, 0x15, 0x07, 0x00, 0xFF, 0x01, 0x01, 0x62,
                       0x1B, 0x52, 0xFE, 0x55, 0x00, 0x00, 0xF3, 0x3E, 0x01, 0x77, 0x07, 0x01, 0x00,
                       0x29, 0x07, 0x00, 0xFF, 0x01, 0x01, 0x62, 0x1B, 0x52, 0xFE, 0x55, 0x00, 0x00,
                       0x54, 0x93, 0x01, 0x77, 0x07, 0x01, 0x00, 0x3D, 0x07, 0x00, 0xFF, 0x01, 0x01,
                       0x62, 0x1B, 0x52, 0xFE, 0x55, 0x00, 0x00, 0x17, 0x6A, 0x01, 0x77, 0x07, 0x01,
                       0x00, 0x60, 0x05, 0x05, 0xFF, 0x01, 0x01, 0x01, 0x01, 0x63, 0x01, 0x90, 0x01,
                       0x01, 0x01, 0x63, 0xB3, 0x92, 0x00, 0x76, 0x05, 0xF2, 0x01, 0x32, 0x1B, 0x62,
                       0x00, 0x62, 0x00, 0x72, 0x65, 0x00, 0x00, 0x02, 0x01, 0x71, 0x01, 0x63, 0x03,
                       0xCD, 0x00, 0x1B, 0x1B, 0x1B, 0x1B, 0x1A, 0x00, 0x75, 0xD8
                      };


// OBIS Codes aud die abgefragt werden soll 
byte sml_Kopf[] = {0x1b, 0x1b, 0x1b, 0x1b, 0x01, 0x01, 0x01, 0x01}; // Startsequenz

// OBIS Code 1.8.0
//byte code1[] = {0x01, 0x00, 0x01, 0x08, 0x00, 0xff};  // Kanal 0
byte code1[] = {0x01, 0x01, 0x01, 0x08, 0x00, 0xff};    // Kanal 1

// OBIS CODE 1.8.1
//byte code2[] = { 0x01, 0x00, 0x01, 0x08, 0x01, 0xff};
byte code2[] = { 0x01, 0x01, 0x01, 0x08, 0x01, 0xff};
// OBIS CODE 1.8.2
//byte code3[] = { 0x01, 0x00, 0x01, 0x08, 0x02, 0xff};
byte code3[] = { 0x01, 0x01, 0x01, 0x08, 0x02, 0xff};

// OBIS CODE 2.8.0
byte code4[] = { 0x01, 0x00, 0x02, 0x08, 0x00, 0xff};
// OBIS CODE 2.8.1
byte code5[] = { 0x01, 0x00, 0x02, 0x08, 0x01, 0xff};
// OBIS CODE 2.8.2
byte code6[] = { 0x01, 0x00, 0x02, 0x08, 0x02, 0xff};
// OBIS Code 10.7.0
// byte code7[] = { 0x01, 0x00, 0x10, 0x07, 0x00, 0xff};
// Hersteller kennung
byte H_ken[] = { 0x01, 0x00, 0x60, 0x32, 0x01, 0x01};

//const byte codeL1[] = {0x01, 0x01, 0x24, 0x07, 0x00, 0xff};
//const byte codeL2[] = {0x01, 0x01, 0x38, 0x07, 0x00, 0xFF};
//const byte codeL3[] = {0x01, 0x01, 0x4C, 0x07, 0x00, 0xFF};

// OBIS Code 1.7.0
byte code7[] = { 0x01, 0x00, 0x01, 0x07, 0x00, 0xff}; // 1.7.0
byte codeL1[] = {0x01, 0x00, 0x15, 0x07, 0x00, 0xff};// 21.7.0 // P auf L1
byte codeL2[] = {0x01, 0x00, 0x29, 0x07, 0x00, 0xFF};// 41.7.0 // P auf L2
byte codeL3[] = {0x01, 0x00, 0x3D, 0x07, 0x00, 0xFF};// 61.7.0 // P auf L3



struct d {
  byte sml_octst[50];
  uint32_t sml_uint32;
  int32_t sml_int32;
} data;


uint32_t kw1, kw2, kw3, kw4, kw5, kw6;

int32_t ID;
signed long W0, W1, W2, W3;
char kennung[30];
char s[30];
int smlpos = 0;

void setup()
{
  Serial1.begin(Baudrate, Protocol, RXD2, TXD2);
  Serial.begin(Baudrate_USB);
  delay(1000);
  Serial.println("Los gehts!");
}

void loop() {
  sensor_auslesen();
} // loop

void sensor_auslesen(){
   for (int i=0; i <= sizeof(sml_message); i ++) {
   //if (Serial1.available() && !is_parsing) {
    myTime = millis();
    inByte = sml_message[i];
    //inByte = Serial1.read();
    //Serial.print(", 0x");
    //Serial.printf("%02X", inByte );
    if (inByte == 0x1B && !sml_start) {
      // Wir haben noch keinen (neuen) Anfang gefunden...
      start_counter++;
      //Serial.print("0x1B erkannt,
      Startcounter:");
      Serial.println(start_counter);
    } else {
      if (inByte == 0x01 && start_counter==4) {
        //Serial.print("0x01 erkannt! (start_2_counter =
        ");Serial.print(start_2_counter);Serial.println(")");
        start_2_counter++;
        if (start_2_counter==4) {
          Serial.print("--> Anfang gefunden - ");
          /*
          if (sml_start) {
            // Anscheinend wurde beim letzten Durchlauf kein Ende gefunden...
            Serial.print(" Leeren, da kein Ende beim letzten Mal ");
            memset(sml, 0, sizeof(sml));
          }
          */
          sml_start=true;
          sml[0] = 0x1B;
          sml[1] = 0x1B;
          sml[2] = 0x1B;
          sml[3] = 0x1B;
          sml[4] = 0x01;
          sml[5] = 0x01;
          sml[6] = 0x01;
          inByteIndex = 7;
          //start_counter = 0;
          start_2_counter = 0;
        }
      } else if (inByte == 0x1A && start_counter==4) {
        Serial.println("--> Ende gefunden!");
        sml[inByteIndex] = 0x1A;
        sml_start=false;
        start_counter = 0;
        start_2_counter = 0;
        parsen();
      } else {
        start_counter = 0;   
      }
      
    }
    if (sml_start) {
      if (inByteIndex < maxBufferSize - 1) {
        sml[inByteIndex] = inByte;
        inByteIndex++;
      }
    }
   }
}

void parsen() {
  is_parsing = true;
  sml[inByteIndex] = '\0'; // Nullterminierung des Char-Arrays
  // put your main code here, to run repeatedly:
  //printdump(sizeof(sml));

  // die ersten 3 byte sind ascii Zeichen
  findcode(sml_Kopf, sizeof(sml_Kopf));
  for (byte i = 0; i < 3; i++) {
    kennung[i] = data.sml_octst[i + 1];
    kennung[i + 1] = 0;
  }

  // der rest ergibt eine Zahl
  for (byte i = 4; i < 10; i++) {
    ID = ID << 8;
    ID += data.sml_octst[i];
  }

  // hier ist eventuell zu prüfen ob der Zähler die Daten als int oder uint
  // ausgibt um den vollen Zahlenumfang zu haben
  // normal solle int reichen

  findcode(code1, sizeof(code1)); kw1 = data.sml_uint32 / 1000;
  findcode(code2, sizeof(code2)); kw2 = data.sml_uint32 / 1000;
  findcode(code3, sizeof(code3)); kw3 = data.sml_uint32 / 1000;
  findcode(code4, sizeof(code4)); kw4 = data.sml_uint32 / 1000;
  findcode(code5, sizeof(code5)); kw5 = data.sml_uint32 / 1000;
  findcode(code6, sizeof(code6)); kw6 = data.sml_uint32 / 1000;

  findcode(code7, sizeof(code7)); W0 = data.sml_int32;

  findcode(codeL1, sizeof(codeL1)); W1 = data.sml_int32;
  findcode(codeL2, sizeof(codeL2)); W2 = data.sml_int32;
  findcode(codeL3, sizeof(codeL3)); W3 = data.sml_int32;


  sprintf(s, "\nID %s %ld", kennung, ID); Serial.println(s);
  Serial.print(F("Zählwerk    A+ ")); Serial.print(kw1); Serial.println("KWh");
  Serial.print(F("Zählwerk T1 A+ ")); Serial.print(kw2); Serial.println("KWh");
  Serial.print(F("Zählwerk T2 A+ ")); Serial.print(kw3); Serial.println("KWh");
  Serial.print(F("Zählwerk    A- ")); Serial.print(kw4); Serial.println("KWh");
  Serial.print(F("Zählwerk T1 A- ")); Serial.print(kw5); Serial.println("KWh");
  Serial.print(F("Zählwerk T2 A- ")); Serial.print(kw6); Serial.println("KWh");
  Serial.print(F("Wirkleistung   ")); Serial.print(W0); Serial.println("W");

  Serial.print(F("Leistung L1  ")); Serial.print(W1); Serial.println("W");
  Serial.print(F("Leistung L2  ")); Serial.print(W2); Serial.println("W");
  Serial.print(F("Leistung L3  ")); Serial.print(W3); Serial.println("W");
  
  inByteIndex = 0;

  // Char-Array löschen
  memset(sml, 0, sizeof(sml));
  is_parsing = false;
}




void findcode(byte *code, uint16_t anz) {
  bool found = false;
  bool sta = false;
  bool kopfdaten = false;
  unsigned int index = 0;
  unsigned int startindex = 0;
  unsigned int i, ii;
  unsigned int n = 0;

  int dazu = 0;
  int64_t erg = 0;
  uint64_t uerg = 0;
  byte maxelm = 0;
  byte elm = 0;
  byte maxloop = 0;

  data.sml_uint32 = 0;
  data.sml_int32 = 0;

  printdbgn();

  // Start abfragen
  if (sml[0] == 0x1B && sml[1] == 0x1B && sml[2] == 0x1B && sml[3]) sta = true;
  if (!sta) return; // abbrechen Anfang falsch

  for ( i = 0; i <= sizeof(sml) - anz; i++) {
    if (sml[i] == code[0]) { // Anfang gefunden
      found = true;
      for (ii = 1; ii < anz; ii++) { // restliche prüfen
        if (sml[i + ii] != code[ii]) found = false;
      }
      index = i + ii;
      startindex = i;

      if (found) { // OBIS Code stimmt

        printdbg(F("code gefunden: "));
        for (byte i = 0; i < anz; i++ ) {
          printhex(code[i]);
        }
        printdbgn();
        break; // for schleife verlassen
      }
    }
  }
  // hier gehts weiter
  // Abfrage geht es um die Kopfdaten
  int s = 0;
  for (i = 0; i < anz; i++) {
    s += code[i];
  }
  //printdbg(F("summe")); printdbgn(s);
  if (s == 112) {
    kopfdaten = true;
    //printhex(sml[index]);
    maxelm = 0;// es gab noch kein Listen-Element
    elm = 1;
  }
  else {
    kopfdaten = false;
    //printhex(sml[index]);
    maxelm = sml[startindex - 2] - 0x70; // Anzahl der Elemnte aus dem zugeh.
    Listen-Elemen
    elm = 2; // das nächste Element ist 2
  }

  printdbg(F("start mit elm  ")); printdbg( elm);
  printdbg(F("  max Elemnt ")); printdbgn(maxelm);

  do {
    maxloop++;
    if (maxloop > 30) { // Notausstieg :-)
      printdbgn(F("kein Elemt erkannt"));
      return;
    }
    // --------- 0x01 auswerten
    switch (sml[index]) {
      case 0x01://leres Element
        printhex(sml[index]);
        printdbg(F("leres erkannt "));
        printdbgn(elm);
        index++;
        elm++;
        break;

      // ---------- Liste auswerten
      case 0x70 ... 0x7f: // Liste
        dazu = sml[index] - 0x70; // neue Elemente aus Liste
        maxelm = maxelm + dazu;
        printhex(sml[index]);

        printdbg(F("element ")); printdbg(elm);
        printdbg(F(" Liste erkannt plus ")); printdbg(dazu);
        printdbg(F(" jetzt max ")); printdbgn(maxelm);

        index++;
        elm++;
        break;
      //----------- Integer auswerten
      case 0x51 ... 0x5f:// integer
        n = sml[index] - 0x50;
        for ( uint16_t i =  0; i < n; i++) {
          printhex(sml[index + i]);
        }
        printdbg(F(" element ")); printdbgn(elm);
        if (elm == maxelm - 1) {
          erg = 0;
          for (uint16_t i =  1; i < n; i++ ) {
            erg = erg << 8;
            erg += sml[index + i];
          }

          if (sml[index - 1] == 0x02) erg = erg * 100;
          else if (sml[index - 1] == 0x01) erg = erg * 10;
          else if (sml[index - 1] == 0xff) erg = erg / 10;
          else if (sml[index - 1] == 0xfe) erg = erg / 100;
          else if (sml[index - 1] == 0xfd) erg = erg / 1000;
          else if (sml[index - 1] == 0xfc) erg = erg / 10000;
          data.sml_int32 = erg;
          printdbg(F("SML_VALUE INT ")); printdbgn(erg);
        }
        elm++;
        index += n;
        break;
      // ------------unsigned Integer auswrten
      case 0x61 ... 0x6f:// unsigned integer
        n = sml[index] - 0x60;
        for (uint16_t i = 0; i < n; i++) {
          printhex(sml[index + i]);
        }
        printdbg(F(" element ")); printdbgn(elm);
        if (elm == maxelm - 1) { // hier sollte SML_VALUE sein
          uerg = 0;
          for (uint16_t i = 1; i < n; i++ ) {
            uerg = uerg << 8;
            uerg += sml[index + i];
          }

          if (sml[index - 1] == 0x02) uerg = uerg * 100;
          else if (sml[index - 1] == 0x01) uerg = uerg * 10;
          else if (sml[index - 1] == 0xff) uerg = uerg / 10;
          else if (sml[index - 1] == 0xfe) uerg = uerg / 100;
          else if (sml[index - 1] == 0xfd) uerg = uerg / 1000;
          else if (sml[index - 1] == 0xfc) uerg = uerg / 10000;
          data.sml_uint32 = uerg;
          printdbg(F("SML_VALUE UINT ")); printdbgn(uerg);
        }

        elm++;
        index += n;
        break;
      //----------- octal String auswerten
      case 0x02 ... 0x1f: // octal String
        n = sml[index];
        for (uint16_t i = 0; i < n; i++) {
          printhex(sml[index + i]);
        }

        if (n > sizeof(data.sml_octst)) n = sizeof(data.sml_octst);
        if ( kopfdaten == true && elm == maxelm - 3) { // Kopfdaten ID abfragen 
        // das ist noch Q&D

          printdbg(F("element ")); printdbg(elm);
          printdbgn(F(" SML_ServerID "));

          for (uint16_t i = 1; i < n; i++) {
            //printhex(sml[index + i]);
            data.sml_octst[i - 1] = sml[index + i];

          }

          //return;
        }
        else {

          for (uint16_t i = 1; i < n; i++) {
            //printhex(sml[index + i]);
            //Serial.println(i);
            data.sml_octst[i - 1] = sml[index + i];
            data.sml_octst[i] = 0;
          }
          printdbg(F("element ")); printdbgn(elm);
        }
        elm++;
        index += n;
        break;

      //----------- octal String länger 15 Zeichen
      case 0x80 ...0x8f:// octalstring länger 14 Zeichen
        n = (sml[index] - 0x80) * 16;
        n = n + sml[index + 1];
        for (uint16_t i = 0; i < n; i++) {
          printhex(sml[index]);
        }
        printdbg(F("element ")); printdbgn(elm);
        elm++;
        index += n;
        break;
    }
  } while (elm <= maxelm);

  return;
}


void printdump(uint16_t n) {

  for (unsigned int i = 0; i < n; i++) {
    if (i % 20 == 0) printdbgn();
    printhex(sml[i]);
  }
  printdbgn();
}

void printhex(byte wert) {

  char hexstring[10];
  if (wert < 16)sprintf(hexstring, "0%x ", wert);
  else sprintf(hexstring, "%x ", wert);
  printdbg(hexstring);
}