#include "TransferDataStructure.h"

dataStruct2 GetSerialData(dataStruct2 dataIn, bool debug = false)
{
  if (Serial.available())
  {
    String data = Serial.readStringUntil('\r');
    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));

    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;
}