#include "../Header/myCam_obj.h"
#include "../Header/quaternion_operation.h"
#include <math.h>

using namespace FlyCapture2;
using namespace cv;
using namespace std;

MyCam::MyCam() {
	cout << "MyCam object created." << endl;

	// init moving avg filter
	Mat temp_zeros = cv::Mat::zeros(3, 1, CV_64F);
	vector<double> temp_zero_vec{ 0.,0.,0. };

	for (int i = 0; i < moving_avg_filter_size; i++) {
		tracking_avg_window.push(temp_zeros);
	}
	for (int i = 0; i < dp_o2p_filter_size; i++) {
		dp_o2p_avg_window.push(temp_zeros);
		dt_avg_window.push(0.);
		angular_vel_avg_window.push(temp_zero_vec);
	}
}

MyCam::~MyCam() {
	// Disconnect the camera
	error = cam.Disconnect();
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return;
	}
	cout << "MyCam object destructed." << endl;
}

int MyCam::Configure(PGRGuid guid, bool Timing_F) {

	// Connect to a camera
	error = cam.Connect(&guid);

	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}

	// Get the camera information
	error = cam.GetCameraInfo(&camInfo);
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}

	PrintCameraInfo(&camInfo);

	if (Timing_F) {
		// Turn Timestamp on
		EmbeddedImageInfo imageInfo;
		imageInfo.timestamp.onOff = true;
		error = cam.SetEmbeddedImageInfo(&imageInfo);
		if (error != PGRERROR_OK)
		{
			PrintError(error);
			return -1;
		}
	}

	// Get the camera configuration
	FC2Config config;
	error = cam.GetConfiguration(&config);
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}

	// Set to drop mode
	config.grabMode = DROP_FRAMES;

	//// Set the number of driver buffers used to 10.
	//config.numBuffers = 10;

	// Set the camera configuration
	error = cam.SetConfiguration(&config);
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}
	std::cout << "Camera config done." << std::endl;

	return 0;
}

int MyCam::StartStreaming() {
	// Start capturing images
	error = cam.StartCapture();
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}
	std::cout << "Streaming started." << std::endl;
	return 0;
}

int MyCam::CaptureImage(MyPrt* myPrinter, bool Timing_F, bool INIT) {
	if (Timing_F) {
		myTimer = clock();
		vector<double> temp_timing_vec;
		timing.push_back(temp_timing_vec);
	}
	// Get prt pos
	myPrinter->GetPos();

	// Get toolpath time stamp
	getPosTime = clock(); // also for computing time delay till cmd
	myToolpath.toolpath_timestamp = getPosTime - myToolpath.toolpath_tmr;

	// Reproject, for updated ROI, in tracking, not calibration
	if (!INIT){
		projectPoints(myTarget.target_obj, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
	}

	if (Timing_F) {
		// Time taken for taking image and getting pos
		//std::cout << "    Till getting pos : " << clock() - myTimer << endl;
		timing[timing.size() - 1].push_back(clock() - myTimer);
	}

	// Get the image
	error = cam.RetrieveBuffer(&rawImage);
	if (error != PGRERROR_OK)
	{
		std::cout << "capture error" << std::endl;
		return -1;
	}

	// PC system clock
	currentStamp_ms = rawImage.GetTimeStamp().microSeconds / 1000.;

	if (currentStamp_ms - lastStamp_ms > 0) { // check if tmr overflow
		stampDuration_ms = currentStamp_ms - lastStamp_ms;
	}
	//std::cout << "    TStamp: " << currentStamp_ms << endl;
	//std::cout << "    Sys clock - TStamp: " << clock() - currentStamp_ms << endl; // offset between cam tmr and sys tmr
	//std::cout << "    TStamp duration: " << stampDuration_ms << endl;
	lastStamp_ms = currentStamp_ms;
	if (Timing_F) {
		timing[timing.size() - 1].push_back(stampDuration_ms);
		timing[timing.size() - 1].push_back(currentStamp_ms);
	}

	//if (Timing_F) {
	//	//// Embedded time stamp
	//	//currentEmbeddedStamp = rawImage.GetMetadata().embeddedTimeStamp;
	//	//std::cout << "    EmbeddedTStamp: " << currentEmbeddedStamp - lastEmbeddedStamp << endl;
	//	//lastEmbeddedStamp = currentEmbeddedStamp;

	//	// time taken till raw image (PC clock)
	//	std::cout << "    Till CaptureRaw: " << clock() - myTimer << endl;
	//}

	// convert to rgb
	rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage);
	//rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &rgbImage);

	/*if (Timing_F) {
		std::cout << "    Till ConvertBGR: " << clock() - myTimer << endl;
	}*/

	// convert to OpenCV Mat
	rowBytes = (unsigned int)((double)rgbImage.GetReceivedDataSize() / (double)rgbImage.GetRows());
	image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(), rowBytes);
	//image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC1, rgbImage.GetData(), rowBytes);

	if (Timing_F) {
		//std::cout << "    Till ConvertMat: " << clock() - myTimer << std::endl;
		timing[timing.size() - 1].push_back(clock() - myTimer);
	}

	return 0;
}

int MyCam::CaptureImage1_gettingPos(MyPrt* myPrinter, bool Timing_F, bool INIT, bool DEFORM, myPCA* deformObj) {
	if (Timing_F) {
		myTimer = clock();
		vector<double> temp_timing_vec;
		timing.push_back(temp_timing_vec);
	}
	// Get prt pos
	myPrinter->GetPos();

	// Get toolpath time stamp
	getPosTime = clock(); // also for computing time delay till cmd
	myToolpath.toolpath_timestamp = getPosTime - myToolpath.toolpath_tmr;

	// Reproject, for updated ROI, in tracking, not calibration
	if (!INIT) {
		if (DEFORM) {
			projectPoints(deformObj->deformed_keyPnts, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
		}
		else {
			projectPoints(myTarget.target_obj, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
		}
	}

	if (Timing_F) {
		// Time taken for taking image and getting pos
		//std::cout << "    Till getting pos : " << clock() - myTimer << endl;
		timing[timing.size() - 1].push_back(clock() - myTimer);
	}

	return 0;
}

int MyCam::CaptureImage1_noGettingPos(bool Timing_F, bool INIT, bool DEFORM, myPCA* deformObj) {
	if (Timing_F) {
		myTimer = clock();
		vector<double> temp_timing_vec;
		timing.push_back(temp_timing_vec);
	}
	// Get toolpath time stamp
	getPosTime = clock(); // also for computing time delay till cmd
	myToolpath.toolpath_timestamp = getPosTime - myToolpath.toolpath_tmr;

	// Reproject, for updated ROI, in tracking, not calibration
	if (!INIT) {
		if (DEFORM) {
			projectPoints(deformObj->deformed_keyPnts, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
		}
		else {
			projectPoints(myTarget.target_obj, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
		}
	}

	if (Timing_F) {
		// Time taken for taking image and getting pos
		//std::cout << "    Till getting pos : " << clock() - myTimer << endl;
		timing[timing.size() - 1].push_back(clock() - myTimer);
	}

	return 0;
}

int MyCam::CaptureImage2_onlyCapture(bool Timing_F) {
	// Get the image
	error = cam.RetrieveBuffer(&rawImage);
	if (error != PGRERROR_OK)
	{
		std::cout << "capture error" << std::endl;
		return -1;
	}

	// PC system clock
	currentStamp_ms = rawImage.GetTimeStamp().microSeconds / 1000.;

	if (currentStamp_ms - lastStamp_ms > 0) { // check if tmr overflow
		stampDuration_ms = currentStamp_ms - lastStamp_ms;
	}
	//std::cout << "    TStamp: " << currentStamp_ms << endl;
	//std::cout << "    Sys clock - TStamp: " << clock() - currentStamp_ms << endl; // offset between cam tmr and sys tmr
	//std::cout << "    TStamp duration: " << stampDuration_ms << endl;
	lastStamp_ms = currentStamp_ms;
	if (Timing_F) {
		timing[timing.size() - 1].push_back(stampDuration_ms);
		timing[timing.size() - 1].push_back(currentStamp_ms);
	}
	return 0;
}

int MyCam::CaptureImage3_postCapturing(bool Timing_F) {
	// convert to rgb
	rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage);
	//rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &rgbImage);

	/*if (Timing_F) {
	std::cout << "    Till ConvertBGR: " << clock() - myTimer << endl;
	}*/

	// convert to OpenCV Mat
	rowBytes = (unsigned int)((double)rgbImage.GetReceivedDataSize() / (double)rgbImage.GetRows());
	image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(), rowBytes);
	//image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC1, rgbImage.GetData(), rowBytes);

	if (Timing_F) {
		//std::cout << "    Till ConvertMat: " << clock() - myTimer << std::endl;
		timing[timing.size() - 1].push_back(clock() - myTimer);
	}

	return 0;
}

int MyCam::StopStream() {
	// Stop capturing images
	error = cam.StopCapture();
	if (error != PGRERROR_OK)
	{
		PrintError(error);
		return -1;
	}
	return 0;
}

int MyCam::CamCalibration(bool Timing_F, bool Read_file) {
	stringstream my_serial_num;
	my_serial_num << camInfo.serialNumber;

	if (!Read_file) {
		// ------------------- Variable init --------------------------------------------------------
		// Timer for debug
		clock_t chessCalibTimer;

		//chessboard config
		double chess_square_size = 4.08; // mm
		int numBoards = 20; // num of board to be detected
		int numCornersHor = 9;
		int numCornersVer = 6;
		int numSquares = numCornersHor * numCornersVer;
		Size board_sz = Size(numCornersHor, numCornersVer);
		vector<Point2f> corners; // recognized features
		vector<Point3f> obj; // prior knowledge of features
		vector<Point3f> chess_axis = { Point3f(0., 0., 0.), Point3f(10., 0., 0.),Point3f(0., 10., 0.),Point3f(0., 0., 10.) }; // o-x-y-z axes, mm

		for (int j = 0; j < numSquares; j++)
			obj.push_back(chess_square_size*Point3f((float)(j / numCornersHor), (float)(j%numCornersHor), 0.0f));

		// Container for sequence of calib images
		vector<vector<Point3f>> object_points;
		vector<vector<Point2f>> image_points;
		vector<Mat> chess_rvecs;
		vector<Mat> chess_tvecs;

		//flags for success in each step
		bool found = 0; // found the chess board in calib
		bool test_found = 0; // found the chess board in test
		bool found_PnP = 0; // successful PnP in test

		// Init cvImg for processing/visualization
		Mat chess_image;
		Mat chess_gray_image;
		Mat chess_gray_image_undist;

		Mat test_image;
		Mat test_image_gray;
		Mat test_rvecs;
		Mat test_tvecs;

		// ------------------- Calibration --------------------------------------------------------
		// Chess board detection
		stringstream file_idx; // for reading files in sequence
		for (int i = 0; i < numBoards; i++)
		{
			file_idx << i + 1;
			cout << "Reading: new_image_"<< camInfo.serialNumber <<"/calib_" << file_idx.str() << ".jpg, ";
			chess_image = cv::imread("new_image_" + my_serial_num.str() + "/calib_" + file_idx.str() + ".jpg", CV_LOAD_IMAGE_COLOR);
			file_idx.str("");
			//chess_image = cv::imread("image/17221483-2018-04-04-150007.jpg", CV_LOAD_IMAGE_COLOR);

			cv::cvtColor(chess_image, chess_gray_image, CV_BGR2GRAY);

			found = findChessboardCorners(chess_gray_image, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

			if (found)
			{
				cornerSubPix(chess_gray_image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
				drawChessboardCorners(chess_image, board_sz, corners, found);

				image_points.push_back(corners);
				object_points.push_back(obj);
				cout << "Snap stored!" << endl;
			}

			imshow("Image with chessboard", chess_image);
			waitKey(1);
		}

		// Calibration
		cv::calibrateCamera(object_points, image_points, chess_image.size(), intrinsic, distCoeffs, chess_rvecs, chess_tvecs);
		cout << "Calibration done." << endl;

		// ------------------- Test --------------------------------------------------------
		// Test calibration results by undistroting the last image
		chessCalibTimer = clock();
		undistort(chess_gray_image, chess_gray_image_undist, intrinsic, distCoeffs);
		if (Timing_F) {
			cout << "Time taken for undistortion: " << clock() - chessCalibTimer << endl;
		}
		imshow("Original gray", chess_gray_image);
		imshow("Undistorted gray", chess_gray_image_undist);
		waitKey(1);

		// Test calibration results by reprojecting axes to the last image
		/*cout << "rvec num: " << chess_rvecs.size() << ", tvec_num: " << chess_tvecs.size() << endl;
		Mat temp_rvecs = chess_rvecs[chess_rvecs.size() - 1];
		Mat temp_tvecs = chess_tvecs[chess_tvecs.size() - 1];
		project_xyzAxes(&chess_image, &temp_rvecs, &temp_tvecs, &intrinsic, &distCoeffs, &corners);*/

		// Test calibration results by reprojecting axes to the test image
		cout << "Reading: test_chess.jpg" << endl;
		test_image = cv::imread("new_image_" + my_serial_num.str() + "/test_chess.jpg", CV_LOAD_IMAGE_COLOR);
		cv::cvtColor(test_image, test_image_gray, CV_BGR2GRAY);
		test_found = findChessboardCorners(test_image_gray, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
		if (test_found)
		{
			cornerSubPix(test_image_gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
			cout << "Test image feature detect succeeded. " << endl;

			if (Timing_F) {
				chessCalibTimer = clock();
			}

			found_PnP = solvePnP(obj, corners, intrinsic, distCoeffs, test_rvecs, test_tvecs);

			if (Timing_F) {
				cout << "Time taken for PnP: " << clock() - chessCalibTimer << endl;
			}

			if (found_PnP) {
				cout << "Test image solvePnP succeeded. " << endl;
				project_xyzAxes(&test_image, &test_rvecs, &test_tvecs, &intrinsic, &distCoeffs, chess_axis);
				// Plot
				imshow("Projected", test_image);
				waitKey(0);
			}

		}
		destroyAllWindows();

		// Save the calibration results
		FileStorage fs("./Data/myCamCalib_" + my_serial_num.str() + ".xml", FileStorage::WRITE);
		fs << "intrinsic" << intrinsic.clone();
		fs << "distCoeffs" << distCoeffs.clone();
		fs.release();
	}
	else {
		// Read the calibration results
		FileStorage fs("./Data/myCamCalib_" + my_serial_num.str() + ".xml", FileStorage::READ);
		fs["intrinsic"] >> intrinsic;
		fs["distCoeffs"] >> distCoeffs;
		fs.release();
	}

	std::cout << "Calibration done." << std::endl;
	return 0;
}

// For getting user input for keypoint position init
void mouseHandler(int event, int x, int y, int flags, void *ptr) {
	if (event == CV_EVENT_LBUTTONDOWN) {
		Manual_ROI*p = (Manual_ROI*)ptr;
		p->pnt_ROI[p->current_idx].x = (float)x;
		p->pnt_ROI[p->current_idx].y = (float)y;
		p->current_idx++;
		cout << "Picked point: " << p->current_idx << ", " << x << ", " << y << endl;
	}
}

int MyCam::initKeyPnt(bool readfile_f, std::string file_name) {
	// Init target info
	if (readfile_f) {
		myTarget.initTarget_readfile(file_name);
	}
	else {
		myTarget.initTarget8Dots(14.09);
	}
	
	// Init region of interest
	myPnt_ROI.Manual_ROI_init(myTarget.pnt_num);
	// Manual input
	while (myPnt_ROI.current_idx < myTarget.pnt_num) {
		cv::imshow("user_ROI", image);
		cv::waitKey(1);
		setMouseCallback("user_ROI", mouseHandler, &myPnt_ROI);
	}
	destroyWindow("user_ROI");

	// Init keypoint
	for (int i = 0; i < myTarget.pnt_num; i++) {
		myTarget.keyPnts[i].configKeyPnt(window_size_h, i, &(myPnt_ROI.pnt_ROI[i]));
		myTarget.keyPnts_img[i] = *(myTarget.keyPnts[i].keypnt); // For PnP
		myTarget.keyPnts[i].keypnt = &(myTarget.keyPnts_img[i]); // Reference the pointer back to keyPnts_img
	}

	// Get ready for next manual init
	myPnt_ROI.current_idx = 0;

	return 0;
}

int MyCam::detectKeyPnt(bool DEBUG) {
	unsigned int succeed_count = 0;

	// Prepare for new detection
	keyPnts_img_4PnP.clear();
	target_obj_4PnP.clear();

	for (int i = 0; i < myTarget.pnt_num; i++) {
		// Adjust searching region (boundary case)
		myTarget.keyPnts[i].ROI_range_casting(image.rows, image.cols);
		if (!myTarget.keyPnts[i].detectROI(&image, DEBUG)) {
			// succeed in detection
			keyPnts_img_4PnP.push_back(myTarget.keyPnts_img[i]);
			target_obj_4PnP.push_back(myTarget.target_obj[i]);
			succeed_count++;
		}
	}
	myTarget.num_active_kpt = succeed_count;
	if (DEBUG) { cout << "    num of keypnt: " << myTarget.num_active_kpt << endl; }
	return 0;
}

int MyCam::poseEstimation(MyPrt* myPrinter, bool DEBUG, bool Prediction, bool INIT) {
	if (DEBUG) { cout << "    " << myTarget.num_active_kpt << " points found." << endl; }
	if (myTarget.num_active_kpt >= 4) {
		bool solvePnP_flag = 0;
		
		if (INIT) {
			solvePnP_flag = solvePnP(target_obj_4PnP, keyPnts_img_4PnP, intrinsic, distCoeffs, target_rvecs, target_tvecs);
		}
		else {
			solvePnP_flag = solvePnP(target_obj_4PnP, keyPnts_img_4PnP, intrinsic, distCoeffs, target_rvecs, target_tvecs, 1);
		}

		if (solvePnP_flag) {
			
			if (cv::norm(target_tvecs, cv::NORM_L2) > 175 || cv::norm(target_tvecs, cv::NORM_L2) < 75) { // around 125 mm for camera M
				std::cout << "num_keypnt: " << myTarget.num_active_kpt << ", " << "tvec_norm: " << cv::norm(target_tvecs, cv::NORM_L2) << endl;
				return -1;
			}

			Rodrigues(target_rvecs, target_rmats);

			// Project plot
			if (DEBUG) {
				project_xyzAxes(&image, &target_rvecs, &target_tvecs, &intrinsic, &distCoeffs, myTarget.target_axis);
				//plot toolpath and tip locaiton to image
				project_pnts(&image, &target_rvecs, &target_tvecs, &intrinsic, &distCoeffs, &(myToolpath.toolpath_pnt), &(myToolpath.toolpath_pnt_imgpnt) );
				//plot tip locaiton to image
				//project_tip(&intrinsic, &distCoeffs, &tip_c, &tip_c_imgpts);
				circle(image, tip_c_imgpts[0], 5, cv::Scalar(0, 0, 255), 1, 4);

			}

			// compute p_o2p
			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();*/

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

			/*cout << "d cam_motion: " << endl << last_cam_mot_p - C_c2p * target_tvecs << endl;
			last_cam_mot_p = C_c2p * target_tvecs;*/

			if (Prediction) { // update v_o2p
				if (!INIT && stampDuration_ms!= 0) {
					dp_o2p = p_o2p - last_p_o2p;

					if (cv::norm(dp_o2p, cv::NORM_L2) > 5) {
						std::cout << "num_keypnt: " << myTarget.num_active_kpt << ", " << "dp_o2p_norm: " << cv::norm(dp_o2p, cv::NORM_L2) << endl;
						return -1;
					}

					v_o2p = 1000 * dp_o2p / stampDuration_ms; // mm/s
					
					// update moving avg filter
					dp_o2p_avg_window_sum = dp_o2p_avg_window_sum - dp_o2p_avg_window.front() + dp_o2p;
					dp_o2p_avg_window.pop();
					dp_o2p_avg_window.push(dp_o2p.clone());

					dt_avg_window_sum = dt_avg_window_sum - dt_avg_window.front() + stampDuration_ms;
					dt_avg_window.pop();
					dt_avg_window.push(stampDuration_ms);

					if (DEBUG) {
						cout << "    Position diff (o2p): " << endl << cv::norm(dp_o2p, cv::NORM_L2) << endl;
						cout << "    Position diff (o2p), sum: " << endl << cv::norm(dp_o2p_avg_window_sum, cv::NORM_L2) << endl;
						cout << "    Duration time (ms): " << stampDuration_ms << endl;
						cout << "    Duration time (ms), sum: " << dt_avg_window_sum << endl;
					}

					// angular velocity computation
					double angle = 0.;
					Rotation2Quaternion(&target_rvecs, &quat);
					ConjugateQuaternion(&last_quat, &last_quat_conj);
					MultiplyQuaternions(&quat, &last_quat_conj, &d_quat);
					Quaternion2Omega(&d_quat, stampDuration_ms, &w, &angle); //dt_avg_window_sum / dp_o2p_filter_size
					// update angular vel moving avg filter
					for (int vidx = 0; vidx < 3; vidx++) {
						angular_vel_avg_window_sum[vidx] = angular_vel_avg_window_sum[vidx] - angular_vel_avg_window.front()[vidx] + w[vidx];
					}
					angular_vel_avg_window.pop();
					angular_vel_avg_window.push(w);

					// data logging
					v_o2p_raw.push_back(v_o2p.clone());
					v_o2p = 1000 * dp_o2p_avg_window_sum / dt_avg_window_sum;
					v_o2p_mov_avg.push_back(v_o2p.clone());	

					dp_o2p_raw.push_back(dp_o2p.clone());
					t_duration_raw.push_back(stampDuration_ms);

					angular_vel.push_back(w);
					for (int vidx = 0; vidx < 3; vidx++) {
						w[vidx] = angular_vel_avg_window_sum[vidx]/ dp_o2p_filter_size;
					}
					angular_vel_avg.push_back(w);
					rot_angle.push_back(angle);
				}
				last_p_o2p = p_o2p.clone();
				last_quat = quat;
			}

			//// Reproject, for updated ROI
			//projectPoints(myTarget.target_obj, target_rvecs, target_tvecs, intrinsic, distCoeffs, myTarget.keyPnts_img);
			return 0;
		}
	}

	cout << "Solve PnP failed. " << endl;
	return -1;
	
}


// After camera setup (calib) and streaming started
int MyCam::HandEyeCalib(MyPrt* myPrinter, std::vector<std::vector<DOUBLE>> *prt_traj, bool read_file, bool verify_f) {

	stringstream my_serial_num;
	my_serial_num << camInfo.serialNumber;

	vector<DOUBLE> speed = { 10., 10., 10. }; // moving speed for handeye
	bool Timing_F = 0; // no need for timing
	Mat calib_gray_image; // temp
	bool found; // flag for corner detection
	bool found_PnP; // flag for PnP

					//chessboard config
	double chess_square_size = 4.08; // mm
	int numCornersHor = 9;
	int numCornersVer = 6;
	int numSquares = numCornersHor * numCornersVer;
	Size board_sz = Size(numCornersHor, numCornersVer);
	vector<Point2f> corners; // recognized features
	vector<Point3f> obj; // prior knowledge of features
	for (int j = 0; j < numSquares; j++) { obj.push_back(chess_square_size*Point3f((float)(j / numCornersHor), (float)(j%numCornersHor), 0.0f)); }
	vector<Point3f> chess_axis = { Point3f(0., 0., 0.), Point3f(10., 0., 0.),Point3f(0., 10., 0.),Point3f(0., 0., 10.) }; // o-x-y-z axes, mm

	Mat tmp_rvecs;
	Mat tmp_tvecs;
	Mat tmp_rmat;

	if (!read_file) {
		vector<vector<DOUBLE>> p_prt(prt_traj->size(), vector<DOUBLE>(prt_traj->size(), 1));
		vector<Mat> p_w(prt_traj->size());
		std::ostringstream name;
		// Save taken image flag
		bool saveImage_F = 0;
		// Detection of chessboard with diff prt position
		int num_success_detect = 0;
		
		for (int i = 0; i < prt_traj->size(); i++) {
			// Move prt
			myPrinter->MoveAbs(&((*prt_traj)[i]), &speed, 1); // wait for cmd to finish
			this->CaptureImage(myPrinter, Timing_F, 1);

			// save the taken image 
			if (saveImage_F) {
				name.str("");
				name << "handeye_" << i << ".jpg";
				imwrite(name.str(), image);
			}
			cout << "d" << endl;
			// Finding corners
			cv::cvtColor(image, calib_gray_image, CV_BGR2GRAY);

			found = findChessboardCorners(calib_gray_image, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

			if (found)
			{
				cornerSubPix(calib_gray_image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
				drawChessboardCorners(image, board_sz, corners, found);
				// store for triangulation
				chess_corners.push_back(corners);
				chess_obj.push_back(obj);
				// PnP
				found_PnP = solvePnP(obj, corners, intrinsic, distCoeffs, tmp_rvecs, tmp_tvecs);

				if (found_PnP) {
					cout << "Test image solvePnP succeeded. " << endl;
					Rodrigues(tmp_rvecs, tmp_rmat); // change to rotation matrix
					project_xyzAxes(&image, &tmp_rvecs, &tmp_tvecs, &intrinsic, &distCoeffs, chess_axis);

					// Update container
					p_prt[num_success_detect] = (*prt_traj)[i];
					p_w[num_success_detect] = tmp_rmat.t() * (tip_c - tmp_tvecs); // p_tip_in_world
					num_success_detect++;
					cout << "HandEye Calib: #" << i << "chessboard found!" << endl;
				}

			}
			// Plot
			imshow("HandEyeCalib", image);
			waitKey(1);
		}
		cv::destroyAllWindows();

		// hand eye calibration
		Mat p_prt_temp0;
		Mat p_prt_temp1;
		p_prt_temp0 = Mat(p_prt[0], 1);
		p_prt_temp1 = Mat(p_prt[1], 1);
		Mat p_prt_mat = p_prt_temp1 - p_prt_temp0;

		Mat p_w_mat = p_w[1] - p_w[0];

		if (num_success_detect == prt_traj->size()) {
			for (int j = 2; j < num_success_detect; j++) {
				// concat p_cam_mat
				hconcat(p_w_mat, p_w[j] - p_w[j - 1], p_w_mat);
				// concat p_prt_mat
				p_prt_temp0 = Mat(p_prt[j - 1], 1);
				p_prt_temp1 = Mat(p_prt[j], 1);
				hconcat(p_prt_mat, p_prt_temp1 - p_prt_temp0, p_prt_mat);
			}
			cout << "p_w_mat = " << endl << " " << p_w_mat << endl << endl;
			cout << "p_prt_mat = " << endl << " " << p_prt_mat << endl << endl;
		}
		else {
			p_prt_temp0.release();
			p_prt_temp1.release();
			return -1;
		}


		// Compute C_w2p
		Mat p_w_mat_inv;
		invert(p_w_mat, p_w_mat_inv, DECOMP_SVD);
		Mat C_w2p;
		C_w2p = p_prt_mat * p_w_mat_inv;
		cout << "C_w2p = " << endl << " " << C_w2p << endl << endl;
		// SVD check
		Mat e, U, V;
		SVDecomp(C_w2p, e, U, V, cv::SVD::FULL_UV);
		cout << "SVD of C_w2p, e = " << endl << " " << e << endl << endl;
		//transpose(V, V);
		C_w2p = U * Mat::eye(3, 3, CV_64F) * V;
		cout << "Adjusted C_w2p = " << endl << " " << C_w2p << endl << endl;
		// Compute C_c2p 
		C_c2p = C_w2p * tmp_rmat; // Based on the last C_w2c (tmp_rmat)
		cout << "C_c2p = " << endl << " " << C_c2p << endl << endl;
		// Print targer height (in cam frame)
		cout << "p_w2c = " << endl << " " << tmp_tvecs << endl << endl;


		imshow("HandEyeCalib", image);
		waitKey(0);
		destroyAllWindows();

		// Release memory
		p_prt_temp0.release();
		p_prt_temp1.release();
		p_w_mat_inv.release();
		e.release();
		U.release();
		V.release();

		// save results
		FileStorage fs("./Data/handEyeCalib_" + my_serial_num.str() + ".xml", FileStorage::WRITE);
		fs << "C_c2p" << C_c2p.clone();
		fs << "chess_obj" << chess_obj;
		fs << "chess_corners" << chess_corners;
		//fs << "tmp_rvecs" << tmp_rvecs; // final rvecs
		//fs << "tmp_tvecs" << tmp_tvecs; // final tvecs
		fs.release();
	}
	else {
		// Read the calibration results
		FileStorage fs("./Data/handEyeCalib_" + my_serial_num.str() + ".xml", FileStorage::READ);
		fs["C_c2p"] >> C_c2p;
		fs["chess_obj"] >> chess_obj;
		fs["chess_corners"] >> chess_corners;
		//fs["tmp_tvecs"] >> tmp_tvecs;
		//fs["tmp_rvecs"] >> tmp_rvecs;
		fs.release();
	}

	if (verify_f) {
		// Verify the precision of C_c2p
		const int num_cycle = 4;
		const int numpnt_cam_traj = 10;
		const double step_length = 2; // mm
		const double h_inc = 0.5; // mm
		const double angle_inc = 2 * PI / (numpnt_cam_traj - 1); // circular inc, rad
		double curr_angle = 0.;
		Mat last_tmp_rvecs;
		Mat last_tmp_tvecs;

		vector<DOUBLE> curr_prt_inc = vector<DOUBLE>(3);
		Mat curr_cam_desired_inc;
		Mat curr_cam_actual_inc;

		Mat cam_inc_mat;
		Mat prt_inc_mat;


		// init prt position to the last final position
		//myPrinter->MoveAbs(&((*prt_traj)[prt_traj->size() - 1]), &speed, 1); // wait for cmd to finish
		myPrinter->MoveAbs(&((*prt_traj)[0]), &speed, 1); // wait for cmd to finish
		// Init tmp_rvecs, tmp_tvecs
		this->CaptureImage(myPrinter, 0, 1);
		// Finding corners
		cv::cvtColor(image, calib_gray_image, CV_BGR2GRAY);
		found = findChessboardCorners(calib_gray_image, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
		if (found)
		{
			cornerSubPix(calib_gray_image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
			drawChessboardCorners(image, board_sz, corners, found);
			// PnP
			found_PnP = solvePnP(obj, corners, intrinsic, distCoeffs, tmp_rvecs, tmp_tvecs);
		}
		last_tmp_rvecs = tmp_rvecs.clone();
		last_tmp_tvecs = tmp_tvecs.clone();

		// Start traj
		for (int j = 0; j < num_cycle; j++) { // num of cycles
			for (int i = 0; i < numpnt_cam_traj; i++) {  // num of pnt on circle
				// current prt inc
				curr_prt_inc[0] = step_length * cos(curr_angle);
				curr_prt_inc[1] = step_length * sin(curr_angle);
				curr_angle = curr_angle + angle_inc;
				if (i == 0) { curr_prt_inc[2] = j * h_inc; }
				else { curr_prt_inc[2] = 0.; }
				// Move prt
				myPrinter->MoveInc(&curr_prt_inc, &speed, 1); // wait for cmd to finish
				if (i + j == 0) { prt_inc_mat = Mat(curr_prt_inc, 1); }
				else {
					hconcat(prt_inc_mat, Mat(curr_prt_inc, 1), prt_inc_mat); // concat p_cam_mat
				}
				// current desired cam inc
				curr_cam_desired_inc = C_c2p.t() * Mat(curr_prt_inc, 1);

				this->CaptureImage(myPrinter, 0, 1);

				// Finding corners
				cv::cvtColor(image, calib_gray_image, CV_BGR2GRAY);

				found = findChessboardCorners(calib_gray_image, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

				if (found)
				{
					cornerSubPix(calib_gray_image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
					drawChessboardCorners(image, board_sz, corners, found);
					// PnP
					found_PnP = solvePnP(obj, corners, intrinsic, distCoeffs, tmp_rvecs, tmp_tvecs);

					if (found_PnP) {
						//cout << "Test image solvePnP succeeded. " << endl;
						Rodrigues(tmp_rvecs, tmp_rmat); // change to rotation matrix
						project_xyzAxes(&image, &tmp_rvecs, &tmp_tvecs, &intrinsic, &distCoeffs, chess_axis);

						// Actual cam inc
						curr_cam_actual_inc = -tmp_tvecs + last_tmp_tvecs;
						if (i + j == 0) { cam_inc_mat = curr_cam_actual_inc.clone(); }
						else {
							hconcat(cam_inc_mat, curr_cam_actual_inc.clone(), cam_inc_mat); // concat p_cam_mat
						}
						//cout << "actual cam inc:" << endl << curr_cam_actual_inc.t() << endl;
						//cout << "desired cam inc: " << endl << curr_cam_desired_inc.t() << endl;
						if (j == num_cycle - 1) {
							cout << "error: " << endl << curr_cam_desired_inc.t() - curr_cam_actual_inc.t() << endl << endl;
						}
						last_tmp_rvecs = tmp_rvecs.clone();
						last_tmp_tvecs = tmp_tvecs.clone();
					}

				}

				// Plot
				imshow("HandEyeCalib", image);
				cv::waitKey(1);
			}
		}

		// Adjust C_c2p
		Mat cam_inc_mat_inv;
		invert(cam_inc_mat, cam_inc_mat_inv, DECOMP_SVD);
		Mat new_C_c2p = prt_inc_mat * cam_inc_mat_inv;
		cout << "old C_c2p" << endl << C_c2p << endl;
		cout << "new C_c2p" << endl << new_C_c2p << endl;
		// SVD
		Mat e, U, V;
		SVDecomp(new_C_c2p, e, U, V, cv::SVD::FULL_UV);
		cout << "SVD of new_C_c2p, e = " << endl << " " << e << endl << endl;
		//transpose(V, V);
		new_C_c2p = U * Mat::eye(3, 3, CV_64F) * V;
		cout << "Adjusted new_C_c2p = " << endl << " " << new_C_c2p << endl << endl;

		C_c2p = new_C_c2p;
		// save results
		FileStorage fs("./Data/handEyeCalib_" + my_serial_num.str() + ".xml", FileStorage::WRITE);
		fs << "C_c2p" << C_c2p.clone();
		fs << "chess_obj" << chess_obj;
		fs << "chess_corners" << chess_corners;

		cv::destroyAllWindows();

	}
	return 0;

}

int MyCam::dummy_tipLocating() {
	tip_c.at<double>(2, 0) = 135.;
	cout << "Tip_c : "<< endl << tip_c << endl;
	return 0;
}

// For getting user input for keypoint position init
void mouseHandlerTip(int event, int x, int y, int flags, void *ptr) {
	if (event == CV_EVENT_LBUTTONDOWN) {
		Point2f *p = (Point2f*)ptr;
		p->x = (float)x;
		p->y = (float)y;
		cout << "Picked point: " << x << ", " << y << endl;
	}
}

// Manually picking tip
int MyCam::tipLocating2D(bool read_tip) {
	if (!read_tip) {
		// undistort the image
		cv::Mat image_undistort;
		undistort(image, image_undistort, intrinsic, distCoeffs);
		//Create a window
		namedWindow("Picking tip location", 1);
		setMouseCallback("Picking tip location", mouseHandlerTip, &(tip_c_imgpts[0]));
		cv::imshow("Picking tip location", image_undistort);
		cv::waitKey(0);
		destroyWindow("Picking tip location");
	}
	return 0;
}

int MyCam::generateCmd(MyPrt* myPrinter, bool wait_flag, bool debug_f, bool Prediction, bool thread_enable, double z_offst, bool DEFORM, myPCA* deformObj, bool LOG_DATA) {  //, Mat target_pnt

	/*cout << "p_t2p: " << endl << " " << p_t2p << endl;
	cout << "tip_c: " << endl << " " << tip_c << endl;
	cout << " target_tvecs: " << endl << " " << target_tvecs << endl;
	cout << "C_c2p: " << endl << " " << C_c2p << endl << endl;*/

	//if (debug_f) {
	//	//plot toolpath and tip locaiton to image
	//	project_pnts(&image, &target_rvecs, &target_tvecs, &intrinsic, &distCoeffs, &(myToolpath.toolpath_pnt));
	//	//plot tip locaiton to image
	//	project_tip(&image, &intrinsic, &distCoeffs, &tip_c);
	//}

	// choose next waypoint and adjust speed
	if (!wait_flag) {
		myToolpath.travel_time = myToolpath.toolpath_time[myToolpath.toolpath_idx] - myToolpath.toolpath_timestamp;
		while (myToolpath.travel_time < 0) {
			// move to next waypoint
			//cout << "Skip one waypnt. " << endl;
			myToolpath.toolpath_idx++;
			if (myToolpath.toolpath_idx >= myToolpath.toolpath_num_waypnt) {
				cout << "    Finish printing" << endl;
				return -1;  // finish printing
			}
			myToolpath.travel_time = myToolpath.toolpath_time[myToolpath.toolpath_idx] - myToolpath.toolpath_timestamp;
		}

		if (LOG_DATA) {
			// logging tracking error
			inter_waypnt_ratio = myToolpath.travel_time / (myToolpath.toolpath_time[myToolpath.toolpath_idx] - myToolpath.toolpath_time[myToolpath.toolpath_idx - 1]);
			if (!DEFORM) {
				// desiredWaypnt = myToolpath.toolpath[myToolpath.toolpath_idx - 1] + inter_waypnt_ratio * (myToolpath.toolpath[myToolpath.toolpath_idx] - myToolpath.toolpath[myToolpath.toolpath_idx - 1]); // wrong math
				desiredWaypnt = myToolpath.toolpath[myToolpath.toolpath_idx] - inter_waypnt_ratio * (myToolpath.toolpath[myToolpath.toolpath_idx] - myToolpath.toolpath[myToolpath.toolpath_idx - 1]);
			}
		}
		//tracking_err_prt = p_o2p + C_c2p * (target_rmats * desiredWaypnt) - p_t2p;
		//// update moving avg filter
		//tracking_avg_window_sum = tracking_avg_window_sum - tracking_avg_window.front() + tracking_err_prt;
		//tracking_avg_window.pop();
		//tracking_avg_window.push(tracking_err_prt.clone());
		//tracking_err_prt = tracking_avg_window_sum / (double)moving_avg_filter_size;
		//cout << "tracking error: " << tracking_err_prt << endl;
		// Apply cmd
		QuaternionPropagate(&quat, &w, myToolpath.travel_time, &next_quat);
		// cout << "next quaternion: " << next_quat[0] << " " << next_quat[1] << " " << next_quat[2] << " " << next_quat[3] << endl;
		if (Prediction &&  w[0] * w[0] + w[1] * w[1] + w[2] * w[2] > w_thres*w_thres) {
			Quaternion2Rotation(&next_quat, &target_rmats);
		}
		

		if (Prediction && cv::norm(v_o2p, cv::NORM_L2) > v_o2p_thres) {
			current_cmd = p_o2p + C_c2p * (target_rmats * myToolpath.toolpath[myToolpath.toolpath_idx]) + v_o2p * myToolpath.travel_time / 1000.0; // computed w.r.t. the frame when the image was captured
		}
		else {
			if (!DEFORM) {
				current_cmd = p_o2p + C_c2p * (target_rmats * myToolpath.toolpath[myToolpath.toolpath_idx]);
			}
			else {
				deformed_waypnt = (deformObj->eigenVectors(cv::Rect(0, 3*(myTarget.pnt_num + myToolpath.toolpath_idx), deformObj->eigenVectors.cols,3)) * deformObj->parameters) + deformObj->ref_and_deform_1d_mat(cv::Rect(0, 3 * (myTarget.pnt_num + myToolpath.toolpath_idx), 1, 3));
				if (LOG_DATA) {
					// for tracking error logging
					last_deformed_waypnt = (deformObj->eigenVectors(cv::Rect(0, 3 * (myTarget.pnt_num + myToolpath.toolpath_idx-1), deformObj->eigenVectors.cols, 3)) * deformObj->parameters) + deformObj->ref_and_deform_1d_mat(cv::Rect(0, 3 * (myTarget.pnt_num + myToolpath.toolpath_idx-1), 1, 3));
					// desiredWaypnt = last_deformed_waypnt + inter_waypnt_ratio * (deformed_waypnt - last_deformed_waypnt); // wrong math
					desiredWaypnt = deformed_waypnt - inter_waypnt_ratio * (deformed_waypnt - last_deformed_waypnt);
				}
				//current_cmd = p_o2p + C_c2p * (target_rmats * myToolpath.toolpath[myToolpath.toolpath_idx]);
				current_cmd = p_o2p + C_c2p * (target_rmats * deformed_waypnt);
			}
			
		}

		current_cmd_vec[0] = current_cmd.at<double>(0, 0);
		current_cmd_vec[1] = current_cmd.at<double>(1, 0);
		current_cmd_vec[2] = current_cmd.at<double>(2, 0) + z_offst;

		//adjust speed (old)
		speed[0] = abs(1000 * (current_cmd_vec[0] - p_t2p.at<double>(0, 0)) / myToolpath.travel_time);
		speed[1] = abs(1000 * (current_cmd_vec[1] - p_t2p.at<double>(1, 0)) / myToolpath.travel_time);
		speed[2] = abs(1000 * (current_cmd_vec[2] - p_t2p.at<double>(2, 0)) / myToolpath.travel_time);

		//// adjust speed (new, with projection method)
		//if (myToolpath.toolpath_idx > 0) {
		//	Mat traj_seg = C_c2p * (target_rmats * (myToolpath.toolpath[myToolpath.toolpath_idx] - myToolpath.toolpath[myToolpath.toolpath_idx - 1]) );
		//	Mat cmd_err = current_cmd - p_t2p;
		//	// tangential difference from current location to designed traj
		//	Mat cmd_err_tang = (cmd_err.dot(traj_seg)) * traj_seg / cv::norm(traj_seg, cv::NORM_L2);
		//	// orthogonal difference from current location to deisgned traj
		//	Mat cmd_err_orth = cmd_err - cmd_err_tang;
		//	Mat cmd_err_orth_norm = cmd_err_orth / cv::norm(cmd_err_orth, cv::NORM_L2);
		//	// assigned speed
		//	speed[0] = abs(1000 * cmd_err_tang.at<double>(0, 0) / myToolpath.travel_time + 500 * cmd_err_orth_norm.at<double>(0, 0) );
		//	speed[1] = abs(1000 * cmd_err_tang.at<double>(1, 0) / myToolpath.travel_time + 500 * cmd_err_orth_norm.at<double>(1, 0) );
		//	speed[2] = abs(1000 * cmd_err_tang.at<double>(2, 0) / myToolpath.travel_time + 500 * cmd_err_orth_norm.at<double>(2, 0) );
		//}

		//cout << "speed: x " << speed[0] << ", y " << speed[1] << ", z " << speed[2] << ", travel time " << myToolpath.travel_time << endl;

		
		// log data of tracking error (in target frame)
		if (LOG_DATA && myToolpath.toolpath_idx > 0) {
			/*inter_waypnt_ratio = myToolpath.travel_time / (myToolpath.toolpath_time[myToolpath.toolpath_idx] - myToolpath.toolpath_time[myToolpath.toolpath_idx - 1]);
			desiredWaypnt = myToolpath.toolpath[myToolpath.toolpath_idx - 1] + inter_waypnt_ratio * (myToolpath.toolpath[myToolpath.toolpath_idx] - myToolpath.toolpath[myToolpath.toolpath_idx - 1]);*/
			Mat tip_in_obj = (target_rmats.t()) * (-target_tvecs + tip_c);
			myToolpath.toolpath_tip_traj.push_back(tip_in_obj);
			myToolpath.toolpath_desired_traj.push_back(desiredWaypnt.clone());
			myToolpath.toolpath_track_error.push_back(tip_in_obj - desiredWaypnt);
			//cout << "Tracking error: " << endl << tip_in_obj - desiredWaypnt << endl;
		}
		

	}
	else { // one time move (init)
		current_cmd = p_o2p + C_c2p * (target_rmats * myToolpath.toolpath[myToolpath.toolpath_idx]);
		current_cmd_vec[0] = current_cmd.at<double>(0, 0);
		current_cmd_vec[1] = current_cmd.at<double>(1, 0);
		current_cmd_vec[2] = current_cmd.at<double>(2, 0) + z_offst;
	}

	if (debug_f) { cout << "    Time delay (cmd gen) since img capture: " << clock() - myToolpath.toolpath_tmr - myToolpath.toolpath_timestamp << endl; }

	//// compute the command to go to desired position in toolpath
	////current_cmd = p_t2p - C_c2p * tip_c + C_c2p * target_tvecs;
	//current_cmd = p_t2p - C_c2p * tip_c + C_c2p * target_tvecs + C_c2p * (target_rvecs * myToolpath.toolpath[myToolpath.toolpath_idx]);
	////cout << "current_cmd: " << endl << " " << current_cmd << endl;

	//// Out put to printer
	//current_cmd_vec[0] = current_cmd.at<double>(0, 0);
	//current_cmd_vec[1] = current_cmd.at<double>(1, 0);
	//current_cmd_vec[2] = current_cmd.at<double>(2, 0);
	////cout << "Command: " << current_cmd_vec[0] << ", " << current_cmd_vec[1] << ", " << current_cmd_vec[2] << endl;

	if (!thread_enable) {
		myPrinter->MoveAbs(&current_cmd_vec, &speed, wait_flag);
	}

	if (debug_f) { cout << "    Time delay (cmd appl) since img capture: " << clock() - myToolpath.toolpath_tmr - myToolpath.toolpath_timestamp << endl; }

	return 0;
}