#include "Marker.h"

#ifdef _DEBUG
#include <iostream>
#endif

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

// マーカー座標の計算
void Marker::Generate(cv::Size imgsize) {
	// 定数の計算
	_Size = (int)(imgsize.height * HEIGHT);
	float width = imgsize.height * HEIGHT / imgsize.width;
	float itvX = (1.0F - 2.0F * MARGIN.x - width) / 2.0F;
	float itvY = (1.0F - 2.0F * MARGIN.y - HEIGHT) / 2.0F;
	// 座標計算
	_Corners.clear();
	for (int iy = 0; iy < 3; iy++) {
		for (int ix = 0; ix < 3; ix++) {
			if (ix == 1 && iy == 1) continue;	// 中心にはマーカーを置かない

			std::vector<cv::Point2f> marker;
			marker.push_back(cv::Point2f(
				MARGIN.x + itvX * ix, MARGIN.y + itvY * iy));
			marker.push_back(cv::Point2f(
				MARGIN.x + itvX * ix + width, MARGIN.y + itvY * iy));
			marker.push_back(cv::Point2f(
				MARGIN.x + itvX * ix + width, MARGIN.y + itvY * iy + HEIGHT));
			marker.push_back(cv::Point2f(
				MARGIN.x + itvX * ix, MARGIN.y + itvY * iy + HEIGHT));
			_Corners.push_back(marker);
		}
	}
}

// マーカーを描画する
void Marker::DrawMarker(cv::Mat& img) {
	if (_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, 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 + outMargin * 2, _Size + outMargin * 2));
		roi2 = cv::Scalar(255, 255, 255);
		cv::Mat roi(img, cv::Rect(p, cv::Size(_Size, _Size)));
		marker.copyTo(roi);
	}
}

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

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

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

// 座標変換（視野画像座標→正規化刺激画像座標）
cv::Point2f Marker::ConvV2I(cv::Point gazeV) {
	if (_Homography.empty()) return cv::Point2f(-1.F, -1.F);
	cv::Mat gazeVe = (cv::Mat_<double>(3, 1) << gazeV.x, gazeV.y, 1);
	cv::Mat gazeIe = _Homography * gazeVe;
	double* pgazeIe = gazeIe.ptr<double>(0);
	float x = (float)(*pgazeIe / *(pgazeIe + 2));
	float y = (float)(*(pgazeIe + 1) / *(pgazeIe + 2));
	if (x < 0 || x > 1.F || y < 0 || y > 1.F) return cv::Point2f(-1.F, -1.F);
	return cv::Point2f(x, y);
}