#include <sys/time.h>
#include <stdlib.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <math.h>

#include "D2XXSerial.h"


/*************************************************************/


D2XXSerial::D2XXSerial(unsigned int index) : devIndex(index) 
{
    shuttingDown = false;
    th_id = 0;
    ftHandle = 0;
    pthread_mutex_init(&data_mtx,NULL);
    pthread_cond_init(&data_cond,NULL);
    iNumDev = 0;
    	
    FT_STATUS	ftStatus;
	char * 	pcBufLD[MAX_DEVICES + 1];
	for(unsigned int i = 0; i < MAX_DEVICES; i++) {
		pcBufLD[i] = cBufLD[i];
	}
	pcBufLD[MAX_DEVICES] = NULL;
    ftStatus = FT_ListDevices(pcBufLD, &iNumDev, FT_LIST_ALL | FT_OPEN_BY_SERIAL_NUMBER);
    if (ftStatus != FT_OK) {
        printf("Error FT_ListDevices(%d)\n", (int)ftStatus);
    }
}

D2XXSerial::~D2XXSerial()
{
    if (ftHandle) {
        Close();
    }
}

void *D2XXSerial::static_read_thread(void *arg) {
    D2XXSerial *that = (D2XXSerial*)arg;
    that->reading_loop();
    return NULL;
}

void D2XXSerial::reading_loop() {
    while (!shuttingDown) {
        FT_STATUS	ftStatus;
        unsigned char buffer[256];
        DWORD dwRxSize = 0 ;
        ftStatus = FT_Read(ftHandle, buffer, 256, &dwRxSize);
        if (ftStatus != FT_OK) {
			printf("Error FT_GetQueueStatus(%d)\n", (int)ftStatus);	
            // TODO: shutdown? reopen?
        }
        pthread_mutex_lock(&data_mtx);
        for (DWORD i=0;i<dwRxSize;i++) {
            data.push_back(buffer[i]);
        }
        while (data.size() > 10000) {
            data.pop_front();
        }
        if (data.size()) {
            this->process_data();
        }
        pthread_mutex_lock(&data_mtx);
    }
}

void D2XXSerial::process_data() {
    pthread_cond_broadcast(&data_cond);
}

bool D2XXSerial::Open(unsigned int speed)
{
    FT_STATUS	ftStatus;
    if (th_id) {
        printf("Error: D2XXSerial::Open: receiving thread is already running\n");
        return false;
    }

    if ((ftStatus = FT_Open(devIndex, &ftHandle)) != FT_OK) {
        printf("Error FT_Open(%d), cBufLD[i] = %s\n", (int)ftStatus, cBufLD[devIndex]);
        return false;
    }

    if((ftStatus = FT_SetBaudRate(ftHandle, speed)) != FT_OK) {
        printf("Error FT_SetBaudRate(%d), cBufLD[i] = %s\n", (int)ftStatus, cBufLD[devIndex]);
        return false;
    }
	FT_SetTimeouts(ftHandle, 100, 100);	// 0.1 second read timeout

    EmptyBuffers();

    pthread_create(&th_id,NULL,static_read_thread,this);

    return true;
}

bool D2XXSerial::Close()
{
    if (th_id) {
        shuttingDown = true;
        pthread_cancel(th_id);
        pthread_join(th_id,NULL);
    }
    th_id = 0;

    FT_Close(ftHandle);
    ftHandle = 0;
    return true;
}

size_t D2XXSerial::Send(const unsigned char * buffer,size_t size)
{
    FT_STATUS ftStatus;
    DWORD wes;
    if (!ftHandle) return 0;
    ftStatus = FT_Write(ftHandle, (unsigned char*)buffer, size, &wes);
    if (ftStatus != FT_OK) {
        printf("Error FT_Write(%d), cBufLD[i] = %s\n", (int)ftStatus, cBufLD[devIndex]);
        return 0;
    }
    return wes;
}
    
size_t D2XXSerial::Receive(unsigned char * buffer, size_t size)
{
    size_t w = 0;
    pthread_mutex_lock(&data_mtx);
    for (w=0;w<std::min(size,data.size());w++) {
        buffer[w] = data.front();
        data.pop_front();
    }
    pthread_mutex_unlock(&data_mtx);
    return w;
}


bool D2XXSerial::EmptyBuffers()
{
    pthread_mutex_lock(&data_mtx);
    data.clear();
    pthread_mutex_unlock(&data_mtx);
    return true;
}

bool D2XXSerial::WaitData(size_t millisec)
{
    struct timespec ts;
    bool data_avalaible = false;
    pthread_mutex_lock(&data_mtx);
    clock_gettime(CLOCK_REALTIME, &ts);
    double timeout = ts.tv_sec + 1e-9*ts.tv_nsec + 1e-3*millisec;
    while (data.size() == 0) {
        clock_gettime(CLOCK_REALTIME, &ts);
        double remaining = timeout - (ts.tv_sec + 1e-9*ts.tv_nsec);
        if (remaining <= 0.0) {
            break;
        }
        ts.tv_sec = (unsigned long)floor(timeout);
        ts.tv_nsec = (unsigned long)floor((timeout - ts.tv_sec)*1e9);
        // Release mutex and wait a bit
        pthread_cond_timedwait(&data_cond, &data_mtx, &ts);
        // data_cond might have been triggered by a read timeout
    }
    data_avalaible = (data.size() > 0);
    pthread_mutex_unlock(&data_mtx);
    return data_avalaible;
}
        


