
#include <opencv2/opencv.hpp>
#include <time.h>
#include "./Header/Detection_classes.h"
#include "./Header/Info_print.h"
#include "./Header/myCam_obj.h"
#include "./Header/myPrinter.h"
#include "./Header/SerialPort.h"
#include "./Header/myPCA.h"
#include "./Header/quaternion_operation.h"

#include <Eigen/dense>
// thread
#include <Windows.h>
#include <process.h>
#include <mutex>

using namespace cv;
using namespace std;

int tip_triangulation(MyCam* cam1, MyCam* cam2, bool read_tri_result, Mat& R, Mat& T);
void init_handeye_traj_manual(vector<vector<DOUBLE>>* traj);
int detectKeyPntTriangulation(MyCam* cam1, MyCam* cam2, bool DEBUG, bool DEFORM, myPCA* deformObj);
int poseEstimationTriangulate(MyCam* cam1, MyCam* cam2, MyPrt* myPrinter, bool DEBUG, Mat R, Mat T, bool DEFORM, myPCA* deformObj);
int rotationFromQuaternion(Mat* pnt, Mat* target, Mat& R, std::vector<double>* quaternion);
int readCycleInfo(std::string filename, std::vector<cv::Mat>& cycleParaVec, std::vector<double>& cycleZVec, int& cycle_period, int& cycle_period_step, float &max_eigen0, int &max_idx, bool feature_flag);

// For printing thread
MyPrt* global_myPrinter_pt;
std::vector<double>* cur_cmd_vec_global;
std::vector<double>* speed_global;
bool thread_activate_f = 0;
bool finish_flag = 0;
CRITICAL_SECTION critical;

// For pulsing thread
SerialPort *dispensor_pt;
char disp_cmd_pt[] = "\x05\x02\x30\x33\x44\x49\x20\x46\x30\x03";
bool finish_pulsing = 0;
bool pulsing_activate_f = 0;
bool wait_turnoff = 0;
CRITICAL_SECTION critical_pulsing;
clock_t pulseTimer;
const int on_time = 4000; // ms
const int off_time = 1000; //ms

// For cyclic deformation 
clock_t cmd_update_timer;
clock_t cmd_in_phase_timer;
int cmd_update_rate = 10; // ms
bool in_phase_flag = 0;
bool in_phase_init_timer_flag = 1;
int cyclic_motion_period = 3000; //ms
int current_phase_step = 0;
std::vector<cv::Mat> myCycleParaVec;
std::vector<double> myCycleZVec;
MyCam* global_myCam_pt;
myPCA* global_myPCA_pt;
double z_offset = 1.;
int* in_phase_offset_idx;
HANDLE hIOMutex = CreateMutex(NULL, FALSE, NULL);

unsigned int __stdcall mythread(void*)
{
	/*std::cout << "period (ms): " << cyclic_motion_period << std::endl;
	std::cout << "update rate (ms): " << cmd_update_rate << std::endl;*/
	while (!finish_flag) {
		if (in_phase_flag && in_phase_init_timer_flag) {
			cmd_in_phase_timer = clock();
			cmd_update_timer = clock();
			in_phase_init_timer_flag = 0;
		}
		if (thread_activate_f) {
			while (!TryEnterCriticalSection(&critical)) {}
			global_myPrinter_pt->MoveAbs(cur_cmd_vec_global, speed_global, 0);
			thread_activate_f = 0;
			LeaveCriticalSection(&critical);
		}
		else if (!in_phase_init_timer_flag && clock() - cmd_update_timer > cmd_update_rate) {
			cmd_update_timer = clock();
			if (in_phase_flag) {
				// apply predicted command
				current_phase_step = ((clock() - cmd_in_phase_timer + ((*in_phase_offset_idx) + 20) * cmd_update_rate) % cyclic_motion_period) / cmd_update_rate;
				if ((int)current_phase_step > myCycleParaVec.size() - 1) { current_phase_step = myCycleParaVec.size() - 1; }
				else { current_phase_step = (int)current_phase_step; }
				//cout << "Phase step: " << current_phase_step << endl;
				WaitForSingleObject(hIOMutex, INFINITE);
				global_myPCA_pt->parameters = myCycleParaVec[current_phase_step].t();// parameter from phase
				global_myCam_pt->p_o2p.at<double>(2) = myCycleZVec[current_phase_step]; // rigid translation z from phase
				global_myCam_pt->myToolpath.toolpath_timestamp = clock() - global_myCam_pt->myToolpath.toolpath_tmr; // update time for search of toolpath point
				if (!finish_flag && global_myCam_pt->generateCmd(global_myPrinter_pt, 0, 0, 0, 1, z_offset, 1, global_myPCA_pt, 0)==-1) {
					finish_flag = 1;
				}
				ReleaseMutex(hIOMutex);
				thread_activate_f = 1;
			}
		}
	}
	//cout << "thread finished" << endl;
	return 0;
}

unsigned int __stdcall mythread_dispensor(void*)
{
	pulseTimer = clock();
	while (!finish_pulsing) {
		if (pulsing_activate_f) {
			while (!TryEnterCriticalSection(&critical_pulsing)) {}
			// turn on
			if (!wait_turnoff && clock() - pulseTimer > off_time) {
				dispensor_pt->writeSerialPort(disp_cmd_pt, 10);
				pulseTimer = clock();
				wait_turnoff = 1;
			}
			// turn off
			if (wait_turnoff && clock() - pulseTimer > on_time) {
				dispensor_pt->writeSerialPort(disp_cmd_pt, 10);
				pulseTimer = clock();
				wait_turnoff = 0;
			}
			LeaveCriticalSection(&critical_pulsing);
		}
	}
	if (wait_turnoff) { // turn off before quit
		dispensor_pt->writeSerialPort(disp_cmd_pt, 10);
	}
	//cout << "thread finished" << endl;
	return 0;
}



int main(int argc, char **argv) {
	bool PRT_HOMING = 0;
	bool THREAD_F = 1; // thread for applying cmd
	bool DEFORM_PULSING_F = 1; // thread for applying pulsing of membrane

	vector<DOUBLE> posCmd = { 0., 0., 30. }; // mm
	vector<DOUBLE> posCmd_start = { 0., 0., 47. }; // mm
	vector<DOUBLE> posCmd_idle = { 0., 0., 47 }; // mm

	vector<vector<DOUBLE>> handEyeCalib_traj;
	init_handeye_traj_manual( &handEyeCalib_traj );

	vector<DOUBLE> speed = { 45., 45., 45. }; // mm/s

	// Init dispensor
	const char* portName = "\\\\.\\COM8";
	SerialPort *dispensor;
	dispensor = new SerialPort(portName);
	if (dispensor->isConnected()) {
		std::cout << "Connection established at port " << portName << endl;
	}
	else {
		std::cout << "Dispensor error " << portName << endl;
	}
	// Init dispensor for pulse
	const char* portName_pulse = "\\\\.\\COM7";
	dispensor_pt = new SerialPort(portName_pulse);
	if (dispensor_pt->isConnected()) {
		std::cout << "Connection established at port " << portName_pulse << endl;
	}
	else {
		std::cout << "Dispensor error " << portName_pulse << endl;
	}

	char disp_cmd[] = "\x05\x02\x30\x33\x44\x49\x20\x46\x30\x03";

	// axisAct: 0-Z, 1-ZZ, else-Z and ZZ
	MyPrt myPrinter(1);
	global_myPrinter_pt = &myPrinter;


	if (myPrinter.InitPrt(PRT_HOMING)) { return - 1; };

	
	//if (myPrinter.MoveAbs(&posCmd, &speed, 1)) { return -1; };
	//if (myPrinter.MoveAbs(&posCmd2, &speed, 1)) { return -1; };

	// System check
	PrintBuildInfo();
	FlyCapture2::Error error;
	BusManager busMgr;
	PGRGuid guid;
	unsigned int numCameras;
	error = busMgr.GetNumOfCameras(&numCameras);
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}
	std::cout << "Number of cameras detected: " << numCameras << endl;


	bool DEBUG = 1;
	bool TIMING = 1;
	bool PREDICTION = 0;
	bool METHOD = 1; // 0-monocular, pnp,1-dualcam, triangulation, no prediction yet
	bool DEFORM = 1;
	bool CYCLIC_PREDICT_FLAG = 0;
	bool CYCLIC_PREDICT_FEATURE = 1; // 0-max value for in-phase detection, 1-min value
	bool IN_PHASE_TEST = 1; // 1-looking for phase, 0-not looking for phase, dummy start, after certain "count"
	bool LOG_DATA_FLAG = 1; // log tracking error etc
	
	bool read_calib_f = 1;
	bool read_handeye_f = 1;
	bool handeye_verify = 0;
	bool read_tip_f = 1;
	bool read_tracking_info_f = 1;

	// prepare for recording
	VideoWriter video("outcpp.avi", CV_FOURCC('M', 'J', 'P', 'G'), 20, Size(1280, 1024));
	bool TRACKING_NO_PRTING = 1; // only tracking and recording, no printing
	if (TRACKING_NO_PRTING) {
		posCmd_start = { 0., 0., 47. }; // mm
		DEBUG = 1;
		THREAD_F = 0;
	}
	else {
		video.release();
	}
	

	int assist_cam_idx = 0; // cam that is used for triangulation
	int tracking_cam_idx = 1; // the single camera that is tracking
	Mat R_rel, T_rel; // coordinate transfer from tracking to assist

	/*std::string toolpath_filename = "./Data/dome_glass_model/myPnt.txt";
	std::string target_filename = "./Data/dome_glass_model/Marker_pnt.txt";*/

	/*std::string toolpath_filename = "./Data/hand_model_18pnt/traj.txt";
	std::string target_filename = "./Data/hand_model_18pnt/marker_pnt.txt";*/


	std::string toolpath_filename = "./Data/lung_05312019/Output4realtime/1_HDR_traj_auto.txt";
	std::string target_filename = "./Data/lung_05312019/Output4realtime/1_HDR_marker_pnt_auto.txt";
	std::string PCA_ref_filename = "./Data/lung_05312019/PCA_result/PCA_ref.txt";
	std::string PCA_deform_normal_filename = "./Data/lung_05312019/PCA_result/PCA_deform_normal.txt";
	std::string PCA_eigenvec_filename = "./Data/lung_05312019/PCA_result/PCA_eigen_vector.txt";

	//std::string toolpath_filename = "./Data/Postprocessing_result_smaller_dot_01272019/Output4realtime/11_HDR_traj_auto.txt";
	//std::string target_filename = "./Data/Postprocessing_result_smaller_dot_01272019/Output4realtime/11_HDR_marker_pnt_auto.txt";
	//std::string PCA_ref_filename = "./Data/Postprocessing_result_smaller_dot_01272019/PCA_result/PCA_ref.txt";
	//std::string PCA_deform_normal_filename = "./Data/Postprocessing_result_smaller_dot_01272019/PCA_result/PCA_deform_normal.txt";
	//std::string PCA_eigenvec_filename = "./Data/Postprocessing_result_smaller_dot_01272019/PCA_result/PCA_eigen_vector.txt";

	// cycle info filename
	std::string cycle_filename = "./Data/Postprocessing_result_smaller_dot_01272019/one_cycle_01292019.txt";
	
	// For feature pnt (deciding in-phase)
	float max_eigen0 = -100.;
	if (CYCLIC_PREDICT_FEATURE == 1) { // init for finding min instead, ignore var name
		max_eigen0 = 100000.;
	}
	int max_idx = 0;
	float eigen0[3] = { 0.,0.,0. }; // llast, last, current
	const float eigen_in_phase_thres = -2.;
	// read cycle file
	readCycleInfo(cycle_filename, myCycleParaVec, myCycleZVec, cyclic_motion_period, cmd_update_rate, max_eigen0, max_idx, CYCLIC_PREDICT_FEATURE);
	in_phase_offset_idx = &max_idx;

	/*std::string toolpath_filename = "./Data/Grid_on_deformable_membrane_060818/Output4realtime/0_HDR_traj_auto.txt";
	std::string target_filename = "./Data/Grid_on_deformable_membrane_060818/Output4realtime/0_HDR_marker_pnt_auto.txt";
	std::string PCA_ref_filename = "./Data/Grid_on_deformable_membrane_060818/PCA_result/PCA_ref.txt";
	std::string PCA_deform_normal_filename = "./Data/Grid_on_deformable_membrane_060818/PCA_result/PCA_deform_normal.txt";
	std::string PCA_eigenvec_filename = "./Data/Grid_on_deformable_membrane_060818/PCA_result/PCA_eigen_vector.txt";*/

	clock_t myTimer;

	// Init deformation obj
	myPCA myPCA_obj;
	global_myPCA_pt = &(myPCA_obj);

	if (DEFORM) {
		myPCA_obj.readfile(PCA_ref_filename, PCA_deform_normal_filename, PCA_eigenvec_filename);
		std::cout << "Num of eigen vectors: " << myPCA_obj.num_eigenvect << endl;
		std::cout << "Num of features: " << myPCA_obj.num_featurepnt << endl;
	}

	// Init camera obj
	std::vector<MyCam> myCamList(numCameras);
	global_myCam_pt = &(myCamList[tracking_cam_idx]);

	if (THREAD_F) {
		cur_cmd_vec_global = &(myCamList[tracking_cam_idx].current_cmd_vec);
		speed_global = &(myCamList[tracking_cam_idx].speed);
	}

	for (unsigned int i = 0; i < numCameras; i++)
	{
		error = busMgr.GetCameraFromIndex(i, &guid);
		if (error != PGRERROR_OK)
		{
			PrintError(error);
			return -1;
		}
		myCamList[i].Configure(guid, TIMING);
		// Calibration
		myCamList[i].CamCalibration(TIMING, read_calib_f);
		// Start Streaming
		myCamList[i].StartStreaming();
		// Handeye calibration
		myCamList[i].HandEyeCalib(&myPrinter, &handEyeCalib_traj, read_handeye_f, handeye_verify);
	}

	// move to desire height to be in focous
	if (!read_tracking_info_f) {
		myPrinter.MoveAbs(&(handEyeCalib_traj[handEyeCalib_traj.size() - 1]), &speed, 1); // wait for cmd to finish
	}
	else {
		myPrinter.MoveAbs(&posCmd_start, &speed, 1); // wait for cmd to finish
	}

	// Tip localization
	for (unsigned int i = 0; i < numCameras; i++) {
		// Capture one image
		myCamList[i].CaptureImage(&myPrinter, TIMING, 1);
		// Tip location
		myCamList[i].tipLocating2D(read_tip_f);
	}

	tip_triangulation(&(myCamList[tracking_cam_idx]), &(myCamList[assist_cam_idx]), read_tip_f, R_rel, T_rel);

	if (!METHOD) { // MEHOD = 0, monocular, turn off assisting camera
		myCamList[assist_cam_idx].StopStream();
	}


	// Init toolpath for the tracking camera
	if (read_tracking_info_f) {
		myCamList[tracking_cam_idx].myToolpath.init_toolpath_read_file(2.0, toolpath_filename); // print speed 2 mm/s
		if (METHOD) { // dual cam
			myCamList[assist_cam_idx].myToolpath.init_toolpath_read_file(2.0, toolpath_filename);
		}
	}
	else {
		myCamList[tracking_cam_idx].myToolpath.init_toolpath_planar_circle(2., 6., 500, 5);
		if (METHOD) { // dual cam
			myCamList[assist_cam_idx].myToolpath.init_toolpath_planar_circle(2., 6., 500, 5);
		}
	}
	
	

	//// Read image (not to be modified)
	// Mat im = cv::imread("image/track_test.jpg", CV_LOAD_IMAGE_GRAYSCALE);
	//// Create gray_BGR img for visualization
	// Mat temp_im_bgr(myCamList[i].image.size(), CV_8UC3);
	// cvtColor(myCamList[i].image, temp_im_bgr, CV_GRAY2RGB);

	// Manual init marker positions
	char key = 0;
	while (key != 'q') {
		// Capture one image
		myCamList[tracking_cam_idx].CaptureImage(&myPrinter, TIMING, 1);
		//cout << "printer_position: " << myPrinter.posData[0] << ", " << myPrinter.posData[1]  << ", " << myPrinter.posData[2] << endl;
		imshow("Placing target", myCamList[tracking_cam_idx].image);
		key = waitKey(1);
	}
	key = 0;
	if (METHOD) { // MEHOD = 1, dualcam
		while (key != 'q') {
			// Capture one image
			myCamList[assist_cam_idx].CaptureImage(&myPrinter, TIMING, 1);
			//cout << "printer_position: " << myPrinter.posData[0] << ", " << myPrinter.posData[1]  << ", " << myPrinter.posData[2] << endl;
			imshow("Placing target", myCamList[assist_cam_idx].image);
			key = waitKey(1);
		}
	}
	destroyWindow("Placing target");

	// Initialize myKeypoint objects with manual input
	myCamList[tracking_cam_idx].initKeyPnt(read_tracking_info_f, target_filename); // error checking to be added
	if (METHOD) { // MEHOD = 1, dualcam
		myCamList[assist_cam_idx].initKeyPnt(read_tracking_info_f, target_filename); // error checking to be added
	}

	// Detect ellipse in sub images.
	if (TIMING) { myTimer = clock(); }

	if (METHOD) { // MEHOD = 1, dualcam
		detectKeyPntTriangulation(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], DEBUG, DEFORM, &myPCA_obj);
		poseEstimationTriangulate(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], &myPrinter, DEBUG, R_rel, T_rel, DEFORM, &myPCA_obj);
	}
	else {
		myCamList[tracking_cam_idx].detectKeyPnt(DEBUG); // error checking to be added
		myCamList[tracking_cam_idx].poseEstimation(&myPrinter, DEBUG, 0, 1);
	}

	if (TIMING) { std::cout << "Time took for all detections in sub image: " << clock() - myTimer << endl; }

	if (DEBUG) {
		cv::imshow("subimage_detection", myCamList[tracking_cam_idx].image);
		waitKey(0);
		cv::destroyWindow("subimage_detection");
	}



	// Realtime tracking
	key = 0;
	int detected_flag = 0;
	//bool finish_flag = 0;

	bool one_time_init = 1; // wait the motion to complete for the first init move (long dist)
	if (TRACKING_NO_PRTING) {
		one_time_init = 0;
	}
	
	unsigned int count = 0;
	
	// for 
	cv::Point2f pix_diff;
	float pix_diff_norm;

	// Start printing
	// threading
	HANDLE myhandleA;
	if (THREAD_F) {
		InitializeCriticalSection(&critical);
		myhandleA = (HANDLE)_beginthreadex(0, 0, &mythread, (void*)0, 0, 0);
	}
	else {
		myhandleA = NULL;
	}
	// threading for pulsing
	HANDLE myhandleB;
	if (DEFORM_PULSING_F) {
		InitializeCriticalSection(&critical_pulsing);
		myhandleB = (HANDLE)_beginthreadex(0, 0, &mythread_dispensor, (void*)0, 0, 0);
	}
	else {
		myhandleB = NULL;
	}

	while (key != 'q' && !finish_flag) {

		std::cout << endl << "Step " << count << endl;
		if (TIMING) { myTimer = clock(); }


		// Capture one image

		//myCamList[tracking_cam_idx].CaptureImage(&myPrinter, TIMING, one_time_init);
		//if (METHOD) { // MEHOD = 1, dualcam
		//	myCamList[assist_cam_idx].CaptureImage(&myPrinter, TIMING, one_time_init);
		//}
		if (METHOD) { // MEHOD = 1, dualcam
			myCamList[tracking_cam_idx].CaptureImage1_gettingPos(&myPrinter, TIMING, one_time_init, DEBUG, &myPCA_obj);
			myCamList[assist_cam_idx].CaptureImage1_noGettingPos(TIMING, one_time_init, DEBUG, &myPCA_obj);

			myCamList[tracking_cam_idx].CaptureImage2_onlyCapture(TIMING);
			myCamList[assist_cam_idx].CaptureImage2_onlyCapture(TIMING);

			myCamList[tracking_cam_idx].CaptureImage3_postCapturing(TIMING);
			myCamList[assist_cam_idx].CaptureImage3_postCapturing(TIMING);
		}
		else {
			myCamList[tracking_cam_idx].CaptureImage(&myPrinter, TIMING, one_time_init);
		}



		if (TIMING) { 
			//cout << "Main Loop till capture: " << clock() - myTimer << endl; 
			myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
		}

		if (METHOD) { // MEHOD = 1, dualcam
			detectKeyPntTriangulation(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], DEBUG, DEFORM, &myPCA_obj);
			if (TIMING) {
				// cout << "Main Loop till detect: " << clock() - myTimer << endl; 
				myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
			}
			detected_flag = poseEstimationTriangulate(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], &myPrinter, DEBUG, R_rel, T_rel, DEFORM, &myPCA_obj);

			// check in-phase for cyclic motion
			if (DEFORM && CYCLIC_PREDICT_FLAG && IN_PHASE_TEST) {
				// cout << "parameters: " << myPCA_obj.parameters << endl;
				eigen0[0] = eigen0[1];
				eigen0[1] = eigen0[2];
				eigen0[2] = myPCA_obj.parameters.at<double>(0);
				// cout << "eigen0: " << eigen0[0] << " " << eigen0[1] << " " << eigen0[2] << endl;

				if (CYCLIC_PREDICT_FEATURE == 0) { // find max value for phase detection
					if (count > 10 && eigen0[1] - max_eigen0 > eigen_in_phase_thres && eigen0[1] > eigen0[0] && eigen0[1] > eigen0[2]) {
						cout << "-------max----------" << eigen0[1] - max_eigen0 << "----------------------" << endl;
						in_phase_flag = 1;
						in_phase_init_timer_flag = 1; // for multiple time of in phase adjustment
					}
				}
				else { // find min value for phase detection
					if (count > 10 && eigen0[1] - max_eigen0 < -eigen_in_phase_thres && eigen0[1] < eigen0[0] && eigen0[1] < eigen0[2]) {
						cout << "--------min---------" << eigen0[1] - max_eigen0 << "----------------------" << endl;
						in_phase_flag = 1;
						in_phase_init_timer_flag = 1; // for multiple time of in phase adjustment
					}
				}
				
			}

			if (TIMING) {
				// cout << "Main Loop till PnP: " << clock() - myTimer << endl; 
				myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
			}
		}
		else {
			// Detect ellipse in sub images.
			myCamList[tracking_cam_idx].detectKeyPnt(DEBUG); // error checking to be added
			if (TIMING) {
				//cout << "Main Loop till detect: " << clock() - myTimer << endl; 
				myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
			}

			// Estimate pose
			detected_flag = myCamList[tracking_cam_idx].poseEstimation(&myPrinter, DEBUG, PREDICTION, one_time_init);
			if (TIMING) {
				//cout << "Main Loop till PnP: " << clock() - myTimer << endl; 
				myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
			}
		}
		

		// Generate cmd
		if (detected_flag == 0) {
			if (one_time_init) {

				/*cv::imshow("subimage_detection", myCamList[i].image);
				key = waitKey(0);*/

				if (!TRACKING_NO_PRTING) {
					myCamList[tracking_cam_idx].generateCmd(&myPrinter, 1, 1, 0, 0, z_offset, DEFORM, &myPCA_obj, 0); // wait
				}

				// Initialize myKeypoint objects with manual input
				myCamList[tracking_cam_idx].CaptureImage(&myPrinter, TIMING, 1);
				if (METHOD) { // dual cam
					myCamList[assist_cam_idx].CaptureImage(&myPrinter, TIMING, 1);
				}
				myCamList[tracking_cam_idx].initKeyPnt(read_tracking_info_f, target_filename); // error checking to be added
				if (METHOD) { // dual cam
					myCamList[assist_cam_idx].initKeyPnt(read_tracking_info_f, target_filename); // error checking to be added
				}

				if (METHOD) { // dual cam
					detectKeyPntTriangulation(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], DEBUG, DEFORM, &myPCA_obj);
					while (poseEstimationTriangulate(&myCamList[tracking_cam_idx], &myCamList[assist_cam_idx], &myPrinter, 1, R_rel, T_rel, DEFORM, &myPCA_obj) == -1) {
						std::cout << "False detection." << endl;
					}
				}
				else {
					myCamList[tracking_cam_idx].detectKeyPnt(1); // error checking to be added
					while (myCamList[tracking_cam_idx].poseEstimation(&myPrinter, 1, 0, 1) == -1) {
						std::cout << "False detection." << endl;
					}
				}
				

				cv::imshow("subimage_detection", myCamList[tracking_cam_idx].image);
				key = waitKey(0);
					
				pix_diff = myCamList[tracking_cam_idx].tip_c_imgpts[0] - myCamList[tracking_cam_idx].myToolpath.toolpath_pnt_imgpnt[myCamList[tracking_cam_idx].myToolpath.toolpath_idx];
				pix_diff_norm = sqrt(pix_diff.x*pix_diff.x + pix_diff.y*pix_diff.y);
				std::cout << "pix_diff_norm" << pix_diff_norm << endl;
				if (pix_diff_norm < 20.) { 
					// Start toolpath clock (offset)
					myCamList[tracking_cam_idx].myToolpath.toolpath_tmr = clock();

					one_time_init = 0;
					// Start dispensing
					dispensor->writeSerialPort(disp_cmd, 10);
					
				}
			}
			else {
				if (!TRACKING_NO_PRTING) {
					WaitForSingleObject(hIOMutex, INFINITE);
					if (!finish_flag && myCamList[tracking_cam_idx].generateCmd(&myPrinter, 0, DEBUG, PREDICTION, THREAD_F, z_offset, DEFORM, &myPCA_obj, LOG_DATA_FLAG) == -1) {// no wait
						// check finish
						finish_flag = 1;
					}
					ReleaseMutex(hIOMutex);
				}
				if (THREAD_F) {
					thread_activate_f = 1;
				}

				// Keep pulsing
				if (DEFORM_PULSING_F) {
					pulsing_activate_f = 1;
				}

				std::cout << "Toolpath idx " << myCamList[tracking_cam_idx].myToolpath.toolpath_idx << endl;
				if (TIMING) {
					//cout << "Main Loop till cmd gen: " << clock() - myTimer << endl; 
					myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(clock() - myTimer);
					
					myCamList[tracking_cam_idx].timing[myCamList[tracking_cam_idx].timing.size() - 1].push_back(count);
					myCamList[assist_cam_idx].timing[myCamList[assist_cam_idx].timing.size() - 1].push_back(count);

					myPCA_obj.parameters_vec.push_back(myPCA_obj.parameters.clone());
					myPCA_obj.parameters_idx.push_back(count);
					myPCA_obj.LS_res_vec.push_back(myPCA_obj.LS_res.clone());

					Mat temp_C_o2p = myCamList[tracking_cam_idx].C_c2p * myCamList[tracking_cam_idx].target_rmats;
					Rotation2Quaternion(&temp_C_o2p, &(myCamList[tracking_cam_idx].quat));
					myCamList[tracking_cam_idx].quat_vec.push_back(myCamList[tracking_cam_idx].quat);
					
					//myCamList[tracking_cam_idx].target_tvecs_vec.push_back(myCamList[tracking_cam_idx].target_tvecs.clone());
					myCamList[tracking_cam_idx].target_tvecs_vec.push_back(myCamList[tracking_cam_idx].p_t2p - myCamList[tracking_cam_idx].C_c2p * myCamList[tracking_cam_idx].tip_c + myCamList[tracking_cam_idx].C_c2p * myCamList[tracking_cam_idx].target_tvecs);
				}
			}
		}
		else { 
			goto cleanup; 
		}
			

		if (DEBUG) {
			cv::imshow("subimage_detection", myCamList[tracking_cam_idx].image);
			key = waitKey(1);
			if (TRACKING_NO_PRTING) {
				// Write the frame into the file 'outcpp.avi'
				video.write(myCamList[tracking_cam_idx].image);
			}

			if (TIMING) { std::cout << "Main Loop till imgshow: " << clock() - myTimer << endl; }
		}

		//// check finish
		//if (myCamList[tracking_cam_idx].myToolpath.toolpath_idx >= myCamList[tracking_cam_idx].myToolpath.toolpath_num_waypnt) {
		//	finish_flag = 1;
		//}

		count++;
		// predictive model, in phase test
		if (CYCLIC_PREDICT_FLAG && !IN_PHASE_TEST) {
			if (count > 100) {
				in_phase_flag = 1;
			}
		}
		
		if (TRACKING_NO_PRTING) { 
			if (count > 1000) {
				break;
			}
		}
	}

	// for thread
	finish_flag = 1;
	WaitForSingleObject(myhandleA, INFINITE);
	CloseHandle(myhandleA);
	if (THREAD_F) {
		DeleteCriticalSection(&critical);
	}

	// for thread pulsing
	// Stop pulsing
	if (DEFORM_PULSING_F) {
		finish_pulsing = 1;
	}
	WaitForSingleObject(myhandleB, INFINITE);
	CloseHandle(myhandleB);
	if (DEFORM_PULSING_F) {
		DeleteCriticalSection(&critical_pulsing);
	}

// move to desire height to be in focous
myPrinter.MoveAbs(&posCmd_idle, &speed, 1); // wait for cmd to finish

cleanup:
	// Stop dispensing
	dispensor->writeSerialPort(disp_cmd, 10);
	// Stop
	myCamList[tracking_cam_idx].StopStream();
	if (METHOD) { // dualcam
		myCamList[assist_cam_idx].StopStream();
	}

	// Stop recording
	if (TRACKING_NO_PRTING) {
		video.release();
	}
	

	// save logged error data
	std::fstream outputFile;
	if (LOG_DATA_FLAG) {
		outputFile.open("./Data/TrackingError.txt", std::ios::out);
		outputFile << "toolpath_tip_traj / toolpath_desired_traj / toolpath_track_error" << std::endl;
		for (int idx = 0; idx < myCamList[tracking_cam_idx].myToolpath.toolpath_tip_traj.size(); idx++) {
			outputFile << myCamList[tracking_cam_idx].myToolpath.toolpath_tip_traj[idx].t() << " " << myCamList[tracking_cam_idx].myToolpath.toolpath_desired_traj[idx].t() << " " << myCamList[tracking_cam_idx].myToolpath.toolpath_track_error[idx].t() << std::endl;
		}
		outputFile.close();
	}
	// save logged vel data
	outputFile.open("./Data/Velocity_profile.txt", std::ios::out);
	outputFile << "raw_v / filtered_v  / raw_dp_o2p / t_duration_ms" << std::endl;
	for (int idx = 0; idx < myCamList[tracking_cam_idx].v_o2p_raw.size(); idx++) {
		outputFile << myCamList[tracking_cam_idx].v_o2p_raw[idx].t() << " " << myCamList[tracking_cam_idx].v_o2p_mov_avg[idx].t() << " " << myCamList[tracking_cam_idx].dp_o2p_raw[idx].t() << " " << myCamList[tracking_cam_idx].t_duration_raw[idx] << std::endl;
	}
	outputFile.close();
	// save logged angular vel data
	outputFile.open("./Data/AngVel_profile.txt", std::ios::out);
	outputFile << "raw angular vel in prt / averaged / angle in camera" << std::endl;
	for (int idx = 0; idx < myCamList[tracking_cam_idx].angular_vel.size(); idx++) {
		Mat temp_angular_vec(myCamList[tracking_cam_idx].angular_vel[idx]);
		Mat temp_angular_vec_avg(myCamList[tracking_cam_idx].angular_vel_avg[idx]);
		temp_angular_vec = myCamList[tracking_cam_idx].C_c2p * temp_angular_vec;
		temp_angular_vec_avg = myCamList[tracking_cam_idx].C_c2p * temp_angular_vec_avg;
		//outputFile << myCamList[tracking_cam_idx].angular_vel[idx][0] << " " << myCamList[tracking_cam_idx].angular_vel[idx][1] << " " << myCamList[tracking_cam_idx].angular_vel[idx][2] << std::endl;
		outputFile << temp_angular_vec.t() << "," << temp_angular_vec_avg.t() << ", " << myCamList[tracking_cam_idx].rot_angle[idx] << std::endl;
	}
	outputFile.close();

	// save logged time data
	if (TIMING) {
		outputFile.open("./Data/timing.txt", std::ios::out);
		outputFile << "till getting pos / img tstamp /img absolute t /till img conversion / mainloop till capture / mainloop till detect / mainloop till pnp / mainloop till cmd gen/ step" << std::endl;
		for (int idx = 0; idx < myCamList[tracking_cam_idx].timing.size(); idx++) {
			for (int jdx = 0; jdx < myCamList[tracking_cam_idx].timing[idx].size(); jdx ++) {
				outputFile << myCamList[tracking_cam_idx].timing[idx][jdx] << " " ;
			}
			outputFile << std::endl;
		}
		outputFile.close();

		if (METHOD) { // dual cam
			outputFile.open("./Data/timing_assist.txt", std::ios::out);
			outputFile << "till getting pos / img tstamp / img absolute t  /till img conversion / step" << std::endl;
			for (int idx = 0; idx < myCamList[assist_cam_idx].timing.size(); idx++) {
				for (int jdx = 0; jdx < myCamList[assist_cam_idx].timing[idx].size(); jdx++) {
					outputFile << myCamList[assist_cam_idx].timing[idx][jdx] << " ";
				}
				outputFile << std::endl;
			}
			outputFile.close();
		}

		// deformation parameter
		if (DEFORM) {
			outputFile.open("./Data/eigen_parameter.txt", std::ios::out);
			outputFile << "step / parameters" << std::endl;
			for (int idx = 0; idx < myPCA_obj.parameters_vec.size(); idx++) {
				outputFile << myPCA_obj.parameters_idx[idx] << " ";
				outputFile << myPCA_obj.parameters_vec[idx].t();
				outputFile << std::endl;
			}
			outputFile.close();

			// rigid body motion (wrt printer frame)
			outputFile.open("./Data/rigid_body_motion.txt", std::ios::out);
			outputFile << "step / tvec / quat" << std::endl;
			for (int idx = 0; idx < myCamList[tracking_cam_idx].quat_vec.size(); idx++) {
				outputFile << myPCA_obj.parameters_idx[idx] << " ";
				outputFile << myCamList[tracking_cam_idx].target_tvecs_vec[idx].t() << " ";
				for (int qi = 0; qi < 4; qi++) {
					outputFile << myCamList[tracking_cam_idx].quat_vec[idx][qi] << " ";
				}
				outputFile << std::endl;
			}
			outputFile.close();

			// deform marker LS residue
			outputFile.open("./Data/marker_deform_LS_res.txt", std::ios::out);
			outputFile << "step / LSres" << std::endl;
			for (int idx = 0; idx < myPCA_obj.LS_res_vec.size(); idx++) {
				outputFile << myPCA_obj.parameters_idx[idx] << " ";
				outputFile << myPCA_obj.LS_res_vec[idx].t() << std::endl;
			}
			outputFile.close();
		}
	}

	return 0;
}

// Triangulate the tip 
int tip_triangulation(MyCam* cam1, MyCam* cam2, bool read_tri_result, Mat& R, Mat& T ) {
	if (!read_tri_result) {
		// Stereo calib
		//Mat R, T, F, E;
		Mat F, E;
		stereoCalibrate(cam1->chess_obj, cam1->chess_corners, cam2->chess_corners, cam1->intrinsic, cam1->distCoeffs, cam2->intrinsic, cam2->distCoeffs, cam1->image.size(), R, T, E, F);
		std::cout << "R: " << endl << R << endl; // C_1to2
		std::cout << "T: " << endl << T << endl;
		std::cout << "self_calib_1to2: " << endl << (cam2->C_c2p.t()) * (cam1->C_c2p) << endl;
		// Triangulate
		cv::Mat cam1_proj = cv::Mat::eye(3, 4, CV_64FC1);
		cv::Mat cam2_proj = cv::Mat::eye(3, 4, CV_64FC1);
		R.copyTo(cam2_proj.rowRange(0, 3).colRange(0, 3));
		T.copyTo(cam2_proj.rowRange(0, 3).col(3));
		// Get 3d homography result
		cv::Mat homo_result;
		cam1->projection_matrix = cam1->intrinsic * cam1_proj;
		cam2->projection_matrix = cam2->intrinsic * cam2_proj;
		triangulatePoints(cam1->projection_matrix, cam2->projection_matrix, cam1->tip_c_imgpts, cam2->tip_c_imgpts, homo_result);
		std::cout << "homo: " << endl << homo_result << endl;
		Mat _homo_result = homo_result.reshape(0);
		std::cout << "reshaped homo: " << endl << _homo_result.t() << endl;
		// Compute eulidean result
		cam1->tip_c.at<double>(0, 0) = (double)(_homo_result.row(0).col(0).at<float>(0, 0) / _homo_result.row(3).col(0).at<float>(0, 0));
		cam1->tip_c.at<double>(1, 0) = (double)(_homo_result.row(1).col(0).at<float>(0, 0) / _homo_result.row(3).col(0).at<float>(0, 0));
		cam1->tip_c.at<double>(2, 0) = (double)(_homo_result.row(2).col(0).at<float>(0, 0) / _homo_result.row(3).col(0).at<float>(0, 0));
		std::cout << "cam1 tip3D: " << endl << cam1->tip_c << endl;
		// reproject tip_imgpts
		project_tip(&(cam1->intrinsic), &(cam1->distCoeffs), &(cam1->tip_c), &(cam1->tip_c_imgpts));
		/*std::vector<cv::Point3f> tip_c_pnt = {cv::Point3f(cam1->tip_c.at<double>(0, 0), cam1->tip_c.at<double>(1, 0), cam1->tip_c.at<double>(2, 0))};
		cv::Mat rvec_eye;
		cv::Rodrigues(cv::Mat::eye(3, 3, CV_64FC1), rvec_eye);
		projectPoints(tip_c_pnt, rvec_eye, cv::Mat::zeros(3, 1, CV_64FC1), cam1->intrinsic, cam1->distCoeffs, cam1->tip_c_imgpts);*/

		// save results
		FileStorage fs("./Data/Tip_location.xml", FileStorage::WRITE);
		fs << "tip_c" << cam1->tip_c.clone();
		fs << "tip_imgpnts" << cam1->tip_c_imgpts;
		fs << "cam1_project_matrix" << cam1->projection_matrix;
		fs << "cam2_project_matrix" << cam2->projection_matrix;
		fs << "R" << R;
		fs << "T" << T;
		fs.release();
	}
	else {
		// Read  results
		FileStorage fs("./Data/Tip_location.xml", FileStorage::READ);
		fs["tip_c"] >> cam1->tip_c;
		fs["tip_imgpnts"] >> cam1->tip_c_imgpts;
		fs["cam1_project_matrix"] >> cam1->projection_matrix;
		fs["cam2_project_matrix"] >> cam2->projection_matrix;
		fs["R"] >> R;
		fs["T"] >> T;
		fs.release();

		circle(cam1->image, cam1->tip_c_imgpts[0], 5, cv::Scalar(0, 0, 255), 1, 4);
		cv::imshow("cam1 Tip location", cam1->image);
		cv::waitKey(0);
		destroyWindow("cam1 Tip location");
	}
	
	return 0;
}

int poseEstimationTriangulate(MyCam* cam1, MyCam* cam2, MyPrt* myPrinter, bool DEBUG, Mat R, Mat T, bool DEFORM, myPCA* deformObj) {
	if (cam1->myTarget.num_active_kpt >= 4) {
		cam1->feature3D.clear();
		cv::Mat homo_result;
		double x, y, z;
		// Triangulation the recognized keypoints
		triangulatePoints(cam1->projection_matrix, cam2->projection_matrix, cam1->keyPnts_img_4PnP, cam2->keyPnts_img_4PnP, homo_result);
		homo_result = homo_result.reshape(0);
		for (int i = 0; i < cam1->myTarget.num_active_kpt; i++) {
			x = (double)(homo_result.row(0).col(i).at<float>(0, 0) / homo_result.row(3).col(i).at<float>(0, 0));
			y = (double)(homo_result.row(1).col(i).at<float>(0, 0) / homo_result.row(3).col(i).at<float>(0, 0));
			z = (double)(homo_result.row(2).col(i).at<float>(0, 0) / homo_result.row(3).col(i).at<float>(0, 0));
			cam1->feature3D.push_back(Point3d(x, y, z));
			//cout << "Triangulated pnt " << i << ": " << x << "," << y << "," << z << endl;
		}
		cv::Mat point3D = Mat(cam1->feature3D).reshape(1).t(); //3xn
		cv::Mat point3D_cent;
		cv::reduce(point3D, point3D_cent, 1, CV_REDUCE_AVG);
		cv::Mat point3D_cent_repeat;
		cv::repeat(point3D_cent, 1, cam1->myTarget.num_active_kpt, point3D_cent_repeat);
		cv::Mat point3D_4svd = point3D - point3D_cent_repeat;

		/*cv::Mat point3D_1 = point3D(cv::Rect(0, 0, 3, cam1->myTarget.num_active_kpt - 1)).clone();
		cv::Mat point3D_2 = point3D(cv::Rect(0, 1, 3, cam1->myTarget.num_active_kpt - 1)).clone();
		cv::Mat point3D_diff = point3D_2 - point3D_1;*/

		cv::Mat target3D = Mat(cam1->target_obj_4PnP).reshape(1).t(); //3xn
		target3D.convertTo(target3D, CV_64F);
		cv::Mat target3D_cent;
		cv::reduce(target3D, target3D_cent, 1, CV_REDUCE_AVG);
		cv::Mat target3D_cent_repeat;
		cv::repeat(target3D_cent, 1, cam1->myTarget.num_active_kpt, target3D_cent_repeat);
		cv::Mat target3D_4svd = target3D - target3D_cent_repeat;

		// Find rotation
		/*cv::Mat target3D_4svd_inv;
		cv::invert(target3D_4svd, target3D_4svd_inv, cv::DECOMP_SVD);
		cv::Mat e, U, V;
		cv::SVDecomp(point3D_4svd * target3D_4svd_inv, e, U, V, cv::SVD::FULL_UV);
		if (DEBUG) {
			cout << "SVD of C_w2p, e = " << endl << " " << e << endl << endl;
		}
		Mat new_e = Mat::eye(cam1->myTarget.num_active_kpt, cam1->myTarget.num_active_kpt, CV_64F);
		new_e.at<double>(cam1->myTarget.num_active_kpt - 1, cam1->myTarget.num_active_kpt - 1) = cv::determinant((V.t())*(U.t()));
		cam1->target_rmats = (V.t())*new_e*(U.t());*/

		// Compute rmat and tvec for tracking cam
		if (rotationFromQuaternion(&point3D_4svd, &target3D_4svd, cam1->target_rmats, &(cam1->quat)) == -1) { return -1;}
		cam1->target_tvecs = point3D_cent - cam1->target_rmats * target3D_cent;
		Rodrigues(cam1->target_rmats, cam1->target_rvecs);
		// Compute rmat and tvec for assistant cam
		cam2->target_tvecs = T + R * cam1->target_tvecs;
		cam2->target_rmats = R * cam1->target_rmats;
		Rodrigues(cam2->target_rmats, cam2->target_rvecs);

		// --------------------------- For deformable -----------------------------------------------
		if (DEFORM) {
			// Transfer marker pnts to target coordinate
			cv::Mat cam1_target_tvecs_repeat;
			cv::repeat(cam1->target_tvecs, 1, cam1->myTarget.num_active_kpt, cam1_target_tvecs_repeat);
			cv::Mat point3D_t = (cam1->target_rmats.t())*(point3D - cam1_target_tvecs_repeat);
			cv::Mat point3D_residue = point3D_t - target3D;
			//cout << "point3d res: " << endl << point3D_residue << endl << endl;
			cv::Mat marker_deform_vec = (point3D_residue - (Mat(deformObj->deform_norm_4PnP).reshape(1)).t()).t(); // nx3
			//find the parameters for deformation
			deformObj->parameters = (deformObj->eigenVectors_sub.t() * deformObj->eigenVectors_sub).inv() * (deformObj->eigenVectors_sub.t() * (marker_deform_vec.reshape(1, 3 * cam1->myTarget.num_active_kpt)));
			//std::cout << "parameters: " << deformObj->parameters.t() << std::endl;

			// Get the least square residue
			deformObj->LS_res = deformObj->eigenVectors_sub * deformObj->parameters - marker_deform_vec.reshape(1, 3 * cam1->myTarget.num_active_kpt);
			// cout << "LS_residue: " << deformObj->LS_res.t() << endl;
		}
		// -------------------------------------------------------------------------------------------


		// Project plot
		if (DEBUG) {
			project_xyzAxes(&(cam1->image), &cam1->target_rvecs, &cam1->target_tvecs, &cam1->intrinsic, &cam1->distCoeffs, cam1->myTarget.target_axis);
			//plot toolpath and tip locaiton to image
			if (!DEFORM) {
				project_pnts(&(cam1->image), &cam1->target_rvecs, &cam1->target_tvecs, &cam1->intrinsic, &cam1->distCoeffs, &(cam1->myToolpath.toolpath_pnt), &(cam1->myToolpath.toolpath_pnt_imgpnt));
			}
			else {
				cv::Mat new_toolpath = (deformObj->eigenVectors(cv::Rect(0, 3 * cam1->myTarget.pnt_num, deformObj->eigenVectors.cols, deformObj->eigenVectors.rows - 3 * cam1->myTarget.pnt_num)) * deformObj->parameters) + deformObj->ref_and_deform_1d_mat(cv::Rect(0, 3 * cam1->myTarget.pnt_num, 1, deformObj->ref_and_deform_1d_mat.rows - 3 * cam1->myTarget.pnt_num));
				new_toolpath = (new_toolpath.reshape(3, new_toolpath.rows/3)).reshape(1); //Nx1 3-channels to Nx3 1-channel
				std::vector<cv::Point3f> new_toolpath_vec;
				//for (int row_idx = 0; row_idx < 1; row_idx++) {
				for (int row_idx = 0; row_idx < new_toolpath.rows; row_idx++) {
					new_toolpath_vec.push_back(Point3f(new_toolpath.row(row_idx)));
				}
				project_pnts(&(cam1->image), &cam1->target_rvecs, &cam1->target_tvecs, &cam1->intrinsic, &cam1->distCoeffs, &(new_toolpath_vec), &(cam1->myToolpath.toolpath_pnt_imgpnt));
				/*cv::projectPoints(new_toolpath_vec, cam1->target_rvecs, cam1->target_tvecs, cam1->intrinsic, cam1->distCoeffs, cam1->myToolpath.toolpath_pnt_imgpnt);
				draw_pnts(&(cam1->image), &(cam1->myToolpath.toolpath_pnt_imgpnt));*/

				//-----project the marker points for ROI casting------
				deformObj->deformed_keyPnts.clear();
				cv::Mat new_def_kpt = (deformObj->eigenVectors(cv::Rect(0, 0, deformObj->eigenVectors.cols, 3 * cam1->myTarget.pnt_num)) * deformObj->parameters) + deformObj->ref_and_deform_1d_mat(cv::Rect(0, 0, 1, 3 * cam1->myTarget.pnt_num));
				new_def_kpt = (new_def_kpt.reshape(3, new_def_kpt.rows / 3)).reshape(1); //Nx1 3-channels to Nx3 1-channel
				//for (int row_idx = 0; row_idx < 1; row_idx++) {
				for (int row_idx = 0; row_idx < new_def_kpt.rows; row_idx++) {
					deformObj->deformed_keyPnts.push_back(Point3f(new_def_kpt.row(row_idx)));
				}
			}

			/*vector<Point2f> target_reproject;
			project_pnts(&(cam1->image), &cam1->target_rvecs, &cam1->target_tvecs, &cam1->intrinsic, &cam1->distCoeffs, &(cam1->target_obj_4PnP), &(target_reproject));*/

			//plot tip locaiton to image
			//project_tip(&intrinsic, &distCoeffs, &tip_c, &tip_c_imgpts);
			circle(cam1->image, cam1->tip_c_imgpts[0], 5, cv::Scalar(0, 0, 255), 1, 4);

		}

		// compute p_o2p
		cam1->p_t2p = Mat(myPrinter->posData, 1); // when the image was captured

		/*cout << "d p_t2p: " << endl << p_t2p - last_p_t2p << endl;
		last_p_t2p = p_t2p.clone();*/

		cam1->p_o2p = cam1->p_t2p - cam1->C_c2p * cam1->tip_c + cam1->C_c2p * cam1->target_tvecs; // Problematic here !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! p_t2p(printer time) and target_tvecs (camera time) are not sync

	}
	else {
		return -1;
	}
	return 0;
}

int detectKeyPntTriangulation(MyCam* cam1, MyCam* cam2, bool DEBUG, bool DEFORM, myPCA* deformObj) {
	unsigned int succeed_count = 0;

	// Prepare for new detection
	cam1->keyPnts_img_4PnP.clear();
	cam2->keyPnts_img_4PnP.clear();
	cam1->target_obj_4PnP.clear();
	cam2->target_obj_4PnP.clear();

	cv::Mat temp_eigen;
	int temp_eigen_idx = 0;
	if (DEFORM) {
		deformObj->deform_norm_4PnP.clear();
		temp_eigen = Mat::zeros(cam1->myTarget.pnt_num * 3, deformObj->num_eigenvect, CV_64F);
	}

	for (int i = 0; i < cam1->myTarget.pnt_num; i++) {
		// Adjust searching region (boundary case)
		cam1->myTarget.keyPnts[i].ROI_range_casting(cam1->image.rows, cam1->image.cols);
		cam2->myTarget.keyPnts[i].ROI_range_casting(cam2->image.rows, cam2->image.cols);
		if (!cam1->myTarget.keyPnts[i].detectROI(&(cam1->image), DEBUG) && !cam2->myTarget.keyPnts[i].detectROI(&(cam2->image), DEBUG)) {
			// succeed in detection
			cam1->keyPnts_img_4PnP.push_back(cam1->myTarget.keyPnts_img[i]);
			cam1->target_obj_4PnP.push_back(cam1->myTarget.target_obj[i]);
			cam2->keyPnts_img_4PnP.push_back(cam2->myTarget.keyPnts_img[i]);
			cam2->target_obj_4PnP.push_back(cam2->myTarget.target_obj[i]);


			if (DEFORM) {
				deformObj->deform_norm_4PnP.push_back(cv::Point3d(deformObj->deform_norm_1d[3*i], deformObj->deform_norm_1d[3 * i + 1], deformObj->deform_norm_1d[3 * i + 2] ));
				deformObj->eigenVectors.row(3 * i).copyTo(temp_eigen.row(3*temp_eigen_idx));
				deformObj->eigenVectors.row(3 * i+1).copyTo(temp_eigen.row(3*temp_eigen_idx+1));
				deformObj->eigenVectors.row(3 * i+2).copyTo(temp_eigen.row(3*temp_eigen_idx+2));
				temp_eigen_idx++;
			}

			succeed_count++;
		}
	}

	if (DEFORM) {
		deformObj->eigenVectors_sub = temp_eigen(cv::Rect(0, 0, temp_eigen.cols, 3*temp_eigen_idx)).clone();
	}
	
	cam1->myTarget.num_active_kpt = succeed_count;
	cam2->myTarget.num_active_kpt = succeed_count;
	if (DEBUG) { std::cout << "    num of keypnt: " << cam1->myTarget.num_active_kpt << endl; }
	return 0;
}

void init_handeye_traj_manual(vector<vector<DOUBLE>>* traj) {
	int num_pnt_handeye = 10;
	traj->reserve(num_pnt_handeye);
	vector<DOUBLE> temp_pos = { -3.5, 0., 42. };
	traj->push_back(temp_pos);
	temp_pos = { -3.5, 3.5, 42. };
	traj->push_back(temp_pos);
	temp_pos = { -3.5, 7., 42. };
	traj->push_back(temp_pos);
	temp_pos = { 0., 7., 42. };
	traj->push_back(temp_pos);
	temp_pos = { 5., 7., 42. };
	traj->push_back(temp_pos);
	temp_pos = { 5., 7., 47. };
	traj->push_back(temp_pos);
	temp_pos = { 0., 7., 47. };
	traj->push_back(temp_pos);
	temp_pos = { -3.5, 7., 47. };
	traj->push_back(temp_pos);
	temp_pos = { -3.5, 3.5, 47. };
	traj->push_back(temp_pos);
	temp_pos = { -3.5, 0., 47. };
	traj->push_back(temp_pos);
}

int rotationFromQuaternion(Mat* pnt, Mat* target, Mat& R, std::vector<double>* quaternion) {
	double Sxx = target->row(0).dot(pnt->row(0));
	double Syy = target->row(1).dot(pnt->row(1));
	double Szz = target->row(2).dot(pnt->row(2));
	double Sxy = target->row(0).dot(pnt->row(1));
	double Syx = target->row(1).dot(pnt->row(0));
	double Sxz = target->row(0).dot(pnt->row(2));
	double Szx = target->row(2).dot(pnt->row(0));
	double Syz = target->row(1).dot(pnt->row(2));
	double Szy = target->row(2).dot(pnt->row(1));
	//Mat P = (Mat_<double>(4, 4) << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx,
	//								Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz,
	//								Szx - Sxz, Sxy + Syx, Syy - Sxx - Szz, Syz + Szy,
	//								Sxy - Syx, Szx + Sxz, Syz + Szy, Szz - Sxx - Syy); 
	//cout << "P: " << P << endl << endl;
	//cv::Mat e, U, V;
	//cv::SVD::compute(P, e, U, V);
	////SVDecomp(P, e, U, V, cv::SVD::FULL_UV);
	//cout << "U: " << U << endl << endl;
	//cout << "Eigen computed from quaternion: " << e << endl << endl;
	//vector<double> q;
	//U.col(0).copyTo(q);
	//Mat R_q = (Mat_<double>(3, 3) << q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3], 2 * (q[1] * q[2] - q[0] * q[3]), 2 * (q[1] * q[3] + q[0] * q[2]),
	//								2 * (q[2] * q[1] + q[0] * q[3]), q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3], 2 * (q[2] * q[3] - q[0] * q[1]), 
	//								2 * (q[3] * q[1] - q[0] * q[2]), 2 * (q[3] * q[2] + q[0] * q[1]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
	//cout << "R_q: " << R_q << endl << endl;
	//R = R_q.clone();

	Eigen::Matrix4d Pp;
	Pp << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx,
		Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz,
		Szx - Sxz, Sxy + Syx, Syy - Sxx - Szz, Syz + Szy,
		Sxy - Syx, Szx + Sxz, Syz + Szy, Szz - Sxx - Syy;
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(Pp);
	if (eigensolver.info() == Eigen::Success) {
		Eigen::Vector4d q = (eigensolver.eigenvectors()).col(3);
		/*cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
		cout << "Here's a matrix whose columns are eigenvectors of A \n"
			<< "corresponding to these eigenvalues:\n"
			<< eigensolver.eigenvectors() << endl;*/
		Mat R_q = (Mat_<double>(3, 3) << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * (q(1) * q(2) - q(0) * q(3)), 2 * (q(1) * q(3) + q(0) * q(2)),
			2 * (q(2) * q(1) + q(0) * q(3)), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3), 2 * (q(2) * q(3) - q(0) * q(1)),
			2 * (q(3) * q(1) - q(0) * q(2)), 2 * (q(3) * q(2) + q(0) * q(1)), q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3));
		//cout << "R_q: " << R_q << endl << endl;
		R = R_q.clone();
		// output quat
		(*quaternion)[0] = q(0);
		(*quaternion)[1] = q(1);
		(*quaternion)[2] = q(2);
		(*quaternion)[3] = q(3);

		return 0;
	}
	return -1;
}

int readCycleInfo(std::string filename, std::vector<cv::Mat>& cycleParaVec, std::vector<double>& cycleZVec, int& cycle_period, int& cycle_period_step, float &max_eigen0, int &max_idx, bool feature_flag) {
	// read ref file and deform file
	std::ifstream infile(filename);
	std::string line;
	std::string temp_number;
	std::vector<std::vector<float> > cycle_para;
	float temp_value;
	int num_pnt = 0;
	int num_parameter = 0;

	while (std::getline(infile, line)) {
		// Got a line
		std::stringstream linestream(line);
		std::vector<float> temp_row;
		num_parameter = 0;
		while (linestream >> temp_number) {
			temp_value = std::strtof((temp_number).c_str(), 0);
			temp_row.push_back(temp_value);

			if (num_parameter == 1) { // eigenvelue 0
				if (feature_flag == 0) { // find max value
					if (temp_value > max_eigen0) {
						max_eigen0 = temp_value;
						max_idx = num_pnt;
					}
				}
				else { // find min value, ignore the naming of variable
					if (temp_value < max_eigen0) {
						max_eigen0 = temp_value;
						max_idx = num_pnt;
					}
				}
				
			}

			num_parameter++;
		}
		cycle_para.push_back(temp_row);
		num_pnt++;
	}

	std::cout << "cycle para: num of pnt: " << num_pnt << std::endl;
	std::cout << "cycle para: num of parameter: " << num_parameter << std::endl;
	cycle_period = (int)cycle_para[num_pnt - 1][0];
	cycle_period_step = ((int)cycle_para[1][0]) - ((int)cycle_para[0][0]);
	std::cout << "cycle para: cycle_period: " << cycle_period << std::endl;
	std::cout << "cycle para: cycle_period_step: " << cycle_period_step << std::endl;
	if (feature_flag == 0) {
		std::cout << "cycle para: max eigen 0 value: " << max_eigen0 << std::endl;
		std::cout << "cycle para: max eigen 0 idx: " << max_idx << std::endl;
	}
	else {
		std::cout << "cycle para: min eigen 0 value: " << max_eigen0 << std::endl;
		std::cout << "cycle para: min eigen 0 idx: " << max_idx << std::endl;
	}
	

	// Extract parameters and rigid motions
	cv::Mat tempCycleParaMat(1, num_parameter-4, CV_64F); // minus time and three rigid ranslation
	for (int row = 0; row < num_pnt; row++) {
		for (int col = 0; col < num_parameter-4; col++) {
			tempCycleParaMat.at<double>(col) = (double)cycle_para[row][col+1];
		}
		//cout << "para " << tempCycleParaMat << endl;
		cycleParaVec.push_back(tempCycleParaMat.clone());
		cycleZVec.push_back((double)cycle_para[row][num_parameter-1]);
	}

	infile.close();
	std::cout << "Cycle parameter read complete." << std::endl;

	return 0;
}