unsigned char *x_data[2] = {NULL, NULL};
unsigned char *y_data[2] = {NULL, NULL};
int data_len[2] = {0,0};
int dataset = 0;
int idx = 0;
int pending = 0;

void setup() {
  Serial.begin(9600);
  //pinMode(LED_BUILTIN, OUTPUT); // set the digital pin as output:
}

unsigned char get_value(unsigned char c) {
  if (c >= '0' && c <= '9') return c - '0';
  if (c >= 'A' && c <= 'F') return (c - 'A') + 10;
  if (c >= 'a' && c <= 'f') return (c - 'a') + 10;
  return 0;
}

void process_data(String &data) {
  int length = data.length();
  unsigned char *xdata = new unsigned char(length/2);
  unsigned char *ydata = new unsigned char(length/2);

  int axis = 0;
  unsigned char x = 0;
  unsigned char y = 0;
  int cnt = 0;
  for (int i = 0; i < length; i += 2) {
    unsigned char hi = (get_value(data[i]) << 4);
    unsigned char lo = get_value(data[i+1]);
    if (axis == 0) {
      x = hi+lo;
      axis = 1;
    }
    else {
      y = hi+lo;
      xdata[cnt] = x;
      ydata[cnt] = y;
      axis = 0;
      Serial.print("(");
      Serial.print(x, HEX);
      Serial.print(",");
      Serial.print(y, HEX);
      Serial.println(")");
      cnt++;
    }
  }
  int ds = (dataset == 0?1:0);
  if (x_data[ds] != NULL) delete x_data[ds];
  if (y_data[ds] != NULL) delete y_data[ds];
  x_data[ds] = xdata;
  y_data[ds] = ydata;
  pending = 1;
  data_len[ds] = length / 4;
}

void loop() {
  if (Serial.available()) { // if there is data comming
    String command = Serial.readStringUntil('\n'); // read string until newline character
    command.replace(" ", "");
    while (command.length() % 4 != 0) command = command + '0';
    Serial.println(command);
    process_data(command);
  }
  if (x_data[dataset] != NULL) {
    Serial.print("Dataset: ");
    Serial.print(dataset);
    Serial.print(" Len: ");
    Serial.print(data_len[dataset]);
    Serial.print(" idx: ");
    Serial.print(idx);
    Serial.print(" - ");
    Serial.print(x_data[dataset][idx],HEX);
    Serial.print(',');
    Serial.println(y_data[dataset][idx],HEX);
    idx++;
    if (idx >= data_len[dataset]) {
      idx = 0;
      if (pending) {
        dataset = (dataset == 0?1:0);
        pending = 0;
        Serial.println("Swapping");
      }
      Serial.println("Starting over");
    }
  }
  else {
    if (pending == 1) {
      dataset = (dataset == 0?1:0);
      pending = 0;
      Serial.println("Swapping");
    }
  }
  delay(500);
}