/*----------------------------------------------------------------------
 *
 * I3DM-GX2 Interface Software
 *
 *----------------------------------------------------------------------
 * (c) 2008 Microstrain, Inc.
 *----------------------------------------------------------------------
 * THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING
 * CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER
 * FOR THEM TO SAVE TIME. AS A RESULT, MICROSTRAIN SHALL NOT BE HELD LIABLE
 * FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY
 * CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY
 * CUSTOMERS OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH
 * THEIR PRODUCTS.
 *---------------------------------------------------------------------*/

/*----------------------------------------------------------------------
 * i3dmgx2_Main_CM.c
 *
 * Main for the 3DM-GX2 Adapter continious/poll data console application.
 *--------------------------------------------------------------------*/		
#include <stdio.h>
#include <sys/timeb.h>
#include <time.h>
#include "i3dmgx2_Errors.h"
#include "i3dmgx2_Cont.h"
#include "i3dmgx2_Utils.h"
#include "i3dmgx2_Utils_CM.h"
#include "i3dmgx2_readWriteDR.h"
#include "i3dmgx2_Serial.h"

int GetComPort(); //prompt user for comport and opens it
void SetDataRate(int portNum); 
void ReadContinuousData(int portNum); //puts the node in continuous mode and retrives and prits data until user interups
void ReadContinuousLogData(int portNum);

int main(int argc, char* argv[])
{
	BOOL bQuit = FALSE;
	BOOL bPrintHeader = TRUE;
    int portNum = 0;
	int Ccount = 0;
	while (portNum <=0){
        portNum = GetComPort();
		if (Ccount++ > 4)
			exit(1);
	}
   
	while(!bQuit){
		int chOption = 0;

		if(bPrintHeader)
		{
			printf("\n");
			printf("Enter an Option: (D)ataRate (C)ontinuous (L)og_Continuous (Q)uit\n");
			printf("D Set DataRate   - Modify or review the current DataRate\n");
			printf("C Continuous     - Put the node in continuous mode and print each record\n");
			printf("L Log Continuous - Put the node in continuous mode log and print each record\n");
			printf("Q Quit           - Quit the application\n");

			bPrintHeader = FALSE;
		}

		//read option from the keyboard
		while(!ReadCharNoReturn(&chOption))
		{
			Sleep(50);
		}

		//
		switch(chOption)
		{
			case 'D':
			case 'd': SetDataRate(portNum); bPrintHeader = TRUE; break;

			case 'C':
			case 'c': ReadContinuousData(portNum); bPrintHeader = TRUE; break;

			case 'L':
			case 'l': 
				      ReadContinuousLogData(portNum); 
				      bPrintHeader = TRUE; 
				      break;

			case 'q':
			case 'Q': bQuit = TRUE; break;

			case 'h':
			case 'H': bPrintHeader = TRUE; break;

			default: printf("Invalid Option\n\n"); bPrintHeader = TRUE;
		}
	}
				
	return 0;
}

//===========================================================================
// GetComPort
//---------------------------------------------------------------------------
// Description: Prompt user for the comport and then opens it.  The user is 
//              prompted until a valid comport is selected and successfully 
//              opened.
//
// Return: HANDLE - handle to the opened comport
//===========================================================================
int GetComPort()
{
	int portNum   = 0;
	int iComPort  = 0;
	int errCount = 0;
	int MaxFail = 5;
	//Get comport number ask user for the comport number and open it
	while(portNum == 0)
	{
		printf("Enter Comport to use:");
		scanf_s("%d", &iComPort);
		
		/* open a port */
        portNum = i3dmgx2_openPort(iComPort, 115200, 8, 0, 1, 1024, 1024);
        if (portNum<0) {
            printf("port open failed. ");
            printf("Comm error %d, %s:\n", portNum, explainError(portNum));
			if (errCount++ >= MaxFail)
				exit(-1);
			iComPort = 0;
		}
		else {
             printf("\n   Using Comm Port #%d \n", portNum);

		}
		
	} 

	return portNum;
}

//===========================================================================
// SetDataRate
//---------------------------------------------------------------------------
// Description: Allows user to set the DataRate between 1Hz and 300Hz
//              Default is 100Hz
//
// Return: A display of the DataRate value which has been set.
//===========================================================================
void SetDataRate(int portNum)
{
	int status = 0, Value = 0, Ival =0;
	int chOption = 0;
	int rDataRate = 0;
	int mDataRate = 0;
	int sDataRate = 0;
	int valCheck = 0;
	BYTE Record[5];
	char AddA = 0xFC;
	char AddB = 0xA2;
    unsigned short nvalS  = 0;

	purge_port(portNum);
	
	status = ReadDataRate(portNum, AddA, AddB, &Record[0] );
	if(status == 0){
		printf("\n\nCurrent value of the DataRate is set at ");
	    nvalS = convert2ushort(&Record[1]);
	    rDataRate = (51200 / nvalS);
	    printf("%d Hz\n\n", rDataRate);
		//printf("%dHz, %d\n\n", rDataRate, nvalS);
	} else {
		printf("Error Reading Data-Rate %d\n", status); // %s explainError(status));
		return;
	}
	while(Value == 0)
	{
		printf("Would you like to Modify the DataRate? (Y)es or (N)o \n");
		while(!ReadCharNoReturn(&chOption))
		{
			Sleep(50);
		}
        //scanf_s("%s", &choice);
		switch (chOption){
			case 'n':
			case 'N': Value = 1; break;

			case 'y':
			case 'Y': while (Ival == 0){
				            printf("Enter a value between 1 and 300 to set:");
				            scanf_s("%d", &mDataRate);
							if (mDataRate < 0 || mDataRate > 300){
								printf(" Invalid entry: %d is out of range  \n", mDataRate);
				                Ival = 0;
							} else Ival = 1;
					  }
				      sDataRate = ( 51200/mDataRate);
				      valCheck = WriteDataRate(portNum, AddA, AddB, sDataRate, &Record[0]);
				      if(valCheck != 0 ){
						  printf("DataRate set Error %d\n", valCheck);
						  break;
					  }
				      if( valCheck == 0) {
					      status = ReadDataRate(portNum, AddA, AddB, &Record[0] );
	                      if(status == 0){
		                      printf("Current value of DataRate is set at ");
	                          nvalS = convert2ushort(&Record[1]);
	                          rDataRate = (51200 / nvalS);
	                          printf("%dHz\n\n", rDataRate);
						      Ival = 0;
						      break;
	                  } else {
		                      printf("Error Reading Data-Rate %d\n", status); //explainError(status));
		                      return;
	                  }
					  break;
						
				//}
					
			default: printf("Invalid Option\n\n");  Value = 0;
           }
		   //return;
		   }			
		
		}
}
/*===========================================================================
* ReadContinuousData
*---------------------------------------------------------------------------
* Description: Puts the node in continuous mode, reads the sensor data and
*              prints to the screen until the user interupts.
*
* Return: HANDLE - handle to the opened comport
*===========================================================================*/
void ReadContinuousData(int portNum)
{
	int iCount = 0;
	BOOL bStopContinuous = FALSE;
	DWORD dwcharsRead = 0;
	I3dmgx2Set Record;
	BYTE cmd_return;
	int Curs_posY = 0;
	int Curs_posX = 0;
	int status    = 0;
	int error_record = 0;
	char consoleBuff[60] = {0};
	long valid_rec = 0;
	unsigned char error_cmd;
	
	//put the node in continuous mode
	status =SetContinuousMode(portNum, 0xC2);
	printf("setcontinuous is %d", status);
	//set up the output for the data
	printf("\n\n");
	printf("Reading streaming data (hit s to Stop streaming).\n");
	printf("C2___________________________Acceleration_______________________________\n");
	printf("            X                       Y                      Z            \n");
	/*  acceleration values go here, save the position */
	printf("\n\n\n"); 
	printf("C2____________________________Angular_Rate______________________________\n");
	printf("            X                       Y                      Z            \n");
	/* angle rate values go here, save the position */
	getConXY(&Curs_posX, &Curs_posY); 
	printf("\n\n\n\n");
	
	//continue until the user hits the s key
	while(!bStopContinuous)
	{
		if(ReadNextRecord(portNum, &Record, &cmd_return) != SUCCESS)
			error_record++;
			if (cmd_return == 0xC2){
			//move to the acceleration position and print the data
		    sprintf_s(consoleBuff, 60, "\t%2.6f\t\t%2.6f\t\t%2.6f", Record.setA[0], Record.setA[1], Record.setA[2]);
		    setConXY(Curs_posX, Curs_posY -5, &consoleBuff[0]);
			sprintf_s(consoleBuff, 60, "\t%2.6f\t\t%2.6f\t\t%2.6f", Record.setB[0], Record.setB[1], Record.setB[2]);
		    setConXY(Curs_posX, Curs_posY, &consoleBuff[0]);
			valid_rec++;
			}else if (cmd_return != 0xc4){
				if((cmd_return == 0xCB || cmd_return == 0xD3) && error_record == 0)
				    error_cmd = cmd_return;
				else
					error_record++;
			}
			
		
		//check for a key every 50 itterations
		if(iCount++ > 50)	{
			int ch = 0;
			if(ReadCharNoReturn(&ch)){
				bStopContinuous = (ch == 's' || ch == 'S');
			}
			//rest the counter
			iCount = 0;
		}
	}
	printf("\n\n\nStopping Continuous Mode...");
	StopContinuousMode(portNum);
	printf("stopped.\n");
	if (error_record > 0)
	    printf("Number of records received in error were %d and received successfully %d\n", error_record, valid_rec);
	else
		printf("All %d records read successfully.\n", valid_rec);
}
/*===========================================================================
* ReadContinuousLogData
*---------------------------------------------------------------------------
* Description: Puts the node in continuous mode, reads the sensor data and
*              prints to the screen and logs data to user file until the user 
*              interupts.
*
* Return: HANDLE - handle to the opened comport
*===========================================================================*/
void ReadContinuousLogData(int portNum)
{
	int iCount = 0;
	BOOL bStopContinuous = FALSE;
	DWORD dwcharsRead = 0;
	I3dmgx2Set Record;
	BYTE cmd_return;
	int Curs_posY = 0;
	int Curs_posX = 0;
	int status    = 0;
	int error_record = 0;
	int valid_check = 0;
	int valid_count = 0;
	char consoleBuff[60] = {0};
	long valid_rec = 0;
	unsigned char error_cmd;
	char ComLogFile[256]; 
	FILE *m_logFile;
	int LogFlag = 0;
	int error_count = 0;
	int errorCode =0, i=0;
	char fw[12] = {0};
    char sn[20] = {0};
	char mListSep[4];
    char mDecSep[4];
	LANGID langId;
	char szLanguage[256]; /*MAX_PATH]; */
	char idchar[] = {'\x02', '\x00', '\x01', '\x03', '\x04'};
	SYSTEMTIME st;
	struct __timeb64 timebuffer;
    char *timeline;
	errno_t err;
    
    _ftime64( &timebuffer );
    timeline = _ctime64( & ( timebuffer.time ) );

	while (LogFlag != 1){
	    printf("Enter Name of LogFile to use:");
		scanf_s("%s", &ComLogFile, 255);
		printf("logFile %s\n", ComLogFile);
		if ( (err = fopen_s( &m_logFile, ComLogFile, "w")) != 0){
			printf("File: %s not opened\n", ComLogFile);
			if(++error_count > 2)
				return;
		}else LogFlag = 1;	
	}
	
	fprintf(m_logFile, "[SESSION START TAG]\n");
	//fprintf(m_logFile, "Session Start Time:%.19s.%hu %s", timeline, timebuffer.millitm, &timeline[20] );
	fprintf(m_logFile, "Session Start Time:%.19s.%hu \n", timeline, timebuffer.millitm );
	fprintf(m_logFile, "Time Source: HOST\n");
	GetLocaleInfo(LOCALE_SYSTEM_DEFAULT,LOCALE_SLIST,mListSep,4);
	fprintf(m_logFile, "List Seprerator: %s\n", mListSep);
	GetLocaleInfo(LOCALE_SYSTEM_DEFAULT,LOCALE_SDECIMAL,mDecSep,4);
    fprintf(m_logFile, "Decimal Seprerator: %s\n", mDecSep);
	langId = GetSystemDefaultLangID (); 
	 i = VerLanguageName (langId, szLanguage, 256); //dwSize = MAX_PATH; 
    fprintf(m_logFile, "Language: %s\n", szLanguage);
	/* 0xE9  get firmware number (as a string) 
    *-----------------------------------------------------------------*/
	//printf("\n 0xE9 Read Firmware Number \n");
    while(valid_check == 0)
	{
       errorCode = i3dmgx2_getFirmwareVersion(portNum, &fw[0]);
	   if (errorCode < 0){
		   purge_port(portNum);
		   if (valid_count++ > 6) {
               printf("Please Halt Current Data Display and Retry, count %d\n", valid_count);
		       return;
		   }
	   }else if (errorCode >= 0){
		   valid_check = 1;
	   }
	} 
	
   /*------------------------------------------------------------------
    * 0xEA  get serial information number, model and options (as string) 
    *-----------------------------------------------------------------*/
	valid_count =0;
    for ( i=0; i<4; i++){   //cycles through the valid device options
	
	   errorCode = i3dmgx2_getDeviceIdentiy(portNum, idchar[i], &sn[0]);
	   if (errorCode < 0){
		    purge_port(portNum);
			i--;
			if (valid_count++ >6){
			    printf("Error Read Device Identity: %s\n", explainError(errorCode));
			}
	   } else{
			 switch( i ) {
			       case 0:
					   fprintf(m_logFile, "Device Name: %s\n",sn);
					   break;
				   case 1:
					   fprintf(m_logFile, "Device Model: %s\n",sn);
					   break;
				   case 2:
					   fprintf(m_logFile, "Device FirmWare Version: %s\n", fw);
					   fprintf(m_logFile, "Device Serial Number: %s\n", sn);
					   break;
				   case 3:
					   fprintf(m_logFile, "Device Options: %s\n", sn);
					   break;
				   case 4:
				   default:
					   break;
            }
		 }
	
	} 

	fprintf(m_logFile, "Command C2\n[DATA START TAG]\n\t Time%s Accel X%s Accel Y%s Accel Z%s AngRate X%s AngRate Y%s AngRate Z%s Ticks\n", mListSep, mListSep, mListSep, mListSep, mListSep, mListSep, mListSep);

	//put the node in continuous mode
	status =SetContinuousMode(portNum, 0xC2);
	printf("setcontinuous is %d", status);
	//set up the output for the data
	printf("\n\n");
	printf("Reading streaming data (hit s to Stop streaming).\n");
	printf("C2___________________________Acceleration_______________________________\n");
	printf("            X                       Y                      Z            \n");
	/*  acceleration values go here, save the position */
	printf("\n\n\n"); 
	printf("C2____________________________Angular_Rate______________________________\n");
	printf("            X                       Y                      Z            \n");
	/* angle rate values go here, save the position */
	getConXY(&Curs_posX, &Curs_posY); 
	printf("\n\n\n\n");
	
	//continue until the user hits the s key
	while(!bStopContinuous)
	{
		GetLocalTime(&st);
		if(ReadNextRecord(portNum, &Record, &cmd_return) != SUCCESS)
			error_record++;
			if (cmd_return == 0xC2){
			//move to the acceleration position and print the data
			fprintf(m_logFile, "\t%02d.%03d", st.wSecond, st.wMilliseconds);
		    sprintf_s(consoleBuff, 60, "\t%2.6f\t\t%2.6f\t\t%2.6f", Record.setA[0], Record.setA[1], Record.setA[2]);
			fprintf(m_logFile, "%s %2.6f%s %2.6f%s %2.6f%s ", mListSep, Record.setA[0],mListSep, Record.setA[1], mListSep, Record.setA[2], mListSep);
		    setConXY(Curs_posX, Curs_posY -5, &consoleBuff[0]);
			sprintf_s(consoleBuff, 60, "\t%2.6f\t\t%2.6f\t\t%2.6f", Record.setB[0], Record.setB[1], Record.setB[2]);
		    fprintf(m_logFile, "%2.6f%s %2.6f%s %2.6f%s %u\n", Record.setB[0], mListSep, Record.setB[1], mListSep, Record.setB[2], mListSep, Record.timer);
			setConXY(Curs_posX, Curs_posY, &consoleBuff[0]);
			valid_rec++;
			}else if (cmd_return != 0xc4){
				if((cmd_return == 0xCB || cmd_return == 0xD3) && error_record == 0)
				    error_cmd = cmd_return;
				else
					error_record++;
			}
			
		
		//check for a key every 50 itterations
		if(iCount++ > 50)	{
			int ch = 0;
			if(ReadCharNoReturn(&ch)){
				bStopContinuous = (ch == 's' || ch == 'S');
			}
			//rest the counter
			iCount = 0;
		}
	}
	printf("\n\n\nStopping Continuous Mode...");
	StopContinuousMode(portNum);
	printf("stopped.\n");
	fclose(m_logFile);
	if (error_record > 0)
	    printf("Number of records received in error were %d and received successfully %d\n", error_record, valid_rec);
	else
		printf("All %d records read successfully.\n", valid_rec);
}