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

//user created includes
#include "communication.h"
#include "parse_packet.h"
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "commands.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();

static void cb(struct ucart_vrpn_TrackerData *);


// global variables
static volatile int keepRunning = 1;
const char *TRACKER_IP = "UAV@192.168.0.120:3883";
int quadlog_file;
int zyboSocket, status, bytes_read;
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;




// 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);
	}**/
}

void *handleQuadResponse() {
	unsigned char buffer[255];

	while(keepRunning) {
		printf("in quad response thread\n"); fflush(0);
		// Clear the buffer and read the message from the socket (from the server)
		memset(buffer, 0, 255);

		// If there was an error reading from the socket, throw an error
		if(read(zyboSocket, buffer, 255) <= 0) {
			fprintf(stderr, "CLI QUAD: ERROR reading from quad.\n");
			fflush(stderr);
			continue;
		}

		pthread_mutex_lock(&quadResponseMutex);
		newQuadResponse = 1;
		memcpy(respBuf, buffer, 255);
		pthread_mutex_unlock(&quadResponseMutex);


		//parse_packet(buffer, &respBuf, &metadata);
		printf("CLI QUAD: %s\n", buffer);
	}

	pthread_exit(NULL);
}

void *handleCliInput() {
	while(keepRunning)
	{
		printf("in cli input thread\n"); fflush(0);
		char userCommand[CMD_MAX_LENGTH] = {};
		fprintf(stderr, "$microcart> ");

		while(fgets(userCommand, sizeof(userCommand), stdin) != NULL && keepRunning) {
			
			// if the user simply hit enter then let them try again
			if((userCommand[0] == '\n') || (userCommand[0] == '\r'))
			{
				fprintf(stderr, "$microcart> ");

				memset(userCommand, 0, CMD_MAX_LENGTH);

				continue;
			}

			pthread_mutex_lock(&cliInputMutex);
			newCliInput = 1;
			memcpy(commandBuf, &userCommand, CMD_MAX_LENGTH);
			pthread_mutex_unlock(&cliInputMutex);

			
			fprintf(stderr, "$microcart> ");

			memset(userCommand, 0, CMD_MAX_LENGTH);
		}
	}

	pthread_exit(NULL);
}

int main(int argc, char **argv)
{
	pthread_t quadResponse, cliInput;
	respBuf = malloc(1024);
	commandBuf = malloc(CMD_MAX_LENGTH);
	


	signal(SIGINT, killHandler);

//	if ((zyboSocket = connectToZybo()) < 0)
//	{
//		perror("Error connecting to Zybo...");
//		exit(1);
//	}
	
	// create vrpnTracker instance
//	tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);

	// Retrieve VRPN data from the control loop
	fprintf(stderr, "CLI: Starting quad receiving thread...\n");
	pthread_create(&quadResponse, NULL, handleQuadResponse, NULL);
	fprintf(stderr, "CLI: Thread quad receiving started.\n");

	fprintf(stderr, "CLI: Starting cli input thread...\n");
	pthread_create(&cliInput, NULL, handleCliInput, NULL);
	fprintf(stderr, "CLI: Thread Cli input started.\n");


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

	//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);
	while(1)
	{
		char tmpRespBuf[1024];
		char tmpCommandBuf[CMD_MAX_LENGTH];
		//check for user input via cli
		if(newCliInput)
		{
			pthread_mutex_lock(&cliInputMutex);
			newCliInput = 0;
			memcpy(tmpCommandBuf, commandBuf, CMD_MAX_LENGTH);
			pthread_mutex_unlock(&cliInputMutex);

			// I can use printf becuase the command was gathered using fgets.
			printf("\nINPUT FOUND via CLI: '%s'\n", tmpCommandBuf); fflush(0);
		}

		//check for update/response from quad
		if(newQuadResponse)
		{
			pthread_mutex_lock(&quadResponseMutex);
			newQuadResponse = 0;
			memcpy(tmpRespBuf, respBuf, 1024);
			pthread_mutex_unlock(&quadResponseMutex);

			printf("\nINPUT FOUND via QUAD: '"); fflush(0);
			fwrite(tmpRespBuf, 1024, 1, stdout);
			printf("'\n"); fflush(0);
		}
		usleep(10000);
	}

	

	ucart_vrpn_tracker_freeInstance(tracker);
	//free(vrpnData);
	free(respBuf);
	free(commandBuf);
	pthread_mutex_destroy(&cliInputMutex);
	pthread_mutex_destroy(&quadResponseMutex);
	close(zyboSocket);
	close(quadlog_file);
	return 0;
}

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

void readAndPrint() {
	// read data from the server
	// this is a blocking call.
	//TODO: Implement a non blocking version of this.
	bytes_read = read(zyboSocket, respBuf, sizeof(respBuf) -1);
	if( bytes_read > 0)
	{
		respBuf[bytes_read] = '\0';
		printf("%s", respBuf);
	}
}

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;
	status = write(zyboSocket, &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 = write(zyboSocket, 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
	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;
	}
}