/*  Telsys CLI client
 *
 *  Copyright (C) 2017-2018  Carl Philipp Klemm <carl@uvos.xyz>
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 3 as published by
 *  the Free Software Foundation.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <stdio.h>   /*fprintf and printf*/
#include <stdbool.h> /*for true and false defines*/
#include <stdint.h>  /*uintX_t and friends*/
#include <signal.h>  /*interupt singal handeling*/
#include <unistd.h>  /*sleep()*/


#include "gattlib.h" /*gattlib helps to keep this under 5 pages*/

/*microcontoler details*/
#define TIMER_PRESCALER 0
#define TIMER_CLOCK_FREQ 32768 //Hz

/*UUIDs*/
#define SERVICE_UUID "66410001-b565-4284-d0a8-54775812af9e"
#define TX_UUID      "66410002-b565-4284-d0a8-54775812af9e"
#define ADC_UUID     "66410003-b565-4284-d0a8-54775812af9e"
#define AUX_UUID     "66410004-b565-4284-d0a8-54775812af9e"

static uuid_t txUuid;
static uuid_t adcUuid;
static uuid_t auxUuid;

/*Sample numbering heads*/
static uint64_t currentAdcSampleId = 0;
static uint64_t currentAuxSampleId = 0;
static uint64_t auxTimeStampHead   = 0;
static uint64_t adcTimeStampHead   = 0;

bool stop = false;

/*Handler to trap interupts*/
void intHandler(int dummy) 
{
    stop = true;
}

/*structs for storing samples*/
typedef struct AdcSample
{
    uint16_t value;
    uint64_t timeStamp;
    uint64_t id;
    uint8_t  channel;
} AdcSample;

typedef struct AuxSample
{
    int16_t accelx;
    int16_t accely;
    int16_t accelz;
    uint8_t temperature;
    uint64_t timeStamp;
    uint64_t id;
} AuxSample;

/*Stdout output formating*/
void print_adc_sample(const AdcSample sample)
{
     printf("ADC, %u, %lu, %lu, %u\n", sample.channel, sample.id, sample.timeStamp, sample.value);
}

void print_aux_sample(const AuxSample sample)
{
    printf("AUX, %lu, %lu, %d, %d, %d, %u\n", sample.id, sample.timeStamp, sample.accelx, sample.accely, sample.accely, sample.temperature); 
}

/*Data helper functions.*/
uint32_t uc_ticks_to_us(const uint16_t ticks)
{
    return ticks*((TIMER_PRESCALER+1)*1000000) / TIMER_CLOCK_FREQ;
}

uint16_t to_equivalent_uint16(const uint8_t *data)
{
    uint16_t result = data[1];
    result = result | (data[0] << 8);
    return result;
}

void decode_adc_paket(const uint8_t* data, size_t length)
{
    if(length > 4)
    {
        int expectedCount = data[0];
        uint_fast8_t totalCount = expectedCount;
        uint32_t currentDelta = uc_ticks_to_us(to_equivalent_uint16(data+1))*totalCount;
        uint8_t channel = data[length-1];
        for(size_t i = 3; i + 2 <= length && expectedCount > 0; i+=2)
        {
            AdcSample tmp;
            tmp.value = to_equivalent_uint16(data+i);
            tmp.id = currentAdcSampleId;
            tmp.channel = channel;
            tmp.timeStamp=adcTimeStampHead+(currentDelta/totalCount)*(totalCount-expectedCount);

            print_adc_sample(tmp);

            expectedCount--;
            currentAdcSampleId++;
        }
        adcTimeStampHead = adcTimeStampHead+currentDelta;
    }
}

void decode_aux_paket(const uint8_t* data, size_t length)
{
    if(length >= 15 )
    {
        AuxSample tmp;
        tmp.timeStamp = auxTimeStampHead;
        tmp.accelx = to_equivalent_uint16(data+2);
        tmp.accely = to_equivalent_uint16(data+4);
        tmp.accelz = to_equivalent_uint16(data+6);
        tmp.temperature = data[14];
        tmp.id = currentAuxSampleId;

        ++currentAuxSampleId;
        auxTimeStampHead += uc_ticks_to_us(to_equivalent_uint16(data));

        print_aux_sample(tmp);
    }
}

/*Callback for incomeing notifications*/
void notification_handler(const uuid_t* uuid, const uint8_t* data, size_t length,  void* instanceVoid)
{
    printf("got data\n");
    if(gattlib_uuid_cmp(uuid, &adcUuid) == 0)      decode_adc_paket(data, length);
    else if(gattlib_uuid_cmp(uuid, &auxUuid) == 0) decode_aux_paket(data, length);
}

void register_and_enable_notification(gatt_connection_t* connection)
{
    uint16_t enable_notification = 0x0001;
    sleep(5);
    gattlib_write_char_by_uuid(connection, &txUuid, &enable_notification, sizeof(enable_notification));
    gattlib_register_notification(connection, &notification_handler, NULL);
    gattlib_notification_start(connection, &adcUuid);
    
    int ret = gattlib_notification_start(connection, &adcUuid);
    if (ret) fprintf(stderr, "Fail to start adc notification.\n");
    ret =  gattlib_notification_start(connection, &auxUuid);
    if (ret) fprintf(stderr, "Fail to start aux notification.\n");
       
}

bool characteristic_discover(gatt_connection_t* connection)
{
    gattlib_characteristic_t* characteristics;
    int characteristicsLength;
    int errorCode;
    bool foundService = false;
    /*Discover all characteristics*/
    errorCode = gattlib_discover_char(connection, &characteristics, &characteristicsLength);
    if(errorCode)
    {
        fprintf(stderr, "Can not discover Characteristics\n");
        return false;
    }
    else
    {
        /*Check if Telsys tx characteristics is amoung them*/
        for (int i = 0; i < characteristicsLength; i++)
        {
            if(gattlib_uuid_cmp(&characteristics[i].uuid, &txUuid) == 0) foundService = true;
        }
        if(foundService == false) 
        {
            fprintf(stderr, "Device Dose not Support Requierd services.\n");
            return false;
        }
    }
    return true;
}

void print_help(char *argv[]) 
{
    printf("Telsys CLI client. useage: \n%s <device address>\n", argv[0]);
}

int main(int argc, char *argv[]) 
{
    /*Test if argument count is correct*/
    if(argc != 2) 
    {
        print_help(argv);
        return 1;
    }
    
    /*Register interupt handler*/
    signal(SIGINT, intHandler);
    signal(SIGTERM, intHandler);
    
    printf("Populate\n");
    /*Populate gattlib uuid objects*/
    gattlib_string_to_uuid(TX_UUID, strlen(TX_UUID), &txUuid);
    gattlib_string_to_uuid(ADC_UUID, strlen(ADC_UUID), &adcUuid);
    gattlib_string_to_uuid(AUX_UUID, strlen(AUX_UUID), &auxUuid);
    
    /*Peform connection*/
    printf("Connect\n");
    gatt_connection_t* connection = NULL;
    connection=gattlib_connect(NULL, argv[1], BDADDR_LE_RANDOM, BT_SEC_LOW, 0, 0);
    if(connection == NULL)
    {
        fprintf(stderr, "Can not connect to device\n");
        return 2;
    }
    
    /*check for characteristics, enable notification*/
    printf("Discover\n");
    if(!characteristic_discover(connection)) 
    {
        gattlib_disconnect(connection);
        return 3;
    }
    /*register notification callback*/
    else 
    {
        printf("Register\n");
        register_and_enable_notification(connection);
    }
    
    while(!stop) 
    {
        printf("Loop\n");
        gattlib_write_char_by_uuid(connection, &txUuid, "on\n", sizeof("on"));
        sleep(1);
    }
    gattlib_disconnect(connection);
    return 0;
}