//GAN BLE cube routines
//The BLE data is heavily encoded with AES128 /16bit

#include "BLEDevice.h"
#include "mbedtls/aes.h"


//GAN Cube variables
char GAN_KEY[] = "abcdefghijklmnop";
unsigned char key10[16] = {198, 202, 21, 223, 79, 110, 19, 182, 119, 13, 230, 89, 58, 175, 186, 162};
uint8_t key11[16] = {67, 226, 91, 214, 125, 220, 120, 216, 7, 96, 163, 218, 130, 60, 1, 241};

uint8_t GAN_V2_KEY[16] = {0x01, 0x02, 0x42, 0x28, 0x31, 0x91, 0x16, 0x07, 0x20, 0x05, 0x18, 0x54, 0x42, 0x11, 0x12, 0x53};
uint8_t GAN_V2_IV[16] = {0x11, 0x03, 0x32, 0x28, 0x21, 0x01, 0x76, 0x27, 0x20, 0x95, 0x78, 0x14, 0x32, 0x12, 0x02, 0x43};
uint8_t MAC[6] = {0x10, 0x23, 0x02, 0x34, 0x12, 0xAB}; //[16, 35, 2, 52, 18, 171]
uint8_t batteryCmd[20] = {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//String GAN_MOVES[12] = {"U","U'","R","R'","F","F'","D","D'","L","L'","B","B'"};

//GAN decoding
uint32_t extractBits(uint8_t data[], int start, int count) {
    uint32_t result = 0;
    for (int i = 0; i < count; i++) {
        int bit = start + i;
        result <<= 1;
        uint8_t currentBit = data[bit/8] & (0x01 << (7 - (bit % 8)));
        if (currentBit != 0) {
            result |= 1;
        }
    }
    return result;
}

void computeKeyIV() {
  int i = 0;
  for (int j = 0; j < sizeof(MAC); j++) {
    GAN_V2_KEY[i] = (uint8_t)(((uint16_t)GAN_V2_KEY[i] + (uint16_t)MAC[j]) % 255);
    GAN_V2_IV[i] = (uint8_t)(((uint16_t)GAN_V2_IV[i] + (uint16_t)MAC[j]) % 255);
    i++;
  }
  for (int i = 0; i < 16; i++) {
    GAN_KEY[i] = GAN_V2_KEY[i];
  }
}

void decrypt(unsigned char * chipherText, char * key, unsigned char * outputBuffer){
  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*)chipherText, outputBuffer);
  mbedtls_aes_free( &aes );
} 

void encrypt(unsigned char * plainText, char * key, unsigned char * outputBuffer){
 
  mbedtls_aes_context aes;
 
  mbedtls_aes_init( &aes );
  mbedtls_aes_setkey_enc( &aes, (const unsigned char*) key, strlen(key) * 8 );
  mbedtls_aes_crypt_ecb( &aes, MBEDTLS_AES_ENCRYPT, (const unsigned char*)plainText, outputBuffer);
  mbedtls_aes_free( &aes );
}

unsigned char* decryptGAN(unsigned char val[]) {
    unsigned char* value = val;
    unsigned char endCipher[16] = {0};
    unsigned char endPlain[16] = {0};
    unsigned char decryptedBytes1[16] = {0};
    unsigned char decryptedBytes2[16] = {0};
    unsigned char startCipher[16] = {0};
    unsigned char startPlain[16] = {0};

    mbedtls_aes_context aes;
 
    char* key;
    unsigned char * outputBuffer;
    
    for (int i = 0; i < 16; i++) {
        endCipher[i] = value[4+i];
    }
    memcpy(endPlain, endCipher, 16);
 
    //encrypt(plainText, key, cipherTextOutput);
    decrypt(endPlain, GAN_KEY, decryptedBytes1);

    for (int i = 0; i < 16; i++) {
        endPlain[i] = decryptedBytes1[i];
    }
    
    for (int i = 0; i < 16; i++) {
        endPlain[i] ^= GAN_V2_IV[i];
        value[4+i] = endPlain[i];
    }
    
    for (int i = 0; i < 16; i++) {
        startCipher[i] = value[i];
    }
    
    memcpy(startPlain, startCipher, 16);

    decrypt(startPlain, GAN_KEY, decryptedBytes2);
    
    for (int i = 0; i < 16; i++) {
        startPlain[i] = decryptedBytes2[i];
    }
    
    for (int i = 0; i < 16; i++) {
        startPlain[i] ^= GAN_V2_IV[i];
        value[i] = startPlain[i];
    }
    return value;
}

unsigned char* encryptGAN(unsigned char val[]) {
    unsigned char* value = val;
    unsigned char endCipher[16] = {0};
    unsigned char endPlain[16] = {0};
    unsigned char decryptedBytes1[16] = {0};
    unsigned char encryptedBytes1[16] = {0};
    unsigned char encryptedBytes2[16] = {0};
    unsigned char startCipher[16] = {0};
    unsigned char startPlain[16] = {0};

    mbedtls_aes_context aes;
 
    char* key;
    unsigned char * outputBuffer;

    for (int i = 0; i < 16; i++) {
        value[i] ^= GAN_V2_IV[i];
    }
    memcpy(startCipher, value, 16);
    encrypt(startCipher, GAN_KEY, encryptedBytes1);
    for (int i = 0; i < 16; i++) {
        startCipher[i] = encryptedBytes1[i];
        value[i] = startCipher[i];
    }
    memcpy(endCipher, value, 16);
    for (int i = 0; i < 16; i++) {
        value[4+i] ^= GAN_V2_IV[i];
        endCipher[i] = value[4+i];
    }
    encrypt(endCipher, GAN_KEY, encryptedBytes2);
    
    for (int i = 0; i < 16; i++) {
        value[4+i] = encryptedBytes2[i];
    }
    return value;
}

void batteryStatus() {
  unsigned char* val = encryptGAN(batteryCmd);
  //pRemoteCharacteristicTx->writeValue(val,20);
}

void setup() {
  computeKeyIV();
  Serial.begin(115200);
  Serial.println("Starting Arduino BLE Client application...");
}

void loop() {
  unsigned char* res0 = encryptGAN(batteryCmd);
  unsigned char* res1 = decryptGAN(res0);
  for (int i = 0; i<20;i++) {
    Serial.print(res0[i]);
    Serial.print(", ");
  }
  Serial.println();
  delay(1000);

}