#include "Marker.h"
#include "ECTrainer.h"
#include "nkcWinUtils.h"

// コンストラクタ
Marker::Marker()
	: _detected(false)
	, _HomographyV2I()
{
	_Dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
	CalcMarkerCorners(IMGSIZE);
	_HomographyV2I.Put(cv::Mat::zeros(3, 3, CV_64F));
}

// マーカー座標の計算
void Marker::CalcMarkerCorners(cv::Size imgsize) {
	// 定数の計算
	int itvX = imgsize.width - (SIZE.x + MARGIN.x * 2);
	int itvY = (imgsize.height - (SIZE.x + MARGIN.x * 2)) / 3;
	// 座標計算
	_Corners.clear();
	for (int ix = 0; ix < 2; ix++) {
		for (int iy = 0; iy < 4; iy++) {
			std::vector<cv::Point2f> vertex;
			vertex.push_back(cv::Point2f(
				(float)(MARGIN.x + itvX * ix), (float)(MARGIN.y + itvY * iy)));
			vertex.push_back(cv::Point2f(
				(float)(MARGIN.x + itvX * ix + SIZE.x), (float)(MARGIN.y + itvY * iy)));
			vertex.push_back(cv::Point2f(
				(float)(MARGIN.x + itvX * ix + SIZE.x), (float)(MARGIN.y + itvY * iy + SIZE.y)));
			vertex.push_back(cv::Point2f(
				(float)(MARGIN.x + itvX * ix), (float)(MARGIN.y + itvY * iy + SIZE.y)));
			_Corners.push_back(vertex);
			//printf("marker %d : (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n", 
			//	(int)_Corners.size(), marker[0].x, marker[0].y
			//	, marker[1].x, marker[1].y
			//	, marker[2].x, marker[2].y
			//	, marker[3].x, marker[3].y);
		}
	}
}

// マーカーの検出
void Marker::Detect(cv::Mat& img) {
	// マーカーの検出
	std::vector<int> markerIds;
	std::vector<std::vector<cv::Point2f>> corners;
	cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
	cv::aruco::detectMarkers(img, _Dictionary, corners, markerIds, parameters);
	cv::aruco::drawDetectedMarkers(img, corners, markerIds);

	// 対応点の列挙
	std::vector<cv::Point2f> cornerI, cornerV;
	for (int i = 0; i < corners.size(); i++) {
		if (markerIds[i] - 1 < _Corners.size()) {
			for (int j = 0; j < corners[i].size(); j++) {
				cornerV.push_back(corners[i][j]);
				cornerI.push_back(_Corners[markerIds[i] - 1][j]);
			}
		}
	}
	if (cornerV.size() < 4) {
		_detected = false;
		return;
	}

	// ホモグラフィ行列の算出
	cv::Mat h = findHomography(cornerV, cornerI);
	if (h.empty()) {
		_detected = false;
	} else {
		_HomographyV2I.Put(h);
		_detected = true;
	}
}

// 座標変換（視野画像座標→画像座標）
cv::Point2f Marker::ConvV2I(cv::Point gazeV) {
	cv::Mat h = _HomographyV2I.Get();
	if (h.empty()) return cv::Point2f(-1.F, -1.F);

	cv::Mat gazeVe = (cv::Mat_<double>(3, 1) << gazeV.x, gazeV.y, 1);
	cv::Mat gazeIe = h * gazeVe;

	double* pgazeIe = gazeIe.ptr<double>(0);
	if (*(pgazeIe + 2) == 0) return cv::Point2f(-1.F, -1.F);

	float x = (float)(*pgazeIe / *(pgazeIe + 2));
	float y = (float)(*(pgazeIe + 1) / *(pgazeIe + 2));
	//std::cout << x << "," << y << std::endl;
	if (x < 0 || x >= (float)IMGSIZE.width || y < 0 || y >= IMGSIZE.height) 
		return cv::Point2f(-1.F, -1.F);
	return cv::Point2f(x, y);
}

#if 0
// マーカーを描画する
void Marker::DrawMarker(cv::Mat& img) {
	if (_Corners.size() < 1) {
		nkc::wut::DebugPrintf(_T("Marker not generated"));
		return;
	}

	cv::Mat marker;
	int outMargin = (int)(img.rows * OUTER_MARGIN);
	for (int i = 0; i < _Corners.size(); i++) {
		cv::aruco::drawMarker(_Dictionary, i + 1, SIZE.x, marker, 1);
		cv::cvtColor(marker, marker, cv::COLOR_GRAY2BGR);
		cv::Point p = cv::Point((int)(_Corners[i][0].x * img.cols), (int)(_Corners[i][0].y * img.rows));
		cv::Mat roi2(img, cv::Rect(p.x - outMargin, p.y - outMargin, SIZE.x + outMargin * 2, SIZE.y + outMargin * 2));
		roi2 = cv::Scalar(255, 255, 255);
		cv::Mat roi(img, cv::Rect(p, cv::Size(SIZE)));
		marker.copyTo(roi);
	}
}
#endif