#include "Marker.h"

#ifdef _DEBUG
#include <iostream>
#endif

// コンストラクタ
Marker::Marker() 
{
	_Dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
	Generate(IMGSIZE);
}

// マーカー座標の計算
void Marker::Generate(cv::Size imgsize) {
	// 定数の計算
	//_Size = (int)(imgsize.height * HEIGHT);
	//float width = HEIGHT;
	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::DrawMarker(cv::Mat& img) {
	if (_Corners.size() < 1) {
#ifdef _DEBUG
		std::cout << "Marker isn't generated." << std::endl;
#endif
		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);
	}
}

// マーカーの検出
bool 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> cornerW, cornerC;
	for (int i = 0; i < corners.size(); i++) {
		if (markerIds[i] - 1 < _Corners.size()) {
			for (int j = 0; j < corners[i].size(); j++) {
				cornerC.push_back(corners[i][j]);
				cornerW.push_back(_Corners[markerIds[i] - 1][j]);
			}
		}
		//printf("(%f,%f) ", corners[i][0].x, corners[i][0].y);
	}
	//if (corners.size() > 0) printf("\n");
	if (cornerC.size() < 4) return false;

	// ホモグラフィ行列の算出
	_HomographyV2I = findHomography(cornerC, cornerW);
	_HomographyI2V = findHomography(cornerW, cornerC);
	return !(_HomographyV2I.empty() || _HomographyI2V.empty());
}

// 座標変換（視野画像座標→画像座標）
cv::Point2f Marker::ConvV2I(cv::Point gazeV) {
	if (_HomographyV2I.empty()) return cv::Point2f(-1.F, -1.F);
	cv::Mat gazeVe = (cv::Mat_<double>(3, 1) << gazeV.x, gazeV.y, 1);
	cv::Mat gazeIe = _HomographyV2I * gazeVe;
	double* pgazeIe = gazeIe.ptr<double>(0);
	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);
}