char Source[] = "004F006E006C0069006E00650020007A0070007200E100760061003A000A0535057D00200561057A0580057805820574002005650574002005320580057605780575057805820574000A04230439049304430440044704D90020002F00200055007900670068007500720063006800650020002F0020062606C7064A063A06C70631068606D50020";
char Output[495];
char Parameter1[75];
uint16_t MessageLength;
uint32_t Timer;


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("It took ");
  Serial.print(micros() - Timer);
  Serial.println(" us to convert it.");

  // 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(0);

}

void HandleUCS2(char* Param1, char* Param2, uint16_t *length) {
  char Param1UCS2[] = "002B003400320030003600300033003400330030003000310032";
  char LocalBuffer[5];
  uint16_t thelen = 2600, idx = 0, OutputSize = sizeof(Output) - 1;
  memset(Parameter1, 0, sizeof(Parameter1));
  memset(Output, 0, sizeof(Output));

  while (! Serial.available()) {
    ;
  }
  // uint32_t Timestamp = millis();
  do { // && (millis() - Timestamp <= timeout)
    if (Serial.available()) {
      LocalBuffer[idx] = Serial.read();
      // thelen--;
      idx++;
      // Timestamp = millis();
    }
    if (idx == 4) {
      UCS2toUTF(LocalBuffer, Param2, OutputSize);
      idx = 0;
      // Timestamp = millis();
    }
  } while (LocalBuffer[idx - 1] != '\n');
  // } while (LocalBuffer[idx - 1] != '\n' && (millis() - Timestamp <= 500));

  Timer = micros();

  // UCS2toUTF(Param1UCS2, Param1, sizeof(Parameter1) - 1);

  *length = strlen(Output);
}

void UCS2toUTF(char* In, char* Out, uint16_t maxlength) {
  uint16_t n = strlen(In), U = 0, OutLen;
  char S[5];  // 4 digits + \0
  char outputBuffer[4]; // Up to 3 characters + \0

  // 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 <= 0x7F) {
      // 1-byte sequence (ASCII)
      if (strlen(Out) + 1 > maxlength) {
        Serial.println("Maxlen 1B.");
        break;
      }
      outputBuffer[0] = (char)U;
      outputBuffer[1] = '\0';
    } else if (U <= 0x7FF) {
      // 2-byte sequence
			if (strlen(Out) + 2 > maxlength) {
        Serial.println("Maxlen 2B.");
        break;
      }
      outputBuffer[0] = (char)(0xC0 | (U >> 6));
      outputBuffer[1] = (char)(0x80 | (U & 0x3F));
      outputBuffer[2] = '\0';
    } else if (U <= 0xFFFF) {
      // 3-byte sequence
			if (strlen(Out) + 3 > maxlength) {
        Serial.println("Maxlen 3B.");
        break;
      }
      outputBuffer[0] = (char)(0xE0 | (U >> 12));
      outputBuffer[1] = (char)(0x80 | ((U >> 6) & 0x3F));
      outputBuffer[2] = (char)(0x80 | (U & 0x3F));
      outputBuffer[3] = '\0';
    } else {
      outputBuffer[0] = '\0';  // It should never happen for right input.
    }

    strcat(Out, outputBuffer);
  }
}