/**
* Lubricant Detecter
* Copyright (C) 2021 Carl Klemm
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* version 3 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the
* Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
* Boston, MA  02110-1301, USA.
*/

#include "uvosunwrap/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<DetectedPoint> detectCharucoPoints(cv::Mat image, 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);
		}
		
		std::vector<DetectedPoint> detections;

		for(size_t i = 0; i < charucoIds.size(); ++i)
		{
			cv::Point2i coordiante(charucoIds[i] % (X_BOARD_SIZE-1), (Y_BOARD_SIZE-2) - charucoIds[i]/(X_BOARD_SIZE-1));
			detections.push_back(DetectedPoint(charucoCorners[i], coordiante));
		}
		
		return detections;
	}
	return std::vector<DetectedPoint>();
}
