#include "charuco.h"
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/highgui.hpp>

static constexpr unsigned int X_BOARD_SIZE = 18;
static constexpr unsigned int Y_BOARD_SIZE = 10;

void createCharucoBoard(unsigned int size, const std::string& fileName)
{
    cv::Ptr<cv::aruco::CharucoBoard> board = 
			cv::aruco::CharucoBoard::create(X_BOARD_SIZE, Y_BOARD_SIZE, 0.03f, 0.02f, 
											cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250 ));
    cv::Mat charucoImage;
    board->draw(cv::Size((size*18)/10, size), charucoImage, 0, 1);
    cv::imwrite(fileName, charucoImage);
}

std::vector<cv::Point2f> detectCharucoPoints(cv::Mat image, std::vector<cv::Point2i>* coordiantes, bool verbose)
{
	cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(X_BOARD_SIZE, Y_BOARD_SIZE, 0.03f, 0.02f, 
												cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250 ));
	cv::Ptr<cv::aruco::DetectorParameters> params = cv::aruco::DetectorParameters::create();
	params->adaptiveThreshWinSizeMin = 3;
	params->adaptiveThreshWinSizeMax = 41;
	params->adaptiveThreshWinSizeStep = 4;
	params->perspectiveRemovePixelPerCell = 16;
	params->perspectiveRemoveIgnoredMarginPerCell = 0.1;
	params->polygonalApproxAccuracyRate = 0.05;
	params->perspectiveRemoveIgnoredMarginPerCell = 0.1;
	params->aprilTagMinClusterPixels = 3;
	
	std::vector<int> arucoIds;
	std::vector<std::vector<cv::Point2f> > arucoCorners;
	cv::aruco::detectMarkers(image, board->dictionary, arucoCorners, arucoIds, params);

	
	if(verbose)
	{
		cv::Mat debugImage;
		image.copyTo(debugImage);
		cv::aruco::drawDetectedMarkers(debugImage, arucoCorners, arucoIds);
		cv::imshow( "Viewer", debugImage );
		cv::waitKey(0);
	}
	
	if (arucoIds.size() > 0) 
	{
		std::vector<cv::Point2f> charucoCorners;
		std::vector<int> charucoIds;
		cv::aruco::interpolateCornersCharuco(arucoCorners, arucoIds, image, board, charucoCorners, 
												charucoIds, cv::noArray(), cv::noArray(), 0);
		if(verbose) 
		{
			cv::Mat debugImage;
			image.copyTo(debugImage);
			cv::aruco::drawDetectedCornersCharuco(debugImage, charucoCorners, charucoIds, cv::Scalar(0, 255, 0));
			cv::imshow("Viewer", debugImage);
			cv::waitKey(0);
		}
		
		if(coordiantes) for( auto id : charucoIds)
			coordiantes->push_back(cv::Point2i(id % (X_BOARD_SIZE-1), (Y_BOARD_SIZE-2) - id/(X_BOARD_SIZE-1)));
		
		return charucoCorners;
	}
	return std::vector<cv::Point2f>();
}