// RC5 Encryption Code for Arduino
#include <Arduino.h>

#define WORD_SIZE 32        // Word size in bits
#define NUM_ROUNDS 12       // Number of rounds
#define KEY_LENGTH 16       // Length of the key in bytes

#define W (WORD_SIZE)       // Word size in bits
#define P32 0xB7E15163      // Magic constant P for 32-bit words
#define Q32 0x9E3779B9      // Magic constant Q for 32-bit words

uint32_t S[2 * NUM_ROUNDS + 2]; // Expanded key table

void RC5_key_setup(const uint8_t *key) {
    uint32_t L[KEY_LENGTH / 4];
    for (uint8_t i = 0; i < KEY_LENGTH / 4; i++) {
        L[i] = ((uint32_t)key[4 * i]) |
               ((uint32_t)key[4 * i + 1] << 8) |
               ((uint32_t)key[4 * i + 2] << 16) |
               ((uint32_t)key[4 * i + 3] << 24);
    }

    S[0] = P32;
    for (uint8_t i = 1; i < 2 * NUM_ROUNDS + 2; i++) {
        S[i] = S[i - 1] + Q32;
    }

    uint32_t A = 0, B = 0;
    uint8_t i = 0, j = 0;
    uint32_t n = 3 * max(KEY_LENGTH / 4, 2 * NUM_ROUNDS + 2);

    for (uint32_t k = 0; k < n; k++) {
        A = S[i] = (S[i] + A + B) << 3 | (S[i] + A + B) >> (W - 3);
        B = L[j] = (L[j] + A + B) << (A + B) | (L[j] + A + B) >> (W - ((A + B) & (W - 1)));
        i = (i + 1) % (2 * NUM_ROUNDS + 2);
        j = (j + 1) % (KEY_LENGTH / 4);
    }
}

void RC5_encrypt(uint32_t *data) {
    uint32_t A = data[0] + S[0];
    uint32_t B = data[1] + S[1];

    for (uint8_t i = 1; i <= NUM_ROUNDS; i++) {
        A = (A ^ B) << B | (A ^ B) >> (W - B);
        A += S[2 * i];
        B = (B ^ A) << A | (B ^ A) >> (W - A);
        B += S[2 * i + 1];
    }

    data[0] = A;
    data[1] = B;
}

void RC5_decrypt(uint32_t *data) {
    uint32_t B = data[1];
    uint32_t A = data[0];

    for (uint8_t i = NUM_ROUNDS; i > 0; i--) {
        B -= S[2 * i + 1];
        B = (B >> A | B << (W - A)) ^ A;
        A -= S[2 * i];
        A = (A >> B | A << (W - B)) ^ B;
    }

    data[1] = B - S[1];
    data[0] = A - S[0];
}

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

    uint8_t key[KEY_LENGTH] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, 0xFE, 0xDC, 0xBA, 0x98, 0x76, 0x54, 0x32, 0x10};
    uint32_t data[2] = {0x12345678, 0x9ABCDEF0};

    RC5_key_setup(key);

    Serial.println("Original Data:");
    Serial.print("A: 0x"); Serial.println(data[0], HEX);
    Serial.print("B: 0x"); Serial.println(data[1], HEX);

    RC5_encrypt(data);

    Serial.println("\nEncrypted Data:");
    Serial.print("A: 0x"); Serial.println(data[0], HEX);
    Serial.print("B: 0x"); Serial.println(data[1], HEX);

    RC5_decrypt(data);

    Serial.println("\nDecrypted Data:");
    Serial.print("A: 0x"); Serial.println(data[0], HEX);
    Serial.print("B: 0x"); Serial.println(data[1], HEX);
}

void loop() {
    // No repeated action
}