/**********************************************************************
** Laser.cpp


*********************************************************************/

#define _CRT_SECURE_NO_DEPRECATE 1


#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 "rangehs.h"         
#include "sio_util.h"   
#include "hload.h"

#include "laser.h"
#include "params.h"

#define VERSION_STRING "Version 1.1\n"

//Laser 
//Buffers 
//------------PL--------------------------
#define NUM_SAMPLES	5000
#define HS_BUFSIZE (NUM_SAMPLES*sizeof(HSIF_SAMPLE))			 // 8000* 4Dword
HSIF_SAMPLE hsbuffer[HS_BUFSIZE/sizeof(HSIF_SAMPLE)];    // Raw sample directly for the Hsif Card. See Sampled Data Format

#define NUM_PROCESS 1000000
#define HPS_BUFSIZE (NUM_PROCESS*sizeof(HSIF_PROC_SAMPLE))    
HSIF_PROC_SAMPLE hpSampleBuf[HPS_BUFSIZE/sizeof(HSIF_PROC_SAMPLE)];   //Processed, calibrated data sample

//COMMINFO Commport;		// "COMMINFO" in serial I/O routine for communication
COMMINFO * pcom = 0;            

HSIF_RESULT		result;		// see manual, page26

// Subrountines
//Laser
FILE * fp;
static void save_file(int lines, FILE* fp);      //see the difference at different saving position

// forward defines
static HSIF_HANDLE init_hsif(PARAMS_REC * parp, COMMINFO * pcom);


int main(int argc, char *argv[])
{
	// Laser
	COMMINFO		Commport;
	pcom = &Commport;
	HSIF_HANDLE		hsif = HSIF_INVALID_HANDLE;   // see manual, page25
    
//	int	serialport = DEFAULT_LASER_COMM;				
//	int	board = DEFAULT_LASER_BOARD;
//	long sample_period = DEFAULT_LASER_SAMPLE_PERIOD;
	int	lines = DEFAULT_LASER_SAMPLE_LINES;              // total lines of range capture data before exiting
//	int	max_range = DEFAULT_MAX_RANGE;       //650 inches
//	int	motor = DEFAULT_LASER_MOTOR_NUM;
//	int	powerlevel = DEFAULT_LASER_MOTOR_POWER;
	char *outLaserFileName;
	
	DWORD	numRead;
	int hplines;

	// get output file name
	outLaserFileName = argv[1];
	fp = fopen(outLaserFileName , "w");

	// load input parameters from properties file

	PARAMS_REC* parp;

	parp = readParamsFile("./properties.txt");
	//dumpParams(stdout, parp);

	printf(VERSION_STRING);   
  
	hsif = init_hsif(parp, pcom);
	if (hsif == HSIF_INVALID_HANDLE) {
		fprintf(stderr, "Initialization of high speed interface card / laser failed\n");
		return 1;
	}

	DWORD lmn = parp->laser_motor_number;
	DWORD lmpl = parp->laser_motor_power_level;
	HsifSetMotorPower(hsif, lmn, lmpl);   
	fprintf(stderr,"Waiting (5 sec.) for mirror to move\n");
	Sleep(5000);		
	
	fprintf(stderr, "Clear Sample Buffer\n"); 
	HsifClearSampleBuffer(hsif);	// for clean buffer each time
	HsifSamplingEnable(hsif);		// need to skip ahead
	fprintf(stderr,"Sampling Enable\n"); //Sleep(DEBUG_SLEEP_DELAY);
        
	fprintf(stderr,"Begin sampling\n");
	
	hplines = lines;
	while((lines--))
	{
		fprintf(stderr, "lines = %d\n", lines);
		if( (HsifGetBufferedSamples( hsif, hsbuffer, NUM_SAMPLES, &numRead, TRUE)) == HSIF_FAIL)
		{
			printf("ERROR: Buffer overflow flag not getting set.\n");
			return(0);
		}			
		fprintf(stderr, "HGBS completed -- numRead %d\n", numRead);
		if( DATA_LOST(hsbuffer[NUM_SAMPLES-1]))
		{
			HsifClearSampleBuffer(hsif);
			HsifSamplingEnable(hsif);
			fprintf(stderr, "data lost\n");
		}

		result = HsifProcessSamples(hsif, hsbuffer, hpSampleBuf+(hplines-lines-1)*NUM_SAMPLES, NUM_SAMPLES);		  
		fprintf(stderr, "HPS complete -- result = %d\n", result);
		if(hpSampleBuf[(hplines-lines-1)*NUM_SAMPLES].timeout)
		{
			printf("ERROR: Sample measurement timeout!\n");
			return(0);
		}

    }
	
	fprintf(stderr,"End sampling\n");
	fprintf(stderr,"Sampling Lines counter: %d\n", lines);
	HsifSetMotorPower(hsif, parp->laser_motor_number, 0);	
		
	fprintf(stderr,"Saving Data,Please Waiting\n");
	save_file((hplines-lines-1), fp);	

ERROR_EXIT:
	if(pcom)
		ClosePort(pcom);
	if(hsif != HSIF_INVALID_HANDLE)
		HsifClose(hsif);
	return(0);
}

/**************************************************************************/

static HSIF_HANDLE init_hsif(PARAMS_REC * parp, COMMINFO * pcom) {

	//Laser
	HINSTANCE	hsifDllHandle = NULL;
	char		modName[MAX_PATH];
	char		*hsif_dll_name;
	DWORD		hsif_card_model;
	DWORD		hsif_min_sample_period;
	HSIF_HANDLE	hsif = HSIF_INVALID_HANDLE;   // see manual, page25

	printf("before hsifdllinit\n");


		// Start SetupHsif -----
	hsifDllHandle = HsifLibraryInit();
	if(hsifDllHandle == NULL) {
		hsif_dll_name = HsifGetDllName();
		fprintf(stderr,"Unable to load %s.dll\n",hsif_dll_name);
		return HSIF_INVALID_HANDLE;
	}

// DLLInfo
	GetModuleFileName(hsifDllHandle,modName,MAX_PATH);
	printf("Using DLL: %s\n\n",modName);
	HsifDllInit();			// Initialize library, See manual, page 25
	printf("after hsifdllinit\n");

	//Initialization: COM, HSIF Card
	//initialize Serialport. About Serial Port see sio_util.h
	if(!HsifOpenPort(pcom, 9600, parp->laser_serial_port)){ 
		pcom = 0;
		fprintf(stderr, "Error: Serial Comm port open failed.\n");
		return HSIF_INVALID_HANDLE;
	}		  
	HsifSendStr(pcom,"I");// set to factory defaults. manually send the command to set the sensor through RS232.
	Sleep(200);       //give 0.2 sec time to take effect. 
										// Syntax: Sleep(Duration) - Where Duration is the duration of the sleep call in milliseconds;           
										//Syntax :  delay(N) , where n = Milliseconds   									 

	fprintf(stderr, "Opened high speed interface.\n");
	hsif = HsifOpen(parp->laser_hsif_board, pcom);   // see rangehs.h about HsifOpen

	if(hsif == HSIF_INVALID_HANDLE) 
	{
		fprintf(stderr, "Error: HsifOpen failed.\n");
		return HSIF_INVALID_HANDLE;
	}

	if(HsifResetBoard(hsif) == HSIF_FAIL) 
	{
		fprintf(stderr, "Error: HsifResetBoard failed.\n");
		return HSIF_INVALID_HANDLE;
	}

	if(HsifLoadCalibrationData(hsif, parp->calibration_file) == HSIF_FAIL) 
	{
		printf("ERROR: Could not read calibration file\n");
		return HSIF_INVALID_HANDLE;
	}				
	// set sample period and maximum sensor range
	printf("max range %d sample period %d\n", parp->laser_max_range, parp->laser_sample_period);
	DWORD sp = parp->laser_sample_period;
	DWORD mx = parp->laser_max_range;
	if(HsifSetSamplePeriod(hsif, 0, mx, sp) == HSIF_FAIL)
	{
		fprintf(stderr, "HsifSetSamplePeriod failed.\n");
		if(HsifGetCardModel(hsif, &hsif_card_model) == HSIF_FAIL)
		{
			fprintf(stderr, "Unknown Hsif card model.\n");
			return HSIF_INVALID_HANDLE;
		}
		
		if(HsifGetMinSamplePeriod(hsif, &hsif_min_sample_period) == HSIF_FAIL)
		{
			fprintf(stderr, "Unknown minimum sample period.\n");
			return HSIF_INVALID_HANDLE;
		}

		fprintf(stderr, "The AR4000 %d KHz model minimum sample period is %d us.\n",
				(hsif_card_model == HSIF_MODEL_AR4000_50K) ? 50 : 200,
				hsif_min_sample_period);

		return HSIF_INVALID_HANDLE;
	}
	fprintf(stderr, "HISF Card Calibration\n"); 
	HsifSetPollMode(hsif,TRUE);	// TRUE = polling allowed for low sample frequency
								// FALSE = a latency of 2048 samples between transfers from. the HSIF card buffer to the PC's local memory
								// FALSE = interrupt driven only better for less cpu activity for faster sampling rates                    	

	HsifClearSampleBuffer(hsif);	// for clean buffer each time

	if(HsifCalibrate(hsif) != HSIF_SUCCESS)
	{
		fprintf(stderr,"Error: Could not get calibration value **\n");
		return HSIF_INVALID_HANDLE;
	}

	fprintf(stderr,"SamplingModeInit\n"); 
	if(HsifSamplingModeInit(hsif) != HSIF_SUCCESS)		// must call this after a calibration // set card to sampling mode
	{
		fprintf(stderr,"Error: Could not initialize sampling mode\n");
		return HSIF_INVALID_HANDLE;
	}

	if(HsifClearEncoder(hsif, ENCODER1|ENCODER2 , TRUE)!= HSIF_SUCCESS)  
	{
		fprintf(stderr,"Error: Failed to clear encoder1\n");
		return HSIF_INVALID_HANDLE;
	} //True: take place when the index pulse occurs;  FALSE: immediately 

	if(HsifCalibrateEncoder(hsif, ENCODER1, 0, 4096) != HSIF_SUCCESS)
	{
		fprintf(stderr,"Error: Failed to set encoder1\n");
		return HSIF_INVALID_HANDLE;
	}	// Set sample period and resolution for encoder1.

	if(HsifCalibrateEncoder(hsif, ENCODER2, 0, 200) != HSIF_SUCCESS)
	{
		fprintf(stderr,"Error: Failed to set encoder2\n");
		return HSIF_INVALID_HANDLE;
	}	// Set sample period and resolution for encode2.

	fprintf(stderr, "max_range %d sample_period %d\n",
			parp->laser_max_range, parp->laser_sample_period);
	if(HsifSetSamplePeriod(hsif, 0, mx, sp) != HSIF_SUCCESS)// Set sample period and resolution
	{
		fprintf(stderr,"Error: Failed to set sample and range\n");
		return HSIF_INVALID_HANDLE;
	}	// Set sample period and resolution
	return hsif;
}


/*
//	Function Name: save_file(int lines)
//	Descriptions:	 Write all data from hpSampleBuf[] buffer to a file(data.txt).	
//
//	Comments:
*/

static void save_file(int lines, FILE *fp)
{
	int i;

	fprintf(fp, "Distance(inches)\t");
	fprintf(fp, "Raw Range(bits)\t");
	fprintf(fp, "Amplitude\t");
	fprintf(fp, "Temp(F)\t");
	fprintf(fp, "Angle1(radins)\t");
	fprintf(fp, "Angle2(radins\t");
	fprintf(fp, "Index1\t");
	fprintf(fp, "Index2\t");
		
    for( i=0; i < (lines*NUM_SAMPLES); i++)
	{
		fprintf(fp,"\n");
		fprintf(fp, "%.2f\t", hpSampleBuf[i].distance);
		fprintf(fp, "%.0f\t", (double)hpSampleBuf[i].rawRange);
		fprintf(fp, "%.1f\t", hpSampleBuf[i].amplitude);
		fprintf(fp, "%.1f\t", hpSampleBuf[i].caltemp);
		fprintf(fp, "%.4f\t", hpSampleBuf[i].angle1);
		fprintf(fp, "%.4f\t", hpSampleBuf[i].angle2);
		fprintf(fp, "%d\t", INPUT1(hpSampleBuf[i]) ? 1:0);
		fprintf(fp, "%d\t", INPUT2(hpSampleBuf[i]) ? 1:0);
	}
	    
    if (fclose(fp))
		fprintf(stderr,"Error: Can't close the file\n");
}