#include "opencv2/highgui.hpp"
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/core.hpp>
#include <iostream>

using namespace cv;
using namespace std;

int main()
{
    Ptr<aruco::Dictionary> dict = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(cv::aruco::DICT_7X7_250));
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();

    int markerX = 3, markerY = 4, marginSize = 10;
    float markerSize = 80.f;

    // draw GridBoard
    Ptr<aruco::GridBoard> gridBoard = aruco::GridBoard::create(markerX, markerY, markerSize, markerSize, dict);
    Mat grid, default_grid;
    gridBoard->draw(Size(markerSize*(2*markerX-1)+2*marginSize, markerSize*(2*markerY-1) +2*marginSize), default_grid, marginSize);
    cvtColor(default_grid, default_grid, CV_GRAY2RGB);
    default_grid.copyTo(grid);
    //imshow("grid", grid);

    // draw detected GridBoard
    vector<vector<Point2f> > corners;
    vector<int> ids;
    aruco::detectMarkers(grid, dict, corners, ids);
    aruco::drawDetectedMarkers(grid, corners, ids, Scalar(0, 0, 255));
    imshow("detected_grid", grid);
    default_grid.copyTo(grid);
    {
        corners.clear();
        for (auto cell : gridBoard->objPoints)
        {
            vector<Point2f> tmpVec;
            Point2f marginOffset = Point2f(marginSize, marginSize);
            for (Point3f p : cell)
            {
                tmpVec.push_back(Point2f(p.x, p.y) + marginOffset);
            }
            corners.push_back(tmpVec);
        }
        aruco::drawDetectedMarkers(grid, corners, gridBoard->ids, Scalar(0, 0, 255));
        imshow("board_detected_grid", grid);
        imwrite("board_detected_grid.jpg", grid);
        default_grid.copyTo(grid);
        corners.clear();
    }

    // draw CharucoBoard
    Ptr<aruco::CharucoBoard> charucoBoard = aruco::CharucoBoard::create(markerX, markerY, 2.f*markerSize, markerSize, dict);
    Mat default_charuco, charuco;
    charucoBoard->draw(Size(2*markerSize*markerX+2*marginSize, 2*markerSize*markerY+2*marginSize), default_charuco, marginSize);
    cvtColor(default_charuco, default_charuco, CV_GRAY2RGB);
    default_charuco.copyTo(charuco);
    //imshow("charuco", charuco);
    
    // draw detected CharucoBoard
    aruco::detectMarkers(charuco, dict, corners, ids);
    aruco::drawDetectedMarkers(charuco, corners, ids, Scalar(0, 0, 255));
    imshow("detected_charuco", charuco);
    default_charuco.copyTo(charuco);
    {
        corners.clear();
        for (auto cell : charucoBoard->objPoints)
        {
            vector<Point2f> tmpVec;
            Point2f marginOffset = Point2f(marginSize, marginSize);
            for (Point3f p : cell)
            {
                tmpVec.push_back(Point2f(p.x, p.y) + marginOffset);
            }
            corners.push_back(tmpVec);
        }
        aruco::drawDetectedMarkers(charuco, corners, charucoBoard->ids, Scalar(0, 0, 255));
        imshow("charuco_detected_grid", charuco);
        imwrite("charuco_detected_grid.jpg", charuco);
        corners.clear();
        default_charuco.copyTo(charuco);
    }

    vector< vector< Point2f > > diamondCorners;
    vector< Vec4i > diamondIds;
    aruco::detectMarkers(charuco, dict, corners, ids);
    aruco::detectCharucoDiamond(charuco, corners, charucoBoard->ids, 2.f * markerSize / markerSize, diamondCorners, diamondIds);
    aruco::drawDetectedDiamonds(charuco, diamondCorners, diamondIds, Scalar(0, 0, 255));
    imshow("detected_diamonds", charuco);
    imwrite("detected_diamonds.jpg", charuco);

    Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1), distCoeffs;
    cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 650;
    cameraMatrix.at< double >(0, 2) = charuco.size().width / 2;
    cameraMatrix.at< double >(1, 2) = charuco.size().height / 2;
    std::vector<cv::Vec3d> rvecs, tvecs;
    cv::aruco::estimatePoseSingleMarkers(diamondCorners, 2.f * markerSize, cameraMatrix, distCoeffs, rvecs, tvecs);
    for (unsigned int i = 0; i < rvecs.size(); i++)
        cv::aruco::drawAxis(charuco, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 30.f);
    imshow("pose_diamonds", charuco);
    default_charuco.copyTo(charuco);
    waitKey();
    return 0;
}