/******************************************************************
gyro.cpp



*/


#include <math.h> //mathematics function
#include <stdio.h>
#include <string.h>
#include <time.h>  //system time
#include <stdlib.h>
#include <process.h>  //access to the task list
#include <errno.h>  //errno value,error message
#include <windows.h>
#include <conio.h>  // console function
#include <sys/types.h>  //system time	
#include <sys/timeb.h>	//system time

#include "params.h"

#define VERSION_STRING "Version 1.1\n"

#define MAX_PACKET_SIZE 255
#define MAX_BUF_SIZE 255

#define NORMAL_HEADER 0xFF
#define NORMAL_HEADER_SIZE 1

#define AHRS_RAW_PACKET_SIZE		24
#define AHRS_COOKED_PACKET_SIZE		24
#define AHRS_ANGLE_PACKET_SIZE		30

#define VOLT_RANGE 5.0
#define TWO_EXP_EIGHT 256
#define TWO_EXP_TWELVE 4096
#define TWO_EXP_THIRTEEN 8192 
#define TWO_EXP_FIFTEEN 32767
#define TWO_EXP_SIXTEEN 65535

#define RAW_MODE 'r'   // raw voltage mode
#define COOKED_MODE 'c'  //scaled sensor mode
#define ANGLE_MODE 'a'

double gGyroRange[] = {100.0, 100.0, 100.0};
double gAccelRange[] = {4.0,4.0,4.0};
double gMagRange[] = {1.00, 1.00, 1.00};
double gSensorVoltConversion = VOLT_RANGE/TWO_EXP_TWELVE;
double fileOutputRate=0.0;

unsigned int userChar = '0';
#define EXIT_CHAR 'q'

//Gyro
static bool check_packet_valid(char mode, unsigned char* packet, unsigned int packetSize);
static void	get_packet(HANDLE fp, unsigned int packetSize, unsigned char *packet); 
static void	output_packet(char mode,double dt, unsigned int packetCnt,
						  unsigned char* packet, bool isDOSTIME, FILE* out);
static void print_packet(char mode, unsigned char* packet);
static void restart_system(HANDLE serialPort, char mode, unsigned int *packetSize);

void runGyro(PARAMS_REC *parp, char * filNamp);

#ifdef GYRO

int main(int argc, char** argv) {

	if (argc != 2) {
		fprintf(stderr, "Output file not specified\nUsage %s <filename>\n", argv[0]);
		return 1;
	}
	PARAMS_REC* parp = readParamsFile("./properties.txt");
	dumpParams(stdout, parp);
	runGyro(parp, argv[1]);
	
}

#endif


//Gyro
char *outGyroFileName;       											//a pointer to output file name
unsigned char packet[MAX_PACKET_SIZE];        //buffer the transfered and received data
FILE *out, *fp; 
	//char comPort[] = "COM2";    									//Seiral port, default is COM1
char *comPort = "COM2";
//unsigned int baudRate = CBR_38400;   					//Baud Rate, default is 38400, 'CBR'-Key words
char mode = ANGLE_MODE;    										//Measurement Modes, defualt is Angle mode.
unsigned long decimationRate=1;
bool outputData=FALSE, badPacket=FALSE;
bool isDOSTIME = FALSE;
unsigned int packetSize=0;
unsigned int packetCnt =0;
unsigned int badpacketCnt =0;
unsigned int i;
double dt=-1;

LPDCB serialCtrl= new DCB;    //DCB structure in MSDN,LPDCB
HANDLE serialPort;
COMMTIMEOUTS commTimeouts;

void runGyro(PARAMS_REC *parp, char * filNamp) {

	//Gyro
	comPort = parp->gyro_serial_port;
	mode = parp->gyro_mode[0];
	out = fopen(filNamp, "wb");
	if (out == 0) {
		fprintf(stderr, "Couldn't open output file %s\n", filNamp);
		return;
	}
	fprintf(stderr, "Saving output gyro data in %s\n", filNamp);
	/* open serial port */
serialPort = CreateFile(comPort,GENERIC_READ|GENERIC_WRITE, 0 /* do not share */,
						NULL, OPEN_EXISTING, FILE_ATTRIBUTE_SYSTEM, NULL);
	//See CreateFile() in MSDN

if (serialPort == INVALID_HANDLE_VALUE) {
	printf("Gyro - could not open serial port, %d\n", GetLastError());
	exit(1);
}
	/*----change NULL to INVALID_HANDLE_VALUE-----*/

#if 0
if (!(SetupComm(serialPort,0x1000, 0x1000))) {
	fprintf(stderr, "Gyro - error setting up comm: %d\n", GetLastError());
	CloseHandle(serialPort);
	exit(1);
}//set internal buffer size, in bytes.
#endif

if (!(GetCommState(serialPort, serialCtrl))) {
	fprintf(stderr, "Gyro - error getting comm: %d\n", GetLastError());
	CloseHandle(serialPort);
	exit(1);
}

serialCtrl->BaudRate = parp->gyro_baud_rate;
serialCtrl->Parity = NOPARITY;
serialCtrl->StopBits = ONESTOPBIT;

if (!(SetCommState(serialPort, serialCtrl))) {
	fprintf(stderr, "Gyro - error setting comm: %d\n", GetLastError());
	CloseHandle(serialPort);
	exit(1);
}
#if 0
if(!(GetCommTimeouts(serialPort, &commTimeouts))){
	fprintf(stderr, " Gyro - error getting comm timeouts:%d/n", GetLastError());
	CloseHandle(serialPort);
	exit(1);
};
#endif
	/*---------------add-----------------*/
	// if (!(GetCommTimeouts(serialPort, &commTimeouts))) {
	//	fprintf(stderr, "error getting Timeouts: %d\n", GetLastError());
	//}
	/*-----------------------------------*/

fprintf(stderr, "runGyro %d\n", __LINE__);
fprintf(stderr, "timeouts %ld %ld %ld\n",
		commTimeouts.ReadIntervalTimeout,
		commTimeouts.ReadTotalTimeoutMultiplier,
		commTimeouts.ReadTotalTimeoutConstant);

commTimeouts.ReadIntervalTimeout = MAXDWORD;
commTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
commTimeouts.ReadTotalTimeoutConstant = 5;
#if 0
if(!(SetCommTimeouts(serialPort, &commTimeouts))){
	fprintf(stderr, "Gyro - error setting comm timeouts:%d\n", GetLastError());
	CloseHandle(serialPort);
	exit(1);
};
#endif
printf("Gyro record is starting now!\n");
printf("For stop, Press 'Q' !\n");

if(mode == ANGLE_MODE) {
	printf("   ROLL   PITCH     YAW   GYROX   GYROY   GYROZ  ACCELX  ACCELY  ACCELZ  MAGX   MAGY   MAGZ  TEMP\n");
}
else {
	printf("   GYROX   GYROY   GYROZ   ACCELX  ACCELY  ACCELZ  MAGX    MAGY    MAGZ  TEMP\n");
}

printf("before restart\n");
restart_system(serialPort, mode, &packetSize);
printf("after restart\n");

while(userChar != EXIT_CHAR) {
//		printf("waiting for exit\n");
	for (i=0; i<decimationRate; i++){ 
		get_packet(serialPort, packetSize, packet); 
		if (!check_packet_valid(mode, packet, packetSize)) {
			if(badpacketCnt == 5) restart_system(serialPort, mode, &packetSize);
			badpacketCnt++; /* at least one packet to restart system */
			badPacket = TRUE;
		} 
		else { 
			badPacket = FALSE;
			badpacketCnt = 0;
		}
		packetCnt++;  // number of packets
		if ((packetCnt%10 == 0)  && !badPacket) {
			print_packet(mode, packet);
		}	 
	}
	if (!badPacket) {
		output_packet(mode, dt, packetCnt, packet, isDOSTIME, out);
	}
	if (kbhit()) {//kbhit()--checks the console for keyboard input, required header <conio.h>
		userChar=getch();
	}

}/* end while */

if (CloseHandle(serialPort) == 0 )
	fprintf(stderr,"Gyro - Port  Closing isn't successed: %d\n", GetLastError());
	if (fclose(out))
		fprintf(stderr,"Gyro - Error: Can't close the file\n");
}

/*--------------------------------------*/
//WriteFile();ReadFile();check_packet_valid()
//
/*--------------------------------------*/

static void restart_system(HANDLE serialPort, char mode, 
						   unsigned int *retPacketSize) 
{
	unsigned char buf[MAX_BUF_SIZE];  //255 bytes, buffer the transfered and received data.
	unsigned long bytesWritten, bytesToWrite;
	unsigned int packetSize=0;   // record the packet size.
	LPDCB serialCtrl= new DCB;

	/* stop continuous mode */
	/*---------PL--------------*/
	// set the Data packets transfer mode- polled mode
	bytesWritten = 1;
	while (bytesWritten > 0) {
		buf[0] = 'P';  // AHRS command, change to polled mode, waiting 'G' to sent data
		bytesToWrite = 1;
		WriteFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
		Sleep(100);
		bytesToWrite = MAX_BUF_SIZE;
		ReadFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
	}

	/* set mode, should get back the capital of the value set */
	/*----------------PL------------*/
	//set AHRS operating Mode
	buf[0] = 0;
	while  (buf[0] != (mode - 0x20)) { //'0x20'= SP = Space, ASCII Codes
		buf[0] = mode;
		bytesToWrite = 1;
		WriteFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
		Sleep(1);
		bytesToWrite = 1;
		ReadFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL); 
	}

	/* stop continuous mode */
	bytesWritten = 1;
	while (bytesWritten > 0) {
		buf[0] = 'P';
		bytesToWrite = 1;
		WriteFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
		Sleep(100);
		bytesToWrite = MAX_BUF_SIZE;
		ReadFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
	}

	/* send a 'G' to find the size of the packet */
	buf[0] = 'G';
	bytesToWrite = 1;
	WriteFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);
	Sleep(100);
	bytesToWrite = MAX_BUF_SIZE;
	ReadFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL); 
	packetSize = bytesWritten;
	check_packet_valid(mode, buf, packetSize);

	/* 
	** now send a C to get continuous mode working
	** then start recording data
	*/
	buf[0] = 'C';
	bytesToWrite = 1;
	WriteFile(serialPort, buf, bytesToWrite, &bytesWritten, NULL);

	*retPacketSize = packetSize; // ?
}

/*--------------------------------*/
// ecah date packet begin wiht a header byte (255 or 0xFF) and end with a checksum.
//
//
/*----------------------------------*/
static bool check_packet_valid(char mode, unsigned char* packet, 
							   unsigned int packetSize)
{
	unsigned int i;
	unsigned int checksum = 0;
	bool retVal = TRUE;  // check packet data valid 


		if (packetSize != AHRS_ANGLE_PACKET_SIZE){
			printf("packet size incorrect: %d should be %d for AHRS\n",
				packetSize, AHRS_ANGLE_PACKET_SIZE);
			return FALSE;}

		//--------------------------------------------------

	//check the packet header
	if (packet[0] != NORMAL_HEADER) {
				printf("Gyro \nunrecognized header byte: ");
				retVal = FALSE;
			}

	//check the packet checksum
	for (i=1; i<packetSize-1; i++) {
		checksum += packet[i];
		}// Sum all packet contents except header and checksum
		//
		if ( packet[i] != (checksum & 0xFF)){
		printf("\nbad checksum:");
		retVal = FALSE;
		}   // may add cauclation here
	//---------------------------------

	if (retVal==FALSE) {
		for (i=0; i<packetSize; i++) 
			printf("%x ", packet[i] &0xFF);
		printf("\n");
		return FALSE;
	}
	else {
		return TRUE;
	}

} /* is packet valid end */



/*--------------------------*/
//Get one packet data for each calling.
//ReadFile(),
//
//
/*----------------------------*/

static void	get_packet(HANDLE fp, unsigned int packetSize, unsigned char *recvBuf) 
{
	unsigned int i;
	unsigned char tmpBuf[MAX_PACKET_SIZE];
	unsigned long bytesRead, byteCnt, bytesToRead;
	
	byteCnt = 0;
		// Read one packet size data from serial port, fp= serialport
	while (byteCnt < packetSize) {
		bytesToRead = packetSize-byteCnt;
		ReadFile(fp, tmpBuf, bytesToRead, &bytesRead, NULL);
		for(i=0; i<bytesRead; i++)
			recvBuf[byteCnt + i] = tmpBuf[i];
		byteCnt += bytesRead;		
	}
} /* end get_packet */



/*---------------------*/
//
//convert and save data
//convertion formula see manual
/*-----------------------*/
static void	output_packet(char mode, double dt, unsigned int packetCnt, 
						  unsigned char* packet, bool isDOSTIME, FILE* out)
{
	unsigned int i, j;
	

    static double initialTime, thisTime;
	static int firstTime = 1;

	static unsigned tmp[50][25];
	static int tmpCnt=0;

	double elapsedTime;
	double accel[3], gyro[3], temp, roll, pitch, mag[3], yaw;
	unsigned int hdxTime;
	unsigned short partNumber=0,bit=0;
	static int adjustTime =1;
	static unsigned long packetCntAdjust =0;
	static double lastOutputTime=0.0;

	if (dt < 0) {  // dt=-1
		if (isDOSTIME) {
			thisTime = (double)clock() / (double)CLOCKS_PER_SEC;  //  clock ticks of elapsed processor time.
			fprintf(out, "%d %f ", packetCnt, thisTime);
			fileOutputRate = 0.0;
		}
		else {
			fprintf(out, "%d ", packetCnt);
			fileOutputRate = 0.0;
		}
	}
	else {
		if (isDOSTIME) {
			thisTime = (double)clock() / (double)CLOCKS_PER_SEC;
			fprintf(out, "%d %f ", packetCnt, thisTime);
			fileOutputRate = 0.0;
		}
		else {
			thisTime = (double)clock() / (double)CLOCKS_PER_SEC;
			if (firstTime) {  // firstTime=1
				initialTime = thisTime;
				lastOutputTime = initialTime;
				firstTime = 0;
			}
			elapsedTime = dt*(packetCnt-packetCntAdjust) + initialTime; // packetCntAdjust=0
			if((elapsedTime-lastOutputTime) < fileOutputRate )
				return;
			else
				lastOutputTime = elapsedTime;

			if (elapsedTime+(dt*4) < thisTime) { /* lost time jump to this time */
				packetCntAdjust = packetCnt;
				initialTime = thisTime;
				elapsedTime = thisTime;
				lastOutputTime = elapsedTime;
			}
			fprintf(out, "%d %f ", packetCnt, elapsedTime);
		}
	}/* end else dt >=0 */
	

	
		i=1;
		roll = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		pitch = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		yaw = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		for (j=0; j<3; j++) {
			gyro[j] = ((short)((packet[i] << 8) + packet[i+1])) * 
				(gGyroRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		for (j=0; j<3; j++) {
			accel[j] = ((short)((packet[i] << 8) + packet[i+1])) *
				(gAccelRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		for (j=0; j<3; j++) {
			mag[j] = ((short)((packet[i] << 8) + packet[i+1])) *
				(gMagRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		/* mystic incantation to get temp in degrees Celsius */
		temp = ((((packet[i] << 8) + packet[i+1]) * 5.0/4096.0) - 1.375)*44.44; 
		i+=2;
		hdxTime = (packet[i] <<8) + packet[i+1];
		fprintf(out, "%f %f %f ", roll, pitch, yaw);
		fprintf(out, "%f %f %f ", gyro[0], gyro[1], gyro[2]);
		fprintf(out, "%f %f %f ", accel[0], accel[1], accel[2]);
		fprintf(out, "%f %f %f ", mag[0], mag[1], mag[2]);
		fprintf(out, "%f %d\n", temp, hdxTime);
}

/*-------------------------------*/
//
//convert and print out the data
//convertion formula see manual
//
/*---------------------------------*/
static void print_packet(char mode, unsigned char* packet)
{
	unsigned int i, j;

	double accel[3], gyro[3], temp, roll, pitch, mag[3], yaw;
	unsigned int hdxTime;
	unsigned short partNumber=0,bit=0;


		i=1;
		roll = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		pitch = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		yaw = ((short)((packet[i] << 8) + packet[i+1])) * 
				(180.0)/TWO_EXP_FIFTEEN;
		i+=2;
		for (j=0; j<3; j++) {
			gyro[j] = ((short)((packet[i] << 8) + packet[i+1])) * 
				(gGyroRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		for (j=0; j<3; j++) {
			accel[j] = ((short)((packet[i] << 8) + packet[i+1])) *
				(gAccelRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		for (j=0; j<3; j++) {
			mag[j] = ((short)((packet[i] << 8) + packet[i+1])) *
				(gMagRange[j]*1.5)/TWO_EXP_FIFTEEN;
			i+=2;
		}
		/* mystic incantation to get temp in degrees Celsius */
		temp = ((((packet[i] << 8) + packet[i+1]) * 5.0/4096.0) - 1.375)*44.44; 
		i+=2;
		hdxTime = (packet[i] <<8) + packet[i+1];
		printf("%7.2f %7.2f %7.2f ", roll, pitch, yaw);
		printf("%7.2f %7.2f %7.2f ", gyro[0], gyro[1], gyro[2]);
		printf("%7.2f %7.2f %7.2f ", accel[0], accel[1], accel[2]);
		printf("%7.2f %7.2f %7.2f ", mag[0], mag[1], mag[2]);
		printf("%8.2f ", temp);
		printf("\r");
}