/* Author: Kris Burney
 *
 * BlueTooth socket program for passing vrpn data to quad.
 */
//system includes
#include <err.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <pthread.h>
#include <assert.h>

//user created includes
#include "communication.h"
#include "commands.h"
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "logger.h"

#define QUAD_BT_ADDR  "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL  0x01
#define CMD_MAX_LENGTH 1024

// function prototypes
void killHandler(int);
void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
// void *handleQuadResponse();
// void *handleCliInput();
// int atomic_check(int*, pthread_mutex_t*);
void performCommand(char *, char *);
int startsWith(const char *, const char *);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );

static void cb(struct ucart_vrpn_TrackerData *);

/* Thread-safe wrappers */
pthread_mutex_t quadSocketMutex;
static ssize_t writeQuad(const char * buf, size_t count);

// global variables
static volatile int keepRunning = 1;
const char *TRACKER_IP = "UAV@192.168.0.120:3883";
static int zyboSocket;
struct ucart_vrpn_tracker * tracker = NULL;
const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`s will be printed as is in the processed logs.\n#\n\0";

pthread_mutex_t quadResponseMutex, cliInputMutex ;
unsigned char *respBuf, *commandBuf;
int newQuadResponse = 0, newCliInput = 0;

// Structures to be used throughout
modular_structs_t structs = {};

// 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;

	if(!(count % 10)) {
		sendVrpnPacket(td);
		updateLogFile(td);
	}

	count++;
	// This will print the vrpn data to the terminal if necissary.
	// Commented out because the callback will cover quad log data
	//   at the end of flight.
	/**if(!(count % 100)) {
	 	printVrpnData(td);
	 	printf("[Info] Received %d tracker updates.\n", count);
	}**/
}


int main(int argc, char **argv)
{
	// pthread_t quadResponse, cliInput;
	fd_set rfds;
	int activity;
	int max_fd = 0;
	
	signal(SIGINT, killHandler);

	if (pthread_mutex_lock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
	}

	// if ((zyboSocket = connectToZybo()) < 0)
	// {
	// 	perror("Error connecting to Zybo...");
	// 	free(respBuf);
	// 	free(commandBuf);
	// 	exit(1);
	// }

	if (pthread_mutex_unlock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
	}
	
	// create vrpnTracker instance
	//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);

	// open the log file
	if(createLogFile(argc, argv[1]))
	{
		perror("Error creating log file...");
		exit(1);
	}
//	writeStringToLog(logHeader);


	// clear the fd set
	FD_ZERO(&rfds);
	// watch for input from stdin (fd 0) to see when it has input
	safe_fd_set(fileno(stdin), &rfds, &max_fd);
	// watch for input from the zybo socket
	//safe_fd_set(zyboSocket, &rfds, &max_fd);

	//printf("zyboSocket = %d, max_fd = %d\n", zyboSocket, max_fd);

	//tell the quad we are ready to send it vrpn data
	printf("sending Start Packet...\n");
	//sendStartPacket();


	// this function will be called whenever tracker receives data
	// ucart_vrpn_tracker_addCallback(tracker, cb);

	// start the prompt
	fprintf(stdout, "$microcart> ");
	while(keepRunning)
	{
		activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
		if(activity == -1) {
			perror("select() ");
		} else if (activity) {
			for(unsigned int fd = 0; fd <= max_fd; ++fd) {
				if (FD_ISSET(fd, &rfds)) {
					if (fd == fileno(stdin)) {
						unsigned char userCommand[CMD_MAX_LENGTH] = {};
						read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
						unsigned int cmdLen = strlen((char*) userCommand);
						// if the user simply hit enter then let them try again
						if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
						{
							// remove newline and return line chars
							if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
							userCommand[cmdLen - 1] = '\0';

							unsigned char *packet;
							formatCommand(userCommand, &packet);

							printf("received input from cli: '%s'\n", userCommand);

							fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));

							// Write the command to the control_loop socket
							// int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
							// if(n < 0) {
							// 	fprintf(stdout, "CLI: ERROR writing to socket\n");
							// }
						}
						

						fprintf(stdout, "$microcart> ");
						memset(userCommand, 0, cmdLen);
					} else if (fd == zyboSocket) {
						
					}
				}
			}
		}
	}

	// ucart_vrpn_tracker_freeInstance(tracker);
	
	if (pthread_mutex_lock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
	}
	close(zyboSocket);
	if (pthread_mutex_unlock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
	}

	closeLogFile();
	return 0;
}

// signal handler to exit while loop of main function
void killHandler(int dummy) {
	keepRunning = 0;
	printf("\nleaving Bluetooth module\n");
}

void sendStartPacket() {
	unsigned char packet[8] = {0};

	metadata_t metadata = 
	{
		BEGIN_CHAR,
		0x04,
		0x01,
		0x01,
		0
	};

	packet[0] = metadata.begin_char; // BEGIN					//PACKET_START_BYTE;
	packet[1] = metadata.msg_type;	// UPDATE				//'U'; // U for vrpn camera update, C for command
	packet[2] = metadata.msg_subtype; // BEGIN UPDATE
	packet[3] = 1; 			// MSG ID(1)
	packet[4] =	0; // MSG ID(2)
	packet[5] = 0; 			// DATALEN(1)
	packet[6] = 0; 	// DATALEN(2)
	
	char checksum = 0;
	int i;
	for(i=0; i < metadata.data_len + 7; i++)
		checksum ^= packet[i];

	packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
	int status = writeQuad((char * ) packet, metadata.data_len + 8);
	if (status != 8)
	{
		perror("Error sending start packet...\n");
		keepRunning = 0;
	}else
	{
		printf("Start packet successfuly sent...\n");
	}
}

void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
	int pSize = sizeof(info) + 8;
	int n;
	char packet[pSize];
	packet[0] = 0xBE; // BEGIN					//PACKET_START_BYTE;
	packet[1] = 0x04;	// UPDATE				//'U'; // U for vrpn camera update, C for command
	packet[2] = 0x00; // N/A
	//TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
	packet[3] = (0x00 & 0x000000ff); 			// MSG ID(1)
	packet[4] =	((0x00 >> 8) & 0x000000ff); // MSG ID(2)
	packet[5] = (sizeof(info) & 0x000000ff); 			// DATALEN(1)
	packet[6] = ((sizeof(info) >> 8) & 0x00000ff); 	// DATALEN(2)
	memcpy(&packet[7], &info, sizeof(info));

	char checksum = 0;
	int i;
	for(i=0; i < pSize - 1; i++)
		checksum ^= packet[i];

	packet[pSize - 1] = checksum; //PACKET_END_BYTE;

	n = writeQuad(packet, pSize);
	if(n < 0) {	
		perror("vrpnhandler: ERROR writing to socket");
		keepRunning = 0;
	}
}

void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
	int status;
	if((status = ucart_vrpn_tracker_getData(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 sock;
	struct sockaddr_rc addr = { -1 };

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

	//set the connection params ie. who to connect to	
	addr.rc_family = AF_BLUETOOTH;
	addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
	str2ba( QUAD_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
	int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));

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

void performCommand(char *cmdName, char * command) {
	for(int i = 0; i < NUM_COMMANDS; ++i)
	{
		if(startsWith(registeredCommands[i].commandText, command));
			fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
	}
}

int startsWith(const char *pre, const char *str) {
    size_t lenpre = strlen(pre),
           lenstr = strlen(str);
    return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
}

/* 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 char * buf, size_t count) {
	ssize_t retval;
	if (pthread_mutex_lock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
	}
	retval = writeQuad(buf, count);
	if (pthread_mutex_unlock(&quadSocketMutex)) {
		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
	}

	return retval;
}