// Optimized 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

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

#ifndef max
#define max(a, b) ((a) > (b) ? (a) : (b))
#endif

// Helper function for 32-bit rotate left
uint32_t rotate_left(uint32_t value, uint8_t shift) {
    return (value << shift) | (value >> (32 - shift));
}

// Helper function for 32-bit rotate right
uint32_t rotate_right(uint32_t value, uint8_t shift) {
    return (value >> shift) | (value << (32 - shift));
}

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] = rotate_left(S[i] + A + B, 3);
        B = L[j] = rotate_left(L[j] + A + B, (A + B) & 31);
        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 = rotate_left(A ^ B, B & 31) + S[2 * i];
        B = rotate_left(B ^ A, A & 31) + 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 = rotate_right(B - S[2 * i + 1], A & 31) ^ A;
        A = rotate_right(A - S[2 * i], B & 31) ^ 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
}