
#include <stdio.h>
#include <tchar.h>
#include <time.h>
#include <fstream>
#include <iostream>
#include <vector>


// Main include file
#include "A3200.h"

// See the pre-build event to see the System Library dlls being copied to the output directory

// This function will print whatever the latest error was
void PrintError();

int main(int argc, char **argv)
{
	
	// Handle to give access to A3200
	A3200Handle handle = NULL;
	DOUBLE positionFeedback;
	// axes to move (x,y,z)
	AXISMASK axisMask = (AXISMASK)(AXISMASK_00 | AXISMASK_02 | AXISMASK_04);
	// axes id
	const int axisNum = 3;
	AXISINDEX axisIdArray[axisNum] = { AXISINDEX_00, AXISINDEX_02, AXISINDEX_04 };

	// Data collection
	DOUBLE posCmd[3] = {50., 50., 50.,}; // mm
	DOUBLE speed[3] = { 45., 45., 45., };; // mm/s
	const int numDataPnt = 300;
	DOUBLE posData[axisNum][numDataPnt];
	DOUBLE velData[axisNum][numDataPnt];
	DOUBLE accData[axisNum][numDataPnt];
	double timeData[axisNum][numDataPnt];
	std::ofstream myfile;

	// Time stamp
	clock_t myTimer;

	// Threshold parameter
	std::vector<DOUBLE> speed_threshold(axisNum);
	std::vector<DOUBLE> acc_threshold(axisNum);
	DOUBLE my_speed_threshold = 500.;

	// Need to configure VELOCITY ON here
	

	printf("Connecting to A3200. Initializing if necessary.\n");
	if (!A3200Connect(&handle)) { PrintError(); goto cleanup; }

	// Get parameter
	for (int i = 0; i < axisNum; i++) {
		if (!A3200ParameterGetValue(handle, PARAMETERID_VelocityCommandThreshold, axisIdArray[i], &(speed_threshold[i]))) { PrintError(); goto cleanup; }
		std::cout << "Vel threshold" << i <<": " << speed_threshold[i] << std::endl;
		if (!A3200ParameterGetValue(handle, PARAMETERID_DefaultRampRate, axisIdArray[i], &(acc_threshold[i]))) { PrintError(); goto cleanup; }
		std::cout << "RampRate: " << acc_threshold[i] << std::endl;
	}
	// Set parameter
	for (int i = 0; i < axisNum; i++) {
		if (!A3200ParameterSetValue(handle, PARAMETERID_VelocityCommandThreshold, axisIdArray[i], my_speed_threshold)) { PrintError(); goto cleanup; }
	}
	// Get parameter
	for (int i = 0; i < axisNum; i++) {
		if (!A3200ParameterGetValue(handle, PARAMETERID_VelocityCommandThreshold, axisIdArray[i], &(speed_threshold[i]))) { PrintError(); goto cleanup; }
		std::cout << "Set Vel threshold" << i << ": " << speed_threshold[i] << std::endl;
	}

	printf("Enabling and homing x,y,z axes.\n");
	if (!A3200MotionEnable(handle, TASKID_Library, axisMask)) { PrintError(); goto cleanup; }
	if (!A3200MotionHome(handle, TASKID_Library, axisMask)) { PrintError(); goto cleanup; }

	printf("Moving axes to a different position.\n");
	/*for (int i = 1; i < 5; i++) {
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			if (!A3200MotionMoveAbs(handle, TASKID_Library, axisIdArray[axisIdx], 30, 1000)) { PrintError(); goto cleanup; }
		}

		if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_InPosition, -1, NULL)) { PrintError(); goto cleanup; }

		if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_InPosition, -1, NULL)) { PrintError(); goto cleanup; }
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			if (!A3200MotionMoveAbs(handle, TASKID_Library, axisIdArray[axisIdx], 0, 1000)) { PrintError(); goto cleanup; }
		}

		if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_InPosition, -1, NULL)) { PrintError(); goto cleanup; }
	}*/



	for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
		if (!A3200MotionMoveAbs(handle, TASKID_Library, axisIdArray[axisIdx], posCmd[axisIdx], speed[axisIdx])) { PrintError(); goto cleanup; }
	}
	//if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_InPosition, -1, NULL)) { PrintError(); goto cleanup; }
	//Recording time and position and velocity
	myTimer = clock();
	for (int dataIdx = 0; dataIdx < numDataPnt; dataIdx++) {
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			timeData[axisIdx][dataIdx] = clock() - myTimer;
			// Getting position
			if (!A3200StatusGetItem(handle, (int)axisIdArray[axisIdx], STATUSITEM_PositionFeedback, 0, &posData[axisIdx][dataIdx])) { PrintError(); goto cleanup; }
			// Getting velocity 
			if (!A3200StatusGetItem(handle, (int)axisIdArray[axisIdx], STATUSITEM_VelocityFeedback, 0, &velData[axisIdx][dataIdx])) { PrintError(); goto cleanup; }
			// Getting Acceleration
			if (!A3200StatusGetItem(handle, (int)axisIdArray[axisIdx], STATUSITEM_AccelerationFeedback, 0, &accData[axisIdx][dataIdx])) { PrintError(); goto cleanup; }
		}
	}
	if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_InPosition, -1, NULL)) { PrintError(); goto cleanup; }

	
	printf("Retrieving position feedback for the first axis.\n");
	if (!A3200StatusGetItem(handle, 0, STATUSITEM_PositionFeedback, 0, &positionFeedback)) { PrintError(); goto cleanup; }
	printf("Position Feedback : %f\n", positionFeedback);

	//Write to log file
	myfile.open("pos_data.txt");
	myfile << "t1, p1, t2, p2, t3, p3, t1, v1, t2, v2, t3, v3, t1, a1, t2, a2, t3, a3" << std::endl;
	for (int dataIdx = 0; dataIdx < numDataPnt; dataIdx++) {
		// position recording
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			myfile << timeData[axisIdx][dataIdx] << "," << (double)posData[axisIdx][dataIdx] << ",";
		}
		// velocity recording
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			myfile << timeData[axisIdx][dataIdx] << "," << (double)velData[axisIdx][dataIdx] << ",";
		}

		// acceleration recording
		for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
			myfile << timeData[axisIdx][dataIdx] << "," << (double)accData[axisIdx][dataIdx] << ",";
		}
		myfile << std::endl;
	}
	myfile.close();

	/*printf("Waiting for 1 second.\n");
	Sleep(1000);*/

cleanup:
	if (handle != NULL) {
		if (!A3200MotionDisable(handle, TASKID_Library, axisMask)) { PrintError(); }
		if (!A3200Disconnect(handle)) { PrintError(); }
	}

#ifdef _DEBUG
	printf("Press ENTER to exit...\n");
	getchar();
#endif
	return 0;
}

void PrintError() {
	CHAR data[1024];
	A3200GetLastErrorString(data, 1024);
	printf("Error : %s\n", data);
}
