#include <stdint.h>

#define LENGTH_OF_BLOCK 8


#define NUMBER_OF_KEYS 8
#define KEY_LENGTH 16

const uint8_t encryptionKeys[NUMBER_OF_KEYS][KEY_LENGTH] = {
	{
		#include "key_0.h"
	},
	{
		#include "key_1.h"
	},
	{
		#include "key_2.h"
	},
	{
		#include "key_3.h"
	},
	{
		#include "key_4.h"
	},
	{
		#include "key_5.h"
	},
	{
		#include "key_6.h"
	},
	{
		#include "key_7.h"
	}
};

const uint8_t * key;

void xxtea_Encrypt(uint32_t * data, uint8_t dataLength);
void xxtea_Decrypt(uint32_t * data, uint8_t dataLength);

void Set_Encryption_Key(uint8_t sensorId)
{
	key = encryptionKeys[sensorId & 0xF];
}

/* The data packets are encrypted using the xxtea algorithm. */
void Encrypt(uint32_t * data, uint8_t dataLength, uint64_t salt)
{
	/* This function assumes that the dataLength is a multiple of the length of the
	 * salt (8)
	 *
	 * The data is encrypted using cipher block chaining:
	 * (https://upload.wikimedia.org/wikipedia/commons/8/80/CBC_encryption.svg)
	 */

	uint8_t i;
	uint8_t * previousCipherBlock = (uint8_t*) &salt;
	uint8_t * currentPlaintextBlock = (uint8_t*) data;

	for (i = 0; i < dataLength/LENGTH_OF_BLOCK; i++)
	{
		/* XOR of current plain text block and previous cipher block */
		//for (j = 0; j < LENGTH_OF_BLOCK; j++)
		//{
		//	currentPlaintextBlock[j] ^= previousCipherBlock[j];
		//}
		*((uint64_t*) currentPlaintextBlock) ^= *((uint64_t*) previousCipherBlock);

		/* Encrypt the block */
		xxtea_Encrypt((uint32_t*) currentPlaintextBlock, LENGTH_OF_BLOCK);

		/* Setup for next block */
		previousCipherBlock = currentPlaintextBlock;
		currentPlaintextBlock += LENGTH_OF_BLOCK;
	}

}


void Decrypt(uint32_t * data, uint8_t dataLength, uint64_t salt)
{
	/* This function assumes that the dataLength is a multiple of the length of the
	 * salt (8)
	 *
	 * The data is encrypted using cipher block chaining:
	 * (https://upload.wikimedia.org/wikipedia/commons/2/2a/CBC_decryption.svg)
	 */

	/* The data is decrypted from back to front as front to back cannot be done in
	 * place
	 */
	uint8_t i;
	uint8_t * previousCipherBlock = (uint8_t*) data + dataLength - 2 * LENGTH_OF_BLOCK;
	uint8_t * currentCipherBlock = (uint8_t*) data + dataLength - LENGTH_OF_BLOCK;

	for (i = dataLength/LENGTH_OF_BLOCK; i > 0; i--)
	{
		/* Decrypt the block */
		xxtea_Decrypt((uint32_t*) currentCipherBlock, LENGTH_OF_BLOCK);

		/* XOR of the decrypted block with cipher block in front of it */
		*((uint64_t*) currentCipherBlock) ^= *((uint64_t*) previousCipherBlock);

		/* Setup for next block */
		currentCipherBlock = previousCipherBlock;
		if (previousCipherBlock != (uint8_t*) data)
		{
			previousCipherBlock -= LENGTH_OF_BLOCK;
		}
		else
		{
			previousCipherBlock = (uint8_t*) &salt;
		}
	}
}

void xxtea_Encrypt(uint32_t * data, uint8_t dataLength)
{
    uint32_t sum = 0, z, y, e;
    uint8_t i = 6 + 52/(dataLength/4), r;
    const int16_t n = dataLength / 4 - 1;

    z = data[n]; // left neighbour for the first round
    do {
        // cycle
        sum += 0x9e3779b9ul;
        e = (sum >> 2) & 3;
        for (r = 0; r <= n; r++) {
            // round
            y = data[(r+1) % (n + 1)]; // right neighbour
            data[r] += ((z>>5 ^ y<<2) + (y>>3 ^ z<<4)) ^ ((sum^y) + (((uint32_t*) key)[(r^e) & 3] ^ z));
            z = data[r]; // left neighbour for the next round
        }
    } while (--i);
}

void xxtea_Decrypt(uint32_t * data, uint8_t dataLength)
{
    uint32_t sum, z, y, e;
    int16_t i = 6 + 52/(dataLength/4), r;
    const int16_t n = dataLength / 4;

    sum = (uint32_t) ((uint64_t) (6 + 52/(dataLength/4)) * 0x9e3779b9ul);
    y = data[0];
    do {
        // cycle
        e = sum >> 2;
        for (r = n-1; r >= 0; --r) {
            // round
            z = data[(r+n-1) % n];
            data[r] -= ((z>>5 ^ y<<2) + (y>>3 ^ z<<4)) ^ ((sum^y) + (((uint32_t*) key)[(r^e) & 3] ^ z));
            y = data[r];
        }
        sum -= 0x9e3779b9;
    } while (--i);
}