char Source[] = "004F006E006C0069006E00650020007A0070007200E100760061003A000A0535057D00200561057A0580057805820574002005650574002005320580057605780575057805820574000A04230439049304430440044704D90020002F00200055007900670068007500720063006800650020002F0020062606C7064A063A06C70631068606D50020";
char Output[200];
char Parameter1[75];
uint8_t MessageLength;


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 UCS2 string...");
  // Serial.flush();
  HandleUCS2(Parameter1, Output, &MessageLength);

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

  // memset(Parameter1, 0, sizeof(Parameter1));
  // memset(Output, 0, sizeof(Output));
  
  delay(500);

}

void HandleUCS2(char* Param1, char* Param2, uint8_t *length) {
  char Param1UCS2[] = "002B003400320030003600300033003400330030003000310032";
  char LocalBuffer[5] = {};
  uint16_t thelen = 300, idx = 0;
  memset(Param1, 0, sizeof(Param1));
  memset(Param2, 0, sizeof(Param2));
  
  while (! Serial.available()) {
    ;
  }
  uint32_t Timestamp = millis();
  while (thelen && (millis() - Timestamp <= 50)) { // && (millis() - Timestamp <= timeout)
    if (Serial.available()) {
      LocalBuffer[idx] = Serial.read();
      thelen--;
      idx++;
      Timestamp = millis();
    }
    if (idx == 4) {
      UCS2toUTF(LocalBuffer, Param2, 160);
      idx = 0;
      Timestamp = millis();
    } 
  }

  UCS2toUTF(Param1UCS2, Param1, 14);
  
  *length = strlen(Param2);
}

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]
    }
    strncat(Out, (byte*)&U, 2);
    if (strlen(Out) >= maxlength) {
      Serial.println("Output is truncated.");
      break;
    }
  }
}