#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>

#define SER_DEVICE "/dev/cu.KeySerial1"
#define SER_SPEED B115200
#define NUM_NODES 8
#define NUM_SENSORS 6
#define PKT_LEN 16

int cel_ser_setup();
int cel_ser_shutdown();
short * cel_ser_read();

int check_offset(unsigned char buf[], int offset);
int do_read(unsigned char buf[], int len);
int cel_ser_findstart(unsigned char * buf);
int convert_input(unsigned char * buf, int len);
void print_packet(unsigned char buf[], int len);

int fd = -1;
struct termios term_before;
short data[NUM_NODES * (NUM_SENSORS + 1)] = {0};

int main(void) 
{
	printf("Setting up serial port\n");
	if (cel_ser_setup())
		printf("Setup serial port\n");
	else
		printf("Failed to setup serial port\n");
	
	//printf("Do something here\n");
	cel_ser_read();
	printf("got past first read\n");
	/*
	while(1) {
		int i, j;
		short *blah;
		blah = cel_ser_read();
		
		for(i=0;i < NUM_NODES; i++) {
			short * cur;
			cur = &blah[i * (NUM_SENSORS + 1)];
			printf("mode: %d ", cur[0]);
			for(j=0; j < NUM_SENSORS; j++) {
				char * sensors[] = {"x", "y", "z", "yaw", "pitch", "roll"};
				printf("%s: %d ", sensors[j], cur[0]);
			}
			printf("\n");
		}
		printf("\n");
	}
	*/
				
	
	if (cel_ser_shutdown() < 0)
		printf("Failed to setup serial port\n");
	else
		printf("Shutdown serial port\n");

	return 0;
}

int cel_ser_setup()
{
	static int done = 0;
	struct termios term;
	int ret;
	
	if(done) {
		fprintf(stderr, "cel_ser_setup: Setup already done.\n");
		return 0;
	}

	// Open serial port
	fd = open(SER_DEVICE, O_RDWR | O_NOCTTY); // | O_NONBLOCK);
	if (fd < 0) {
		fprintf(stderr, "cel_ser_setup: Failed to open %s: %s\n", SER_DEVICE, strerror(errno));
		return 0;
	}

	// Read in current serial port settings
	ret = tcgetattr(fd, &term);
	if (ret < 0) {
		fprintf(stderr, "cel_ser_setup: Failed get status of %s: %s\n", SER_DEVICE, strerror(errno));
		cel_ser_shutdown();
		return 0;
	}

	// Backup current serial port settings
	memcpy(&term_before, &term, sizeof(struct termios));

	// Clear termios structure
	cfmakeraw(&term);

	// Set serial port speed
	ret = cfsetspeed(&term, SER_SPEED);
	if (ret < 0) {
		fprintf(stderr, "cel_ser_setup: Failed to speed of %s to %s: %s\n", SER_DEVICE, SER_SPEED, strerror(errno));
		cel_ser_shutdown();
		return 0;
	}

	// Set data size
	term.c_cflag &= ~CSIZE;
	term.c_cflag |= CS8;
	
	// Set no parity
	term.c_cflag &= ~PARENB;
	
	// Set stop bits
	term.c_cflag &= ~CSTOPB;
	
	//Set no software flow control
	term.c_cflag &= ~(IXON | IXOFF | IXANY);
	
	//Set no hardware flow control
	term.c_cflag &= ~CRTSCTS;
	
	//Set raw mode input mode
	term.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

	//Set raw output mode
	term.c_oflag &= ~OPOST;
	
	// Write out new serial port settings
	ret = tcsetattr(fd, TCSANOW, &term);
	if (ret < 0) {
		fprintf(stderr, "cel_ser_setup: Failed to write out new settings to %s: %s\n", SER_DEVICE, strerror(errno));
		cel_ser_shutdown();
		return 0;
	}	

	done = 1;
	printf("Setup of %s done\n", SER_DEVICE);
	return 1;
}

int cel_ser_shutdown()
{
	int ret;
	
	if(fd < 0)
		return 1;
	
	// Write out old serial port settings
	ret = tcsetattr(fd, TCSANOW, &term_before);
	if (ret < 0) {
		fprintf(stderr, "cel_ser_shutdown: Failed to write out old settings to %s: %s\n", SER_DEVICE, strerror(errno));
	}

	// Close serial port
	ret = close(fd);	
	if (ret < 0) {
		fprintf(stderr, "cel_ser_shutdown: Error closing device %s: %s\n", SER_DEVICE, strerror(errno));
		return 0;
	}
	
	return 1;
}

short * cel_ser_read()
{
	int started = 0;
	int done = 0;
	int i, j, offset = -1;
	short val = 0;
	unsigned char buf[PKT_LEN * 2];
	unsigned char start_bytes[] = {'\r', '\n'};
		
	// Reset the data
	memset(data, 0xFF, sizeof(data));
	for(i=0; i < NUM_NODES; i++)
		data[i * NUM_SENSORS] = i + 1;

	while(!done) {
		if(!started) {
			offset = cel_ser_findstart(buf);
			printf("findstart returned\n");
			if(offset < 0)
				return NULL;
			else
				started = 1;
		}
		
		for(i=0; i < NUM_NODES; i++) {	
			if(i != 0 || offset < 0) {
				if(!do_read(buf, PKT_LEN))
					return NULL;
			}
			if(!check_offset(buf, 0)) {
				fprintf(stderr, "cel_ser_read: Packet stream corrupt, resyncing\n");
				started = 0;
				continue;
			} else {
				short * cur;
				cur = &data[i * (NUM_SENSORS + 1)];
				
				if(i != (buf[0] - '0')) {
					for(j=0; j< NUM_SENSORS; j++)
						cur[j] = -1;
					continue;
				}
				
				for(j=0; j< NUM_SENSORS; j++) 
					//Store the values, masking off the top 4 bits
					cur[j] = buf[2 + (j * 2)] & 4096;
			}
		}
	}
	
	return data;
}

int check_offset(unsigned char buf[], int offset) {
	unsigned char *cur;
	//int i;
	
	cur = &buf[offset];
	
	if(cur[0] < '1' || cur[0] > '8' ||
		cur[1] != ':' ||
		cur[14] != '\n' ||
		cur[15] != '\r')
		return 0;
	/*
	for(i=0; i < NUM_SENSORS; i++) {			
		if((cur[2 + (i * 2)] >> 4) != i + 2)
			return 0;
	}
	*/
	return 1;	
}

int do_read(unsigned char * buf, int len) {
	int ret;
			
	ret = read(fd, buf, len);
	switch(ret) {
		case 0:
			fprintf(stderr, "cel_ser_read: Read EOF from device %s, shutting down\n", SER_DEVICE);
			cel_ser_shutdown();
			return 0;
			break;
		case -1:
			fprintf(stderr, "cel_ser_read: Error reading from device %s, shutting down: %s\n",
				SER_DEVICE, strerror(errno));
			cel_ser_shutdown();
			return 0;
			break;
		default:
			if(ret == len)
				return 1;
				break;
			fprintf(stderr, "cel_ser_read: Failed to read requested number of bytes from device %s\n", SER_DEVICE);
			return 0;
			break;
	}
}

int cel_ser_findstart(unsigned char * buf)
{
	int i, offset = -1;
	
	while(1) {		
		printf(".");
		// Read in 2 packets worth of data so we can find the delimiter
		if(!do_read(buf, PKT_LEN * 2))
			return -1;
		
		print_packet(buf, PKT_LEN * 2);
		// Search buffer for start delimiter
		for(i=0; i < PKT_LEN + 1; i++) {
			if(check_offset(buf, i)) {
				offset = i;
				break;
			}
		}
		
		// Copy first found packet to buffer
		if(offset >= 0) {
			memmove(buf, &buf[offset], PKT_LEN);
			return offset;
		} else
			continue;
	}
	
	return 1;
}

int convert_input(unsigned char * buf, int len)
{
	int i;
	int val = 0;
	for(i=0; i < len; i++) {
		val |= buf[i] << (len - i); 
	}
	return val;	
}

void print_packet(unsigned char * buf, int len) {
	int i;
	for(i=0; i < len; i++) {
		/*if(buf[i] == '\r')
			printf("  \\r,");
		else if(buf[i] == '\n')
			printf("  \\n,");
		else if(isalnum(buf[i]) || ispunct(buf[i]))
			printf("%2c,", buf[i]);
		else*/
			printf(" %02x,", buf[i]);
	}
	printf("\n");
	fflush(stdout);
}