#include <stdio.h>
#include <typeinfo.h>
#include <windows.h>
#include "cbw.h"
#include <fstream.h>
#include <string>
#include <time.h>
#include "serial.h"


// Declaration of the main routine 
void homingRoutine( HANDLE m_hCom);
HANDLE InitializeCOMPort();


// Implementation of the routine 
HANDLE InitializeCOMPort( void ) {
	HANDLE     m_hCom;
	char*     m_sComPort;
	DCB      m_dcb;
	COMMTIMEOUTS m_CommTimeouts;
	BOOL bWriteRC;
	BOOL     bReadRC;
	DWORD iBytesWritten;
	DWORD iBytesRead;
	BOOL     m_bPortReady;

//set the properties of the serial port
//Values seen below assume factory default where:
//BaudRate=9600
//DataBits=8
//Parity=None
//StopBits=1
	m_sComPort = "Com1";
	m_hCom = CreateFile(m_sComPort, 
			GENERIC_READ | GENERIC_WRITE,
			0, // exclusive access
			NULL, // no security
			OPEN_EXISTING,
			0, // no overlapped I/O
			NULL); // null template 

	if(m_hCom == INVALID_HANDLE_VALUE) {
			printf("Error opening serial port.\n");
			exit(1);
	}

	m_bPortReady = SetupComm(m_hCom, 128, 128); // set buffer sizes
	m_bPortReady = GetCommState(m_hCom, &m_dcb);
	m_dcb.BaudRate = 9600;
	m_dcb.ByteSize = 8;
	m_dcb.Parity = NOPARITY;
	m_dcb.StopBits = ONESTOPBIT;
	m_dcb.fAbortOnError = TRUE;
	m_bPortReady = SetCommState(m_hCom, &m_dcb);


	//set timeouts for communications
	m_bPortReady = GetCommTimeouts (m_hCom, &m_CommTimeouts);

	m_CommTimeouts.ReadIntervalTimeout = 50;
	m_CommTimeouts.ReadTotalTimeoutConstant = 50;
	m_CommTimeouts.ReadTotalTimeoutMultiplier = 10;
	m_CommTimeouts.WriteTotalTimeoutConstant = 50;
	m_CommTimeouts.WriteTotalTimeoutMultiplier = 10;
	
	m_bPortReady = SetCommTimeouts (m_hCom, &m_CommTimeouts);
	return m_hCom;
}


void homingRoutine(HANDLE m_hCom){
	BOOL bWriteRC;
	BOOL     bReadRC;
	DWORD iBytesWritten;
	DWORD iBytesRead;
	//Output Homing routine complete
	printf("Homing Routine Complete.\n");
}


void main() {

	//declare constants
	char number[3];

	// variables used with the com port
	
	char       sBuffer[128];
	__time64_t time1;

	int i;
		for(i = 0; i < 128; i++) {
				sBuffer[i] = '\0';
		}
   

	m_sComPort = "Com1";  //for use with Comport1 ----- Peut-etre a ne pas utiliser ------
	HANDLE m_hCom = InitializeCOMPort();
 
	//Setup for penetrometer motor
	Serial motor(1); //uses com1 for the motor  
	motor.Setup();
	char buff[1024];
	char* str = (char*)malloc( 1024*sizeof( char ) );
	int bufsize = 1024;
	int strlen = 1024;
	for(int i=1; i<=10; i++) {
		bufsize = 1024;
		motor.WaitForChar(buff, bufsize);
	}
	motor.WriteBuffer("4", 1);
	bufsize = 1024;
	printf( "Motor ready\n" );

	char*     m_sComPort;
	DCB      m_dcb;
	COMMTIMEOUTS m_CommTimeouts;
	BOOL bWriteRC;
	BOOL     bReadRC;
	DWORD iBytesWritten;
	DWORD iBytesRead;
	BOOL     m_bPortReady;
	motor.WaitForChar(buff, bufsize, '-');

//main for loop
   for(int j=1;j<=50;j++)  {

		homingRoutine(m_hCom);


	 //motor controller reads input one character at a time, hence the for loops below.
	str = "v=721:@2=1:home:w=20:init\r";//initialize motor
	strlen = 26;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');

	str = "a=@%d120:print(@%d028)\r";//initialize motor
	strlen = 23;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');

	str = "@2=1:v=721\r";//initialize motor
	strlen = 11;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');

	str = "@%d13c=0:@%d126=3:@%d100=13:@%d105=3\r";//initialize motor (for motor 1)
	strlen = 37;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');
    
	//set motor speed to 50 turns/sec
	//negative means the penetrometer goes down, max speed is 100 turns/sec
	//if you change velocities, make sure delta v is <= 100 turns/sec
	str = "@%d13c=-50:move\r";//motor 1 ------- rajouter le mot "move" pour faire bouger le moteur, peut ne pas être indispensable 
   	strlen = 11;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');


	time1 = _time64( NULL );
	//run motor for 19.5 seconds (insert penetrometer a total of 1 mm)
	//in this particular setup, the gearhead ratio is 1:975, 1 turn=1mm
	printf( "Motor is running for 19.5 seconds\n" );	
	while( _time64( NULL ) < time1 + 19.5 ){
		continue;
	}

	//set motor speed to 0
	str = "@%d13c=0\r";
	strlen = 9;
	for(int i=0; i<strlen; i++) {
		motor.WriteBuffer(str + i, 1);
		motor.WaitForChar(buff, bufsize, str[i] );
	}
	bufsize = 1024;
	motor.WaitForChar(buff, bufsize, '-');

	printf( "Lowering of penetrometer complete\n" );

  	}//end main for loop

//Output Homing routine complete
	printf("Homing Routine Complete.\n");
	
//Once loop is done output to screen
	printf("Program finished.\n");
 
//close the file handle when we're done
	CloseHandle(m_hCom);

}