#include "ArucoUtils.h"

using namespace Tango::TCC::CardDetector;

Ptr<Dictionary> dictionary;

ArucoUtils::ArucoUtils()
{
	dictionary = getPredefinedDictionary(aruco::DICT_4X4_50);
}


ArucoUtils::~ArucoUtils()
{
}

vector<Point> ArucoUtils::getArcusVertices(Mat image)
{
	vector<Point> vertices;

	vector<int> ids;
	vector<vector<Point2f>> corners;

	aruco::DetectorParameters* params = new aruco::DetectorParameters();
	params->cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX;
	params->perspectiveRemovePixelPerCell = 50;
	params->minDistanceToBorder = 0;


	aruco::detectMarkers(image, dictionary, corners, ids, &(*params));
	if (corners.size() == 4)
	{
		//aruco::drawDetectedMarkers(image, corners, ids);

		vertices =
		{
			Point(0,0),
			Point(0,0),
			Point(0,0),
			Point(0,0)
		};

		InputArrayOfArrays _corners = corners;
		InputArray _ids = ids;

		int nMarkers = (int)_corners.total();

		for (int i = 0; i < nMarkers; i++) {

			Mat currentMarker = _corners.getMat(i);
			int id = _ids.getMat().ptr< int >(0)[i] - 1;

			switch (id)
			{
			case 0:
				vertices[0] = currentMarker.ptr< Point2f >(0)[0];
				break;
			case 1:
				vertices[1] = currentMarker.ptr< Point2f >(0)[1];
				break;
			case 2:
				vertices[2] = currentMarker.ptr< Point2f >(0)[2];
				break;
			case 3:
				vertices[3] = currentMarker.ptr< Point2f >(0)[3];
				break;
			}
		}
	}

	return vertices;
}

Mat ArucoUtils::applyHomography(Mat image, vector<cv::Point> vertices, Size destination_size)
{
	Mat im_dst = Mat::zeros(destination_size, CV_8UC3);

	vector<Point2f> pts_dst;

	pts_dst.push_back(Point2f(0, 0));
	pts_dst.push_back(Point2f(destination_size.width - 1, 0));
	pts_dst.push_back(Point2f(destination_size.width - 1, destination_size.height - 1));
	pts_dst.push_back(Point2f(0, destination_size.height - 1));

	Mat tform = findHomography(vertices, pts_dst);
	warpPerspective(image, im_dst, tform, destination_size);
	return im_dst;
}