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

}

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