char Output[200];
char Parameter1[75];


void setup() {
  Serial.begin(115200);

  // Serial.println("Insert UCS2 string...");

  // HandleUCS2(Parameter1, Output);

  // Serial.print("Parameter 1: ");
  // Serial.print(Parameter1);
  // Serial.println("«");
  // Serial.print("Parameter 2: ");
  // Serial.print(Output);
  // Serial.println("«");

}

void loop() {
  Serial.println("Insert two number HEX string...");
  // Serial.flush();
  HandleUCS2(Parameter1, Output);

  Serial.print("Parameter 1: ");
  Serial.print(Parameter1);
  Serial.println("«");
  Serial.print("Parameter 2: ");
  Serial.print(Output);
  Serial.println("«");
  Serial.println();

  memset(Parameter1, 0, sizeof(Parameter1));
  memset(Output, 0, sizeof(Output));

  delay(500);

}

void HandleUCS2(char* Param1, char* Param2) {
  char Param1UCS2[] = "002B003400320030003600300033003400330030003000310032";
  char LocalBuffer[] = "0000";
  // strcpy_P(TempBuffer, PSTR("0000\0"));
  uint16_t thelen = 300, idx = 2;
  while (! Serial.available()) {
    ;
  }
  
  uint32_t Timestamp = millis();
  while (thelen && (millis() - Timestamp <= 150)) { // && (millis() - Timestamp <= timeout)
    if (Serial.available()) {
      LocalBuffer[idx] = Serial.read();
      thelen--;
      idx++;
      Timestamp = millis();
    }
    if (idx == 4) {
      UCS2toUTF(LocalBuffer, Param2, 160);
      idx = 2;
      // Serial.println(TempBuffer);
      Timestamp = millis();
    }
  }

  UCS2toUTF(Param1UCS2, Param1, 14);
  // Serial.print("Temp Buffer: ");
  // Serial.println(TempBuffer);

}

void UCS2toUTF(char* In, char* Out, uint8_t maxlength) {
  uint16_t n = strlen(In), U = 0;
  char S[5] = {};  // 4 digits + \0
  uint8_t UL, UH;

  // Serial.println("Length of the UCS2 Input to converter is: ");
  // Serial.println(n);

  if (n % 4 != 0) {
    Serial.println("The Source is not complete.");
  }

  for (int i = 0; i < n ; i += 4) {
    strncpy(S, &In[i], 4);
    U = strtoul(S, NULL, 16);
    if (U > 128) {
      UL = ( (U & 0x00FF) | 0B10000000 ) & 0B10111111;
      UH = (U >> 6) | 0B11000000;
      U = (UL << 8) | UH; // [UL UH]
    }
    if (strlen(Out) >= maxlength) {
      Serial.println("Output is truncated.");
      break;
    }
    strncat(Out, (byte*)&U, 2);
  }
}