#include "../Header/myPrinter.h"

// axisAct: 0-Z, 1-ZZ, else-Z and ZZ
MyPrt::MyPrt(int axisAct) {
	std::cout << "Creating printer object" << std::endl;

	if (axisAct == 0) {
		// Z
		std::cout << "x,y,z to be used" << std::endl;
		// axes to move (x,y,z)
		axisMask = (AXISMASK)(x_mask | y_mask | z_mask);
		// axes id
		axisNum = 3;
		z_idx = 2;
		axisIdArray[0] = x_id;
		axisIdArray[1] = y_id;
		axisIdArray[2] = z_id;
	}
	else if (axisAct == 1) {
		// ZZ
		std::cout << "x,y,zz to be used" << std::endl;
		// axes to move (x,y,z)
		axisMask = (AXISMASK)(x_mask | y_mask | zz_mask);
		// axes id
		axisNum = 3;
		zz_idx = 2;
		axisIdArray[0] = x_id;
		axisIdArray[1] = y_id;
		axisIdArray[2] = zz_id;
	}
	else {
		// Z + ZZ
		std::cout << "x,y,z,zz to be used" << std::endl;
		// axes to move (x,y,z)
		axisMask = (AXISMASK)(x_mask | y_mask | z_mask | zz_mask);
		// axes id
		axisNum = 4;
		z_idx = 2;
		zz_idx = 3;
		axisIdArray[0] = x_id;
		axisIdArray[1] = y_id;
		axisIdArray[2] = z_id;
		axisIdArray[3] = zz_id;
	}
}

MyPrt::~MyPrt() {
	std::cout << "Destructing printer object" << std::endl;
	if (handle != NULL) {
		if (!A3200MotionDisable(handle, TASKID_Library, axisMask)) { PrintError(); }
		if (!A3200Disconnect(handle)) { PrintError(); }
	}

}

int MyPrt::InitPrt(bool home_f) {

	// for config speed and acceleration 
	std::vector<DOUBLE> speed_threshold(axisNum);
	std::vector<DOUBLE> acc_threshold(axisNum);
	DOUBLE my_speed_threshold = 500.;

	std::cout << "Connecting to A3200. Initializing if necessary." << std::endl;;
	if (!A3200Connect(&handle)) { PrintError(); return -1; }

	// Get parameter
	for (int i = 0; i < axisNum; i++) {
		if (!A3200ParameterGetValue(handle, PARAMETERID_VelocityCommandThreshold, axisIdArray[i], &(speed_threshold[i]))) { PrintError(); return -1; }
		std::cout << "Vel threshold " << i << " :" << speed_threshold[i] << std::endl;
		if (!A3200ParameterGetValue(handle, PARAMETERID_DefaultRampRate, axisIdArray[i], &(acc_threshold[i]))) { PrintError(); return -1; }
		std::cout << "RampRate " << i << " :" << 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(); return -1; }
	}
	// Get parameter
	for (int i = 0; i < axisNum; i++) {
		if (!A3200ParameterGetValue(handle, PARAMETERID_VelocityCommandThreshold, axisIdArray[i], &(speed_threshold[i]))) { PrintError(); return -1; }
		std::cout << "Set Vel threshold " << i << " :" << speed_threshold[i] << std::endl;
	}

	// Enabling and Homing
	std::cout << "Enabling and homing x,y,z axes." << std::endl;
	if (!A3200MotionEnable(handle, TASKID_Library, axisMask)) { PrintError(); return -1; }
	if (home_f) {
		if (!A3200MotionHome(handle, TASKID_Library, axisMask)) { PrintError(); return -1; }
	}
	

	return 0;
	
}



int MyPrt::MoveAbs(std::vector<DOUBLE>* posCmd, std::vector<DOUBLE>* velCmd, bool wait) {
	//for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
	for (int axisIdx = axisNum-1; axisIdx >=0; axisIdx--) {
		if (!A3200MotionMoveAbs(handle, TASKID_Library, axisIdArray[axisIdx], (*posCmd)[axisIdx], (*velCmd)[axisIdx])) { PrintError(); return -1; }
	}
	if (wait) {
		//std::cout << "Waiting motion to complete." << std::endl;
		if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_MoveDone, -1, NULL)) { PrintError(); return -1; }  //WAITOPTION_InPosition
	}
	return 0;
}

int MyPrt::MoveInc(std::vector<DOUBLE>* distCmd, std::vector<DOUBLE>* velCmd, bool wait) {
	for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
	//for (int axisIdx = axisNum - 1; axisIdx >= 0; axisIdx--) {
		if (!A3200MotionMoveInc(handle, TASKID_Library, axisIdArray[axisIdx], (*distCmd)[axisIdx], (*velCmd)[axisIdx])) { PrintError(); return -1; }
	}
	if (wait) {
		//std::cout << "Waiting motion to complete." << std::endl;
		if (!A3200MotionWaitForMotionDone(handle, axisMask, WAITOPTION_MoveDone, -1, NULL)) { PrintError(); return -1; }  //WAITOPTION_InPosition
	}
	return 0;
}

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

int MyPrt::GetPos() {
	for (int axisIdx = 0; axisIdx < axisNum; axisIdx++) {
		if (!A3200StatusGetItem(handle, (int)axisIdArray[axisIdx], STATUSITEM_PositionFeedback, 0, &(posData[axisIdx]))) { PrintError(); return -1; }
	}
	return 0;
}

//int MyPrt::Dispense() {
//	/*if (!A3200CommandExecute(handle, TASKID_Library, "$hFile2 = FILEOPEN \"COM1\", 2", NULL)) { PrintError(); return -1; }
//	if (!A3200CommandExecute(handle, TASKID_Library, "COMMINIT $hFile2, \"baud = 9600 parity = N data = 8 stop = 1\"", NULL)) { PrintError(); return -1; }
//	if (!A3200CommandExecute(handle, TASKID_Library, "COMMSETTIMEOUT $hFile2, 0, 0, 0", NULL)) { PrintError(); return -1; }
//	if (!A3200CommandExecute(handle, TASKID_Library, "FILEWRITE $hFile2, \"{#C}\", 0x05, 0x02, \"03DI F0\", 0x03", NULL)) { PrintError(); return -1; }
//	if (!A3200CommandExecute(handle, TASKID_Library, "G1 X10 Y0", NULL)) { PrintError(); return -1; }*/
//
//	/*TASKSTATE current_state = TASKSTATE_Inactive;
//	if (!A3200ProgramBufferedRun(handle, TASKID_Library, "./Data/Dispensing_cmd.pgm")) { PrintError(); return -1; }
//	while (current_state != TASKSTATE_ProgramComplete) {
//		if (!A3200ProgramGetTaskState(handle, TASKID_Library, &current_state)) { PrintError(); return -1; }
//		std::cout << current_state << " not completed" << std::endl;
//	}*/
//	return 0;
//}