#include "mbedtls/aes.h"

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}

String decryptBuffer(String cipherText, char* key) {
	// returns decrypted String of ciphered text (length: 16 characters)
	String decipheredTextString = "1234567890123456";

	// encrypt ciphered chipherText buffer of length 16 characters to plain text
	mbedtls_aes_context aes;

	mbedtls_aes_init(&aes);
	mbedtls_aes_setkey_dec(&aes, (const unsigned char*)key, strlen(key) * 8);
	mbedtls_aes_crypt_ecb(&aes, MBEDTLS_AES_DECRYPT, (const unsigned char*)cipherText.c_str(), (unsigned char*)decipheredTextString.c_str());
	mbedtls_aes_free(&aes);

	// removes eventually \0 inside
	decipheredTextString = String(decipheredTextString.c_str());
	return decipheredTextString;
}

String decryptString(String cipherText, char* key) {
	constexpr int BLOCK_SIZE = 16;
	String buffer = "";
	String decipheredTextString = "";

	// Decrypt each block
	for (int block = 0; block < cipherText.length() / BLOCK_SIZE; block++) {
		buffer = "";
		for (int j = block * BLOCK_SIZE; j < (block + 1) * BLOCK_SIZE; j++) {
			buffer += cipherText[j];
		}

		decipheredTextString += decryptBuffer(buffer, key);
	}

	// Check for and remove padding if present
	if (!decipheredTextString.isEmpty()) {
		int lastByte = decipheredTextString[decipheredTextString.length() - 1];

		if (lastByte > 0 && lastByte <= BLOCK_SIZE) {
			bool isPadded = true;
			for (int i = 1; i <= lastByte && isPadded; i++) {
				if (decipheredTextString[decipheredTextString.length() - i] != lastByte) {
					isPadded = false;
				}
			}

			if (isPadded) {
				decipheredTextString = decipheredTextString.substring(0, decipheredTextString.length() - lastByte);
			}
		}
	}

	return decipheredTextString;
}