#include "../Header/myGraphic.h"

void draw_axes(cv::Mat* img, std::vector<cv::Point2f>* imgpts) {
	line(*img, (*imgpts)[0], (*imgpts)[1], cv::Scalar(255, 0, 0), 2);
	line(*img, (*imgpts)[0], (*imgpts)[2], cv::Scalar(0, 255, 0), 2);
	line(*img, (*imgpts)[0], (*imgpts)[3], cv::Scalar(0, 0, 255), 2);
}

void draw_pnts(cv::Mat* img, std::vector<cv::Point2f>* imgpts) {
	for (int i = 0; i < imgpts->size(); i++) {
		circle(*img, (*imgpts)[i], 1, cv::Scalar(255, 0, 0), 1, 4);
	}
}

void project_xyzAxes(cv::Mat* img, cv::Mat* rvecs, cv::Mat* tvecs, cv::Mat* mtx, cv::Mat* dist, std::vector<cv::Point3f> axis) {
	//project 3D axes to image plane
	std::vector<cv::Point2f> imgpts;
	projectPoints(axis, *rvecs, *tvecs, *mtx, *dist, imgpts);
	draw_axes(img, &imgpts);
}

void project_pnts(cv::Mat* img, cv::Mat* rvecs, cv::Mat* tvecs, cv::Mat* mtx, cv::Mat* dist, std::vector<cv::Point3f>* pnts, std::vector<cv::Point2f>* imgpts) {
	//project a vector of point
	//std::vector<cv::Point2f> imgpts;
	projectPoints(*pnts, *rvecs, *tvecs, *mtx, *dist, *imgpts);
	draw_pnts(img, imgpts);
}

void project_tip(cv::Mat* mtx, cv::Mat* dist, cv::Mat* tip, std::vector<cv::Point2f>* imgpts) {

	// for point3f format
	cv::Point3f temp_tip_pnt(*tip);
	std::vector<cv::Point3f> temp_tip_vec = { temp_tip_pnt };

	cv::Mat local_rmat = cv::Mat::eye(3, 3, CV_64FC1);
	cv::Mat local_rvec;
	cv::Rodrigues(local_rmat, local_rvec);
	cv::Mat local_tvec = cv::Mat::zeros(3, 1, CV_64FC1);

	projectPoints(temp_tip_vec, local_rvec, local_tvec, *mtx, *dist, *imgpts);
	//circle(*img, (*imgpts)[0], 5, cv::Scalar(0, 0, 255), 1, 4);
}