/* Original Author: Kris Burney & Jake Drahos
 * Last Edits: Matthew Kelly & Dane Larson
 *
 * Interface between Quad, front end, Virtual and VRPN tracker.
 * Main uses a standard socket select loop to receive data from any 
 * source described above. Based on the data received from sockets it 
 * calls receive functions based on the socket received from.
 */

#define _GNU_SOURCE
#define _BSD_SOURCE

//system includes
#include <err.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/stat.h>
//#include <bluetooth/bluetooth.h> Bluetooth not currently used
//#include <bluetooth/rfcomm.h> Bluetooth not currently used
#include <pthread.h>
#include <assert.h>
#include <errno.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <netinet/tcp.h>
#include <fcntl.h>
#include <sys/stat.h>

//user created includes
#include "commands.h"
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "packet.h"
#include "param.h"
#include "update.h"
#include "config.h"
#include "source.h"
#include "output.h"
#include "nodes.h"
#include "bitwise.h"
#include "backend_adapter.h"
#include "backend_utils.h"

#define CMD_MAX_LENGTH 4096

/* Backend-internal command magics */
#define TD_MAGIC "TRACKERDATA"

// function prototypes
void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo(int index);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );
static void safe_close_fd(int fd, pthread_mutex_t *mutexLock);

static void cb(struct ucart_vrpn_TrackerData *);
static int new_client(int fd);
/* Return index of client, or -1 */
static ssize_t get_client_index(int fd);
/* Returns pointer to client buffer, or -1 */
static char * get_client_buffer(int fd);
/* Return pointer to client pending responses, or -1*/
static int * get_client_pend_responses(int fd);
/* Return positive integer if successful, -1 otherwise */
static int clientAddPendResponses(int fd, uint16_t packet_id);
/* Return positive integer if successful, -1 otherwise */
static int clientRemovePendResponses(int fd, uint16_t packet_id);
/* Returns -1 on error */
static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
/* Receive data from quad */
static void quad_recv(int index);
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
static int wasDisconnected(int fd);
/* handle controller responses from quad to frontend */
static void handleResponse(struct metadata *m, uint8_t * data);

/* Thread-safe wrappers */
static ssize_t readQuad(char * buf, size_t count, int index);
static ssize_t writeQuad(const uint8_t * buf, size_t count);
static ssize_t writeQuadIndex(const uint8_t * buf, size_t count, int index);
static int local_comm_channel;

static unsigned int currMessageID = 0;

// global variables
static volatile int keepRunning = 1;
static char trackable[254] = "UAV";
static int backendSocket;
//TODO - need to remove this
struct ucart_vrpn_tracker * tracker = NULL;

#define MAX_CLIENTS 32
#define CLIENT_BUFFER_SIZE 1024
#define CLIENT_MAX_PENDING_RESPONSES 15
static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
static int client_fds[MAX_CLIENTS];
static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES];
fd_set rfds_master;
int max_fd = 0;

static FILE * quadlog_file;
static int quadlog_file_open = 0;
static char user_specified_log_name[256] = "";

static void sig_handler(int s) {
    printf("Caught SIGPIPE from quad fifo..\n");
}

/**
 * Handler function for exit signals. This allows the backend to properly close.
 */
static void sig_exit_handler(int s) {
    printf("Exiting with condition %d\n", s);
    keepRunning = 0;
}

// Callback to be ran whenever the tracker receives data.
// Currently doing much more than it should. It will be slimmed down 
// in the future.
static void cb(struct ucart_vrpn_TrackerData * td) {
    static int count = 0;
    sendVrpnPacket(td);
    count++;
}

int main(int argc, char **argv)
{
    int activity;
    int i,j;

    //Setup exit signals
    signal(SIGINT, sig_exit_handler);
    signal(SIGABRT, sig_exit_handler);
    signal(SIGQUIT, sig_exit_handler);
    signal(SIGTERM, sig_exit_handler);

    FD_ZERO(&rfds_master);

    /* Determine socket path */
    char * backend_socket_path = DEFAULT_SOCKET;
    if (getenv(SOCKET_ENV)) {
        backend_socket_path = getenv(SOCKET_ENV);
    }

    /* Unlink if it exists */
    unlink(backend_socket_path);

    /* Create socket */
    mode_t old_umask = umask(0111);
    backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
    if (backendSocket < 0) {
        err(-1, "socket");
    }

    printf("backendSocket = %d\n", backendSocket);

    /* Create sockaddr and bind */
    struct sockaddr_un sa;
    sa.sun_family = AF_UNIX;
    strncpy(sa.sun_path, backend_socket_path, 107);
    sa.sun_path[107] = '\0';
    if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
        err(-1, "bind");
    }
    umask(old_umask);

    /* Listen */
    if (listen(backendSocket, 16)) {
        err(-1, "listen");
    }

    /* Add to socket set */
    safe_fd_set(backendSocket, &rfds_master, &max_fd);

    /* Initialize client buffers */
    for (i = 0; i < MAX_CLIENTS; i++) {
        client_fds[i] = -1;
        client_buffers[i][0] = '\n';
        for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) {
            client_pending_responses[i][j] = -1;
        }
    }

    //Socket and communication setup
    for (i = 0; i < NUM_TRACKABLES; i++) {
        //Lock the trackable communication mutex
        if (pthread_mutex_lock(&(trackables[i].socket_mutex))) {
            err(-2, "pthread_mutex_lock (%s:%d):", __FILE__, __LINE__);
        }
        //Check for trackable type
        if (trackables[i].isAdapter == 0) {
            if ((trackables[i].socket = connectToZybo(i)) < 0) {
                perror("Error connecting to Quad...");
                break;
            }
            if (!local_comm_channel) {
            //if (trackables[i].isLocal == 0) {    
                printf("zyboSocket = %d\n", trackables[i].socket);
            } else {
                signal(SIGPIPE, sig_handler);
                printf("zybo_fifo_rx = %d\nzybo_fifo_tx = %d\n", trackables[i].fifo_rx, trackables[i].fifo_tx);
            }
        } else {
            printf("Starting client socket: %s\n", trackables[i].name);
            if (adapterConnect(&(trackables[i])) != 0) {
                perror("Error connecting to Adapter...");
                break;
            }
        }
        //Unlock the trackable communication mutex
        if (pthread_mutex_unlock(&(trackables[i].socket_mutex))) {
            err(-2, "pthread_mutex_unlock (%s:%d):", __FILE__, __LINE__);
        }
    }
    if (i != NUM_TRACKABLES) {
        for (j = 0; j < (i-1); j++) {
            if (trackables[j].isAdapter == 1) {
                adapterDisconnect(&(trackables[j]));
            }
            //else if (trackables[j].isLocal == 1) {
            else if (local_comm_channel) {
                safe_close_fd(trackables[j].fifo_rx, &trackables[j].socket_mutex);
                safe_close_fd(trackables[j].fifo_tx, &trackables[j].socket_mutex);
            } 
            else {
                if (!getenv(NOQUAD_ENV)) {
                    safe_close_fd(trackables[j].socket, &trackables[j].socket_mutex);
                }
            }
        }
        exit(1);
    }

    // watch for input from stdin (fd 0) to see when it has input
    safe_fd_set(fileno(stdin), &rfds_master, &max_fd);

    //Setup safe fd set for all trackables
    for (i = 0; i < NUM_TRACKABLES; i++) {
        if (trackables[i].isAdapter == 0) {
            if (!getenv(NOQUAD_ENV)) {
                // watch for input from the zybo socket
                safe_fd_set(trackables[i].socket, &rfds_master, &max_fd);
            }
        } else {
            // watch for input from the adapter sockets
            safe_fd_set(trackables[i].socket, &rfds_master, &max_fd);
        }
    }

    //Check for a user specified log name
    if(argc >= 2) {
        strncat(user_specified_log_name, argv[1], strlen(argv[1]));
    }

    //Create VRPN trackers if using environment variable not set
    if(!getenv(NOVRPN_ENV)){
        printf("Creating VRPN trackers...\n");
        for (i = 0; i < NUM_TRACKABLES; i++) {
            //create vrpnTracker instance
            trackables[i].tracker = ucart_vrpn_tracker_createInstance(trackables[i].server_name);
            // this function will be called whenever tracker receives data
            ucart_vrpn_tracker_addCallback(trackables[i].tracker, cb);
        }    
        //TODO - remove tracker instance
        tracker = trackables[0].tracker;
        printf("Attempting to Start VRPN thread... ");
        if (ucart_vrpn_start()) {
            warnx("Failed\n");
        }
        else {
            printf("Success\n");
        }
    } else {
        printf("Ignoring VRPN information...\n");
    }

    struct timeval timeout = {
        .tv_sec = 1,
        .tv_usec = 0
    };

    //Main backend loop
    while(keepRunning) {
        fd_set rfds;
        rfds = rfds_master;
        activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
        if(activity == -1) {
            perror("select() ");
        } else if (activity) {
            for(int fd = 0; fd <= max_fd; ++fd) {
                if (FD_ISSET(fd, &rfds)) {
                    if(wasDisconnected(fd)){
                        break;
                    }
                    if (fd == fileno(stdin)) {
                        /**
                         * Ignore stdin from the backend
                         */
                    } else if (fd == backendSocket) {
                        int new_fd = 0;
                        new_fd = accept(backendSocket, NULL, NULL);
                        if (new_fd < 0) {
                            warn("accept");
                        } else {
                            printf("Connection\n");
                            if (new_client(new_fd)) {
                                printf("Added client\n");
                                safe_fd_set(new_fd, &rfds_master, &max_fd);
                            }
                        }
                    } else if (get_client_index(fd) > -1) {
                        /* It is a socket to a frontend */
                        client_recv(fd);
                    } else {
                        for (i = 0; i < NUM_TRACKABLES; i++) {
                            if (fd == trackables[i].socket) {
                                quad_recv(i);
                            }
                        }
                    }
                }
            }
        } else {
            timeout.tv_sec = 1;
            timeout.tv_usec = 0;
        }
    }

    // Free VRPN trackers and stop VRPN Tracker Thread
    if(!getenv(NOVRPN_ENV)){
        if (ucart_vrpn_stop()) {
            warnx("VRPN thread stop failed...\n");
        } else {
            printf("VRPN thread stopped\n");
        }
        for (i = 0; i < NUM_TRACKABLES; i++) {
            ucart_vrpn_tracker_freeInstance(trackables[i].tracker);
        }
    }
 
    //Disconnecting from trackables
    printf("Disconnecting from Trackables\n");
    for (i = 0; i < NUM_TRACKABLES; i++) {
        if (trackables[i].isAdapter == 1) {
            adapterDisconnect(&(trackables[i]));
        }
        //else if (trackables[i].isLocal == 1) {
        else if (local_comm_channel) {
            safe_close_fd(trackables[i].fifo_rx, &trackables[i].socket_mutex);
            safe_close_fd(trackables[i].fifo_tx, &trackables[i].socket_mutex);
        } 
        else {
            if (!getenv(NOQUAD_ENV)) {
                safe_close_fd(trackables[i].socket, &trackables[i].socket_mutex);
            }
        }
    }

    //Close log file if one was open
    if (quadlog_file_open) {
        fclose(quadlog_file);
    }
    
    return 0;
}

void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
    uint8_t packet[64];
    struct metadata m;
    uint8_t data[128];

    struct position_update u;
    u.id = currMessageID;
    u.x = info->x;
    u.y = info->y;
    u.z = info->z;
    u.pitch = info->pitch;
    u.roll = info->roll;
    u.yaw = info->yaw;

    if (EncodeUpdate(&m, data, 128, &u) < 0) {
        warnx("Big problems. sendVrpnPacket . EncodeUpdate");
        return;
    }
    m.msg_id = currMessageID++;

    ssize_t psize;
    if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
        warnx("Big problems. sendVrpnPacket. EncodePacket");
        return;
    }

    for (int i = 0; i < NUM_TRACKABLES; i++) {
        if (strcmp(trackables[i].server_name, info->server_name) == 0) {
            writeQuadIndex(packet, psize, i);
            break;
        }
    }
}

void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
    int status;
#if VRPN_DEBUG_PRINT == 1
    for (int i = 0; i < NUM_TRACKABLES; i++) {
        if (strcmp(trackable, trackables[i].name) == 0) {
            break;
        }
    }
#endif
    //if ((status = ucart_vrpn_tracker_getData(trackables[i].tracker, td)) < 0) {
    // TODO - remove tracker instance
    if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0) {
        perror("Error receiving VRPN data from tracker...");
        keepRunning = 0;
    }
}

/* void getVRPNPacket(struct ucart_vrpn_TrackerData *td, int index) {
    int status;
    if ((status = ucart_vrpn_tracker_getData(trackables[i].tracker, td)) < 0) {
        perror("Error receiving VRPN data from tracker...");
        keepRunning = 0;
    }
} */

void printVrpnData(struct ucart_vrpn_TrackerData * td) {
    printf("FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
        td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
}

int connectToZybo(int index) {
    int sock;
    int status = -1;

    if (getenv(NOQUAD_ENV)) {
        return 0;
    }

    // Check for local comm channel or bluetooth
    //if (trackables[index].isLocal == 1) {
    if (getenv(QUAD_COMM_ENV) && strcmp(getenv(QUAD_COMM_ENV), "local") == 0) {
        printf("Using Local Fifo Settings\n");
        local_comm_channel = 1;
        char * fifo_rx = trackables[index].fifo_tx_path;
        char * fifo_tx = trackables[index].fifo_rx_path;
            
        trackables[index].fifo_tx = open(fifo_tx, O_WRONLY | O_NONBLOCK);
        if (trackables[index].fifo_tx < 0) {
            warnx("Open zybo_fifo_tx...");
            return -1;
        }

        trackables[index].fifo_rx = open(fifo_rx, O_RDWR | O_NONBLOCK);
        if (trackables[index].fifo_rx < 0) {
            warnx("Opening zybo_fifo_rx...");
            return -1;
        }
        status = 0;
        sock = trackables[index].fifo_rx;
    }
    /*else if (trackables[index].isBluetooth == 1) { //Bluetooth functionality currently disabled
        printf("Using BT Settings\n");
        struct sockaddr_rc addr;

        //allocate a socket
        sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

        //set the connection params ie. who connect to
        addr.rc_family = AF_BLUETOOTH;
        addr.rc_channel = trackables[index].bt_channel;
        str2ba( trackables[index].bt_addr, &addr.rc_bdaddr );

        printf("Attempting to connect to zybo. Please be patient...\n");
        // blocking call to connect to socket sock ie. zybo board
        status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
    }*/
    else {
        printf("Using WIFI settings\n");
        struct sockaddr_in addr;

        addr.sin_family = AF_INET;

        /* Quick and Dirty */
        if (!inet_aton(trackables[index].quad_ip, &addr.sin_addr)) {
            printf("Default IP %s is invalid\n",
            trackables[index].quad_ip);
            return -1;
        }

        /* Quick 'n dirty, oh yeah! */
        addr.sin_port = htons(trackables[index].quad_port);

        sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
        if (sock < 0) {
            perror("socket");
            return -1;
        }
        printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));

        status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));

        int flag = 1;
        int result = setsockopt(sock,    /* socket affected */
                           IPPROTO_TCP,     /* set option at TCP level */
                           TCP_NODELAY,     /* name of option */
                           (char *) &flag,  /* the cast is historical cruft */
                           sizeof(int));    /* length of option value */
    }

    // connection failed
    if(status < 0) {
        close(sock);
        perror("connect");
        return -1;
    } else {
        printf("connection successful!...\n");
        return sock;
    }
}

/* add a fd to fd_set, and update max_fd */
int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
    assert(max_fd != NULL);

    FD_SET(fd, fds);
    if (fd > *max_fd) {
        *max_fd = fd;
    }
    return 0;
}

/* clear fd from fds, update max fd if needed */
int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
    assert(max_fd != NULL);

    FD_CLR(fd, fds);
    if (fd == *max_fd) {
        (*max_fd)--;
    }
    return 0;
}

static ssize_t writeQuad(const uint8_t * buf, size_t count) {
    ssize_t retval;
    int index = 0;
    for (int i = 0; i < NUM_TRACKABLES; i++) {
        if (strcmp(trackables[i].name, trackable) == 0) {
            index = i;
#if BACKEND_DEBUG_PRINT == 1
            printf("Index: %d\tName: %s\n", index, trackables[index].name);
            printf("packetToQuad = '");
            for(int i = 0; i < (int)count; ++i) {
                printf(" %.2x ", buf[i]);
            }
            printf("'\n");
#endif
            return writeQuadIndex(buf, count, index);
        }
    }
    //FIXME - Should never reach here
    return -1;
}

static ssize_t writeQuadIndex(const uint8_t * buf, size_t count, int index) {
    ssize_t retval;
    
#if BACKEND_DEBUG_PRINT == 1
    printf("packetToQuad = '");
    for(int i = 0; i < (int)count; ++i) {
        printf(" %.2x ", buf[i]);
    }
    printf("'\n");
#endif

    if (trackables[index].isAdapter == 0 && getenv(NOQUAD_ENV)) {
        return count;
    }

    if (pthread_mutex_lock(&trackables[index].socket_mutex)) {
        err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
    }

    //if (trackables[index].isLocal == 1) {
    if (trackables[index].isAdapter == 0 && local_comm_channel) {
        retval = write(trackables[index].fifo_tx, buf, count);
    } else {
        if (trackables[index].isAdapter == 0) {
            retval = write(trackables[index].socket, buf, count);
        } else {
            retval = adapterWrite(trackables[index].conn, (const char *) buf, count);
        }
    }

    if (pthread_mutex_unlock(&trackables[index].socket_mutex)) {
        err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
    }

    return retval;
}

static ssize_t readQuad(char * buf, size_t count, int index) {
    ssize_t retval;
    if (trackables[index].isAdapter == 0 && getenv(NOQUAD_ENV)) {
        return count;
    }
    if (pthread_mutex_lock(&trackables[index].socket_mutex)) {
        err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
    }

    retval = read(trackables[index].socket, buf, count);

    if (pthread_mutex_unlock(&trackables[index].socket_mutex)) {
        err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
    }
    return retval;
}

static int new_client(int fd) {
    ssize_t new_slot = -1;
    for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
        if (client_fds[i] < 0) {
            new_slot = i;
            break;
        }
    }
    if (new_slot == -1) {
        warnx("Ran out of room! Consider increasing MAX_CLIENTS!");
        return 0;
    }

    client_fds[new_slot] = fd;
    client_buffers[new_slot][0] = '\0';

    return 1;
}

static ssize_t get_client_index(int fd) {
    for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
        if (client_fds[i] == fd) {
            return i;
        }
    }

    return -1;
}

static char * get_client_buffer(int fd) {
    ssize_t slot = get_client_index(fd);
    if (slot == -1) {
        return NULL;
    } else {
        return client_buffers[slot];
    }
}

static int * get_client_pend_responses(int fd) {
    ssize_t slot = get_client_index(fd);
    if (slot == -1) {
        return NULL;
    } else {
        return client_pending_responses[slot];
    }
}

static int clientAddPendResponses(int fd, uint16_t packet_id) {
    int *pendingResponses = get_client_pend_responses(fd);
    for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
        if(pendingResponses[i] == -1) {
            pendingResponses[i] = packet_id;
            return i;
        }
    }
    return -1;
}

static int clientRemovePendResponses(int fd, uint16_t packet_id) {
    int *pendingResponses = get_client_pend_responses(fd);
    for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
        if(pendingResponses[i] == packet_id) {
            pendingResponses[i] = -1;
            return i;
        }
    }
    return -1;
}

static int remove_client(int fd) {
    ssize_t slot = get_client_index(fd);
    if(slot == -1)
        return -1;
    char *clientBuffer = get_client_buffer(fd);
    if(clientBuffer == NULL)
        return -1;
    clientBuffer[0] = '\0';
    int *pendingResponses = get_client_pend_responses(fd);
    if(pendingResponses == NULL)
        return -1;
    for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
        pendingResponses[i] = -1;
    }
    client_fds[slot] = -1;
    return 0;
}

static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
    if (pthread_mutex_lock(mutexLock)) {
        err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
    }
    close(fd);
    if (pthread_mutex_unlock(mutexLock)) {
        err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
    }
}

static void client_recv(int fd) {
    char * buffer;
    ssize_t len_pre;
    char cmdString[64];
    char * cursor;
    ssize_t r;
    int index = 0;


    buffer = get_client_buffer(fd);
    len_pre = strlen(buffer);
    cursor = buffer + len_pre;

    r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
    if (r < 0) {
        warn("read (fd: %d)", fd);
    }
    buffer[len_pre + r] = '\0';

    /* Parse buffer and handle commands */
    while (1) {
        /* not using strtok because reasons */
        size_t len = strlen(buffer);
        ssize_t newline = -1;
        for (size_t i = 0; i < len; i++) {
            if (buffer[i] == '\n') {
                newline = i;
                break;
            }
        }

        /* No newline found. End parsing */
        if (newline == -1) {
            break;
        }
        buffer[newline] = '\0';
        printf("Client(%d) : '%s'\n",fd, buffer);

        char * first_word;
        char * tmp = strdup(buffer);
#if BACKEND_DEBUG_PRINT == 1
        printf("tmpbuff = '%s'\n", tmp);
#endif
        first_word = strtok(tmp, " ");
#if BACKEND_DEBUG_PRINT == 1
        printf("first word = '%s'\n", first_word);
#endif
        ssize_t msg_type, i;
        for (i = 0; i < MAX_TYPE_ID; ++i) {
            if ((msg_type = findCommand(first_word)) != -1)
                break;
        }

        free(tmp);

        if (msg_type == -1) {
            /* buffer was not a quad command, handling internally to
             * backend instead of forwarding to quad
             */
            if (strncmp(buffer, TD_MAGIC, strlen(TD_MAGIC)) == 0) {
                /* Request for tracker data */
                struct ucart_vrpn_TrackerData td;
                // TODO - check tracker of a specific trackable, also implement a request
                // that can get the vrpn of a specific trackable
                if (tracker == NULL) {
                    char * dummy = TD_MAGIC " 1.0 2.0 3.0 4.0 5.0 6.0\n";
                    write(fd, dummy, strlen(dummy));
                } else if (ucart_vrpn_tracker_getData(tracker, &td)) {
                    write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
                } else {
                    /* more than sufficient buffer */
                    char buffer[2048];
                    /* Map VRPN XYZ to Height Lat Long (right now it's
                     * a guess). Format is Height Lat Long P R Y */
                    if (snprintf(buffer,
                            2048,
                            TD_MAGIC " %lf %lf %lf %lf %lf %lf\n",
                            td.z,
                            td.y,
                            td.x,
                            td.pitch,
                            td.roll,
                            td.yaw) >= 2048) {

                        /* Output longer than buffer */
                        warnx("Increase format buffer size, output was too long!");
                        write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
                    }
                    write(fd, buffer, strlen(buffer));
                }
            }
            else if (strncmp(buffer, "gettrackable", strlen("gettrackable")) == 0) {
            char buffer[256];
                if (snprintf(buffer,
                    256,
                    "gettrackable %s\n",
                    trackable) >= 256) {
                    warnx("Increase format buffer size, output was too long!");
                    write(fd, "gettrackable ERROR\n", strlen("gettrackable ERROR\n"));
                }
                write(fd, buffer, strlen(buffer));  
            }
            else if (strncmp(buffer, "settrackable", strlen("settrackable")) == 0) {
                char check[256];
                sscanf(buffer,
                    "settrackable %s",
                    check);
                if (strcmp(check,"(null)") != 0) {
                    for (i = 0; i < NUM_TRACKABLES; i++) {
                        if (strcmp(trackables[i].name, check) == 0) {
                            sscanf(buffer,
                                "settrackable %s",
                                trackable);
                            break;
                        }
                    }
                }
            }

        } else {
            uint8_t packet[64];
            struct metadata m;
            uint8_t *data = malloc(sizeof(*data) * 128);
            ssize_t result;
            ssize_t psize;

            printf(" found a msg_type of %ld\n", msg_type);

            switch (msg_type) {
                case SETPARAM_ID:
                    result = EncodeSetParam(&m, data, 128, buffer);
                    break;
                case GETPARAM_ID:
                    result = EncodeGetParam(&m, data, 128, buffer);
                    break;
                case SETSOURCE_ID:
                    result = EncodeSetSource(&m, data, 128, buffer);
                    break;
                case GETSOURCE_ID:
                    result = EncodeGetSource(&m, data, 128, buffer);
                    break;
                case GETOUTPUT_ID:
                    result = EncodeGetOutput(&m, data, 128, buffer);
                    break;
                case GETNODES_ID:
                    result = EncodeGetNodes(&m, data, 128, buffer);
                    break;
                case ADDNODE_ID:
                    result = EncodeAddNode(&m, data, 128, buffer);
                    break;
                case OUTPUT_OVERRIDE_ID:
		    result = EncodeSetOutputOverride(&m, data, 128, buffer);
		    break;

                default:
                    result = -1;
                    break;
            }

            if (result < 0) {
                warnx("Big problems. client_recv. EncodeMetaData");
                free(data);
                return;
            }

            m.msg_id = currMessageID++;

            if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
                warnx("Big problems. client_recv. EncodePacket");
                free(data);
                return;
            }

            /* Only add the client to the pending responses if it was a getparam command */
            if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID ||
                m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
                if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
                    warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
                }
            }


            int retval = writeQuad(packet, psize);
            // printf("sent %d bytes\n", retval);

            free(data);
        }

        char * rest = &buffer[newline] + 1;
        size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
        /* Delete parsed data and move the rest to the left */
        memmove(buffer, rest, restLen +1);
    }
}

static void quad_recv(int index) {
    static unsigned char respBuf[CMD_MAX_LENGTH];
    static size_t respBufLen;
    static int receiving_logs; 

    struct metadata m;
    uint8_t data[CMD_MAX_LENGTH];
    size_t respLen;
    ssize_t datalen;
    size_t packetlen;

    respLen = readQuad((char *) respBuf + respBufLen,
            CMD_MAX_LENGTH - respBufLen, index);
    if (respLen <= 0) {
        perror("ERROR reading from quad...\n");
        respBufLen = 0;
        return;
    }
    respBufLen += respLen;

#if BACKEND_DEBUG_PRINT == 1
    printf("packetFromQuad = '");
    for(int i = 0; i < (int)respBufLen; ++i) {
        printf(" %.2x ", respBuf[i]);
    }
    printf("'\n");
#endif

    while(respBufLen) {
        datalen = DecodePacket(&m, data, CMD_MAX_LENGTH, respBuf, respBufLen);
        if (datalen == -1) {
            warnx("No start Byte");
            for (size_t i = 0; i < respBufLen; ++i) {
                if (respBuf[i] == BEGIN_CHAR) {
                    memmove(respBuf, respBuf + i, respBufLen - i);
                    respBufLen -=i;
                    return;
                }

            }
            respBufLen = 0;
            return;
        }
        if (datalen == -5) {
            warnx("Chechsum mismatch");
            for (size_t i = 0; i < respBufLen; ++i) {
                if (respBuf[i] == BEGIN_CHAR) {
                    memmove(respBuf, respBuf + i, respBufLen - i);
                    respBufLen -=i;
                    return;
                }
            }
            respBufLen = 0;
            return;
        }
        if (datalen < 0){
            /* Not enough data yet. We need to wait for more */
            return;
        }

        packetlen = PacketSize(&m);
        memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);
        respBufLen -= packetlen;

        char * debug_string;
        switch (m.msg_type) {
            case DEBUG_ID:
                /* in case of debug. Quad send null terminated string in data */
                debug_string = strndup((char *)data, m.data_len);
                printf(" (Quad) : %s\n", debug_string);
                free(debug_string);
                break;
            case LOG_ID:
                if (!quadlog_file_open) {
                    char log_file[256];
                    create_log_name(log_file, 256, user_specified_log_name);
                    printf("New log file created: '%s'\n", log_file);
                    quadlog_file = fopen(log_file, "w");
                    quadlog_file_open = 1;
                }
                if (!receiving_logs) {
                    printf("(Quad) : Log found\n");
                    receiving_logs = 1;
                } else {
                    printf(".");
                    fflush(0);
                }
                fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
                break;
            case SEND_RT_ID:
                quadlog_file = fopen("quad_log_data.txt", "w");
		//TODO Add formatting here to populate .txt file in a way QT will be able to read
		char * formatted_data = malloc(sizeof(data) + 1);
		
                fwrite((char *) formatted_data, sizeof(char), m.data_len, quadlog_file);
                fclose(quadlog_file);
		free(formatted_data);
                break;
            case RESPPARAM_ID:
            case RESPSOURCE_ID:
            case RESPOUTPUT_ID:
            case RESPNODES_ID:
            case RESPADDNODE_ID:
                handleResponse(&m, data);
                break;
            case LOG_END_ID:
                if (quadlog_file_open) {
                    fclose(quadlog_file);
                    quadlog_file_open = 0;
                }
                printf("\n(Quad) : Log End found\n");
                receiving_logs = 0;
                break;
            default:
                printf("(Backend): message type %d ignored from quad\n", m.msg_type);
                break;
        }
    }
}

static void handleResponse(struct metadata *m, uint8_t * data)
{
    ssize_t result = 0;
    char *buffer = malloc(sizeof(*buffer) * 128);
    if (!buffer) {
        warnx("failed immediatly");
        return;
    }

    switch (m->msg_type) {
        case RESPPARAM_ID:
            result = DecodeResponseParam(buffer, 128, m, data);
            break;
        case RESPSOURCE_ID:
            result = DecodeResponseSource(buffer, 128, m, data);
            break;
        case RESPOUTPUT_ID:
            result = DecodeResponseOutput(buffer, 128, m, data);
            break;
        case RESPNODES_ID:
            result = DecodeResponseGetNodes(&buffer, 128, m, data);
            break;
        case RESPADDNODE_ID:
            result = DecodeResponseAddNode(buffer, 128, m, data);
            break;
        default:
            result = -2;
            break;
    }

    if (result == -2) {
        warnx("DecodeResponse error");
        free(buffer);
        return;
    } else if (result < 0) {
        warnx("DecodeResponse error");
        return;
    }

    // printf("msg to client = '%s'\n", buffer);

    for(int fd = 0; fd <= max_fd; ++fd) {
        if (get_client_index(fd) > -1) {
            clientRemovePendResponses(fd, m->msg_id);
            write(fd, buffer, result);
        }
    }

    free(buffer);
}

static int wasDisconnected(int fd) {
    char buff;
    if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
    {
        remove_client(fd);
        safe_fd_clr(fd, &rfds_master, &max_fd);
        printf("fd %d has disconnect and was removed\n", fd);
        return 1;
    }
    return 0;
}