#include <glib.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <mcrypt.h>
#include <pthread.h>

#include <string.h>

#include "gattlib.h"

static uuid_t g_uuid;
const uint8_t BT_key[4] = {0x5a, 0x68, 0x5b, 0xb6};
const uint8_t key[16] = {BT_key[0], 00, BT_key[1], 21, BT_key[2], 00, BT_key[3], 12, BT_key[2], 00, BT_key[1], 68, BT_key[0], 00, BT_key[1], 88 };
static uint8_t data[16];
int data_length = 16;

MCRYPT mcrpt;
double uV[5];
uint8_t raw_data[72];
enum Channels{
    AF3 = 0,
    T7  = 1,
    Pz  = 2,
    T8  = 3,
    AF4 = 4
};

int k = 0;

double raw(uint8_t h, uint8_t l){
    return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 + 4201.02564096001);
}

void notification_handler(const uuid_t* uuid, const uint8_t* val, size_t values_length, void* user_data) {
	int i, j, k;
    //for(i = 0; i < values_length; i++)
    //    std::cout<<(int)val[i]<<"\t";
    //std::cout<<std::endl;
    mdecrypt_generic (mcrpt, val, data_length);
    for(i = 0; i < 9; i++)
    {
        for(k = 7; k >= 0; k--)
            raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1;
    }

    for(i = 0; i < 5; i++)
    {
        uint8_t h = 0, l = 0;

        j=-1;
        while((++j) < 8)
            h = (h << 1) | ((uint8_t)(raw_data[(14 * i ) + j] ));
        j--;
        while((++j) < 14)
            l = (l << 1) | ((uint8_t)(raw_data[(14 * i ) + j]));

        uV[i] = raw(h,l);
    }

    //for (i = 0; i < data_length; i++) {
		//if(val[9] == 194 || val[9]==0)continue;
        //std::cout<<std::dec<<(int)val[9]<<" ";
		//std::cout<<std::dec<<(int)val[i]<<"\t";
	//}

    std::cout.precision(10);
	std::cout<<k<<"\t";
	for (i = 0; i < 5; i++) {
		std::cout<<uV[i]<<"\t";
	}

    /*int n=((val[1] & 0b00000011) << 5 ) + (val[2] >> 3);
    for (c = 7; c >= 0; c--)
  {
    k = n >> c;
    if (k & 1)
      printf("1");
    else
      printf("0");
  }*/
    printf("\n");
	//if(val[0]==0)printf("\n");
}

static void usage(char *argv[]) {
	printf("%s <device_address> <UUID>\n", argv[0]);
}

GMainLoop *loop;

void run(){
    loop = g_main_loop_new(NULL, 0);
    g_main_loop_run(loop);
    pthread_exit(NULL);
}

int main(int argc, char *argv[]) {

    mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL);
    mcrypt_generic_init(mcrpt, key, data_length, NULL);
	int ret;
    gatt_connection_t* connection;


	if (argc != 3) {
		usage(argv);
		return 1;
	}

	if (gattlib_string_to_uuid(argv[2], strlen(argv[2]) + 1, &g_uuid) < 0) {
		usage(argv);
		return 1;
	}
	connection = gattlib_connect(NULL, argv[1], BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0);
	if (connection == NULL) {
		fprintf(stderr, "Fail to connect to the bluetooth device.\n");
		return 1;
	}
	gattlib_register_notification(connection, notification_handler, NULL);
	ret = gattlib_notification_start(connection, &g_uuid);
	if (ret) {
		fprintf(stderr, "Fail to start notification\n.");
		return 1;
	}
    pthread_t thread;
    pthread_create(&thread, NULL,run,NULL);
	getchar();
    g_main_loop_quit(loop);
	g_main_loop_unref(loop);
	gattlib_notification_stop(connection, &g_uuid);
	gattlib_disconnect(connection);
    mcrypt_generic_deinit (mcrpt);
	mcrypt_module_close(mcrpt);
	return 0;
}