#include "TransferDataStructure.h"

String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = { 0, -1 };
  int maxIndex = data.length() - 1;

  for (int i = 0; i <= maxIndex && found <= index; i++) {
    if (data.charAt(i) == separator || i == maxIndex) {
      found++;
      strIndex[0] = strIndex[1] + 1;
      strIndex[1] = (i == maxIndex) ? i + 1 : i;
    }
  }
  return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}

dataStruct2 GetSerialData(dataStruct2 &dataIn, bool debug = false)
{
  if (Serial.available() && Serial.peek() != -1)
  {
    //    String data = Serial.readStringUntil('\r');
    //    while (Serial.read() >= 0);
    //    int maxIndex = data.length() - 1;
    //    int index = 0;
    //    int next_index;
    //    String data_word;
    //    String channel;
    //    String angle;
    //    do {
    //      next_index = data.indexOf(',', index);
    //
    //      data_word = data.substring(index, next_index);
    //      channel = data_word.substring(0, data_word.indexOf('_'));
    //      angle = data_word.substring(data_word.indexOf('_') + 1);
    //
    //      if (isDigit(angle[0])) // prevent error overwriting first entry.
    //        dataIn.servoPositions[channel.toInt()] = angle.toFloat();
    //
    //      index = next_index + 1;
    //    } while ((next_index != -1) && (next_index < maxIndex));


    /*String s = Serial.readStringUntil('s');
      for (int i = 0; i < 18; i++)
      {
      Serial.setTimeout(10);
      String c = Serial.readStringUntil('_');
      String v = Serial.readStringUntil(',');
      String cc = Serial.readStringUntil('=');
      int channel = c.toInt();
      int value = v.toInt();
      int crc = cc.toInt();

      if ((crc == (channel) * value) && value >= 0 && value <= 180)
        dataIn.servoPositions[channel] = value;*/
    //byte recv[18 + 1];
    if (Serial.available() >= 18 + 1 + 2 + 30)
    {
      bool recc = false;
      byte recv[256] = {0};
      //Serial.readBytesUntil(0xffffff, recv, 255);
      Serial.readBytesUntil(0xff, recv, 255);
      for (int i = 0; i < 255 - 19; i++)
      {
        //  if (recv[i + 1] == 0xff && recv[i + 2] == 0xff && recv[i + 3] == 0xff)
        if (recv[i + 19] == 0xfe)
        {
          for (int j = 0; j < 18 + 1; j++)
          {
            recv[j] = recv[(i) + j];
          }
          recc = true;

          break;
        }
      }


      //s.getBytes(recv, s.length());
      byte crc = 0x00;
      for (int i = 0; i < 18; i++)
      {
        crc += recv[i];
      }


      if (crc == recv[18])
      {

        if (recc)
        {
          for (int i = 0; i < 18; i++)
          {
            dataIn.servoPositions[i] = recv[i];
          }
        }
      }
      //}

      dataIn.remotePasstroughControl = true;

      if (debug)
      {
        Serial.println("Enabled Servo Passtrough mode!");

        Serial.println("Output line: ");
        for (int i = 0; i < 18; i++)
        {
          Serial.print(i);
          Serial.print(", ");
          Serial.print(dataIn.servoPositions[i]);
          Serial.println("");
        }
        Serial.println("");
      }
    }
  }

  return dataIn;
}