#include <stdint.h>
const uint32_t MAX_UINT = 0xFFFFFFFF;

uint32_t readUInt32BE(const uint8_t* buffer, int offset) {
  return ((uint32_t)buffer[offset] << 24) |
         ((uint32_t)buffer[offset + 1] << 16) |
         ((uint32_t)buffer[offset + 2] << 8) |
         ((uint32_t)buffer[offset + 3]);
}

uint32_t readUInt32LE(const uint8_t* buffer, int offset) {
  return ((uint32_t)buffer[offset]) |
         (((uint32_t)buffer[offset + 1]) << 8) |
         (((uint32_t)buffer[offset + 2]) << 16) |
         (((uint32_t)buffer[offset + 3]) << 24);
}

void writeUInt32BE(uint8_t *buffer, int offset, uint32_t value) {
  buffer[offset] = (value >> 24) & 0xFF;
  buffer[offset + 1] = (value >> 16) & 0xFF;
  buffer[offset + 2] = (value >> 8) & 0xFF;
  buffer[offset + 3] = value & 0xFF;
}

void TEA(uint8_t *src, uint8_t *pwdkey, bool isEncrypt, uint8_t *newbyte) {
  const int srclen = strlen((char*)src);
  const uint32_t TIMES = 32;
  const uint32_t DELTA = 0x9e3779b9;
  uint32_t a = readUInt32LE(pwdkey, 0);
  uint32_t b = readUInt32LE(pwdkey, 4);
  uint32_t c = readUInt32LE(pwdkey, 8);
  uint32_t d = readUInt32LE(pwdkey, 12);
  //  Serial.println(a);
  //  Serial.println(b);
  //  Serial.println(c);
  //  Serial.println(d);

  int n = isEncrypt ? (8 - (srclen % 8)) : 0;
  //  Serial.println(n);

  uint8_t mybyte[srclen + n] = {n};
  for (int i = n; i < srclen + n; i++) {
    mybyte[i] = src[i - n];
  }

  for (int offset = 0; offset < srclen + n; offset += 8) {
    uint32_t y = readUInt32BE(mybyte, offset);
    uint32_t z = readUInt32BE(mybyte, offset + 4);
    uint32_t sum = isEncrypt ? 0 : DELTA * TIMES;

    if (!isEncrypt) {
      sum = TIMES == 32 ? 0xC6EF3720 : 0xE3779B90;
    }

    for (int i = 0; i < TIMES; i++) {
      if (isEncrypt) {
        sum += DELTA;
        y += ((z << 4) + a ^ z + sum ^ (z >> 5) + b);

        uint32_t ztest =((y << 4) + c ^ y + sum ^ (y >> 5) + d);
                        Serial.print("y1:");
        Serial.println(y);

        z += ((y << 4) + c ^ y + sum ^ (y >> 5) + d);



        Serial.print("y:");
        Serial.println(y);
        Serial.print("z:");
        Serial.println(z);
        Serial.print("sum:");
        Serial.println(sum);

      } else {
        z -= ((y << 4) + c ^ y + sum ^ (y >> 5) + d);
        y -= ((z << 4) + a ^ z + sum ^ (z >> 5) + b);
        sum -= DELTA;
      }
    }
    Serial.println(y);
    Serial.println(z);
    Serial.println(sum);

    /*
      "y_:-1003859360"
      "z_:1194178305"
      "sum_:-957401312"
    */


    writeUInt32BE(newbyte, offset, y);
    writeUInt32BE(newbyte, offset + 4, z);
  }
}


void setup() {
  Serial.begin(9600);
  // put your setup code here, to run once:
  uint8_t src[5] = {0x68, 0x61, 0x6c, 0x6c, 0x6f};
  uint8_t pwdkey[16] = {0x45, 0x23, 0xF1, 0x0F, 0x21, 0x43, 0x65, 0x87, 0x32, 0x48, 0x73, 0x89, 0x02, 0xEF, 0xCD, 0xAB};
  uint8_t result[8] = {};
  TEA(src, pwdkey, true, result);
  for (int i = 0; i < 8; i++) {
    Serial.print(result[i], HEX);
    Serial.print(" ");
  }

  Serial.println();
}

void loop() {
  // put your main code here, to run repeatedly:
}