#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "binocularMarkCam_Export.h"
#include <opencv2/opencv.hpp>
#include "aruco.hpp"
#include "aruco/charuco.hpp"
#include <limits>

#define _OUTPUT_DEBUG_DATA  0

//version 1.0.0 : 初始版本
std::string	m_strVersion = "1.0.0";
//读版本号
const char* wd_charuco3DMarkVersion(void)
{
	return m_strVersion.c_str();
}

/*Breif：二维码标定板角点函数*/
void detectCharucoCorners(const cv::Mat& img,
    const SWD_BQ_CharucoMarkInfo markInfo,
    const SWD_BQ_MarkBoardInfo boardInfo,
    std::vector<int>& markerIdsAll,
    std::vector<std::vector<cv::Point2f> >& markerCornersAll,
    std::vector<int>& charucoIds,
    std::vector<std::vector<cv::Point2f>>& charucoCorners)
{
    cv::Mat gray;
    if (img.channels() == 3)
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    else
        gray = img.clone();

    enum cv::aruco::PredefinedDictionaryType dictType;
    if (1 == markInfo.dictType)
        dictType = cv::aruco::DICT_6X6_1000;
    else
        dictType = cv::aruco::DICT_6X6_1000;  //目前只支持DICT_6X6_1000

    cv::aruco::Dictionary aruco_dict = cv::aruco::getPredefinedDictionary(dictType);
    cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters();
    params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;

    cv::Ptr<cv::aruco::Dictionary> ptrDictionary = cv::makePtr<cv::aruco::Dictionary>(aruco_dict);
    cv::Ptr<cv::aruco::DetectorParameters> ptrParams = cv::makePtr<cv::aruco::DetectorParameters>(params);
#if 0
    // 优化检测器参数（提升检测稳定性）
    ptrParams->adaptiveThreshWinSizeMin = 3;
    ptrParams->adaptiveThreshWinSizeMax = 23;
    ptrParams->adaptiveThreshWinSizeStep = 10;
    ptrParams->minMarkerPerimeterRate = 0.01; // 检测更小的标记
#endif
    //检测所有角点
    cv::aruco::detectMarkers(gray, ptrDictionary, markerCornersAll, markerIdsAll, ptrParams);

    //搜索Mark(搜索各个不同的标定板）
    for (int board = 0; board < boardInfo.totalBoardNum; board++)
    {
        int startId = board * boardInfo.boardIdInterval;
        int endId = startId + boardInfo.boardChaucoIDNum - 1;

        std::vector<int> ids;
        for (int i = 0; i < boardInfo.boardChaucoIDNum; i++)
            ids.push_back(startId + i);

        //搜索
        std::vector<int> markerIds;
        std::vector<std::vector<cv::Point2f> > markerCorners;
        for (int i = 0; i < markerIdsAll.size(); i++)
        {
            if ((markerIdsAll[i] >= startId) && (markerIdsAll[i] <= endId))
            {
                markerIds.push_back(markerIdsAll[i]);
                markerCorners.push_back(markerCornersAll[i]);
            }
        }
        if (markerIds.size() > 0)
        {
            std::vector<int> board_charucoIds;
            std::vector<cv::Point2f> board_charucoCorners;

            cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(markInfo.patternSize, markInfo.checkerSize, markInfo.markerSize, aruco_dict, ids);
            cv::Ptr<cv::aruco::CharucoBoard> ptrBoard = cv::makePtr<cv::aruco::CharucoBoard>(board);
            cv::aruco::interpolateCornersCharuco(
                markerCorners,
                markerIds,
                gray,
                ptrBoard,
                board_charucoCorners,
                board_charucoIds);
            if (board_charucoIds.size() > 0)
            {
                // 亚像素精确化
                cv::cornerSubPix(gray, board_charucoCorners, cv::Size(11, 11), cv::Size(-1, -1),
                    cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));

                charucoIds.push_back(startId);
                charucoCorners.push_back(board_charucoCorners);
            }
        }
    }
    return;
}

SVzNL3DPoint _getStereoPoints3d(cv::Point2f left, cv::Point2f right, const cv::Mat& Q, double offset = 0)
{
    const double cx = Q.at<double>(0, 3);
    const double cy = Q.at<double>(1, 3);
    const double f = Q.at<double>(2, 3);
    const double t = 1.0 / Q.at<double>(3, 2);
    //std::cout << "Cx: " << cx << ", Cy: " << cy << ", f: " << f << ", t: " << t << std::endl;
    SVzNL3DPoint result;
    double dis = left.x - right.x + offset;
    result.x = (left.x + cx) * t / dis;
    result.y = (left.y + cy) * t / dis;
    result.z = f * t / dis;
    //std::cout << "L: " << left[_i] << ", R: " << right[_i] << ", dis: " << dis << std::endl;
    return result;
}

typedef struct
{
    int markID;
    std::vector<cv::Point2f> left;
    std::vector<cv::Point2f> right;
}_MarkerPairing;

//提取查科Mark3D信息
void wd_BQ_getCharuco3DMark(
    cv::Mat& leftImage,
    cv::Mat& rightImage,
    cv::Mat& cameraMatrixL,
    cv::Mat& distCoeffsL,
    cv::Mat& cameraMatrixR,
    cv::Mat& distCoeffsR,
    cv::Mat& R1,
    cv::Mat& R2,
    cv::Mat& P1,
    cv::Mat& P2,
    cv::Mat& Q,
    SWD_BQ_CharucoMarkInfo markInfo,
    SWD_BQ_MarkBoardInfo boardInfo,
    double disparityOffset,
	std::vector<SWD_charuco3DMark>& marks)
{
    std::vector<int> markerIdsLeft;
    std::vector<std::vector<cv::Point2f> > markerCornersLeft;  
    std::vector<int> charucoIdsLeft;
    std::vector<std::vector<cv::Point2f>> charucoCornersLeft;//每个Mark有4个角点对应4个二维码
    detectCharucoCorners(leftImage,
        markInfo,
        boardInfo,
        markerIdsLeft,
        markerCornersLeft,
        charucoIdsLeft,
        charucoCornersLeft);

    std::vector<int> markerIdsRight;
    std::vector<std::vector<cv::Point2f> > markerCornersRight;
    std::vector<int> charucoIdsRight;
    std::vector<std::vector<cv::Point2f>> charucoCornersRight;
    detectCharucoCorners(rightImage,
        markInfo,
        boardInfo,
        markerIdsRight,
        markerCornersRight,
        charucoIdsRight,
        charucoCornersRight);

#if _OUTPUT_DEBUG_DATA
    cv::Mat imageCopyLeft = leftImage.clone();
    if (markerIdsLeft.size() > 0)
        cv::aruco::drawDetectedMarkers(imageCopyLeft, markerCornersLeft, markerIdsLeft);
    if (charucoIdsLeft.size() > 0)
        cv::aruco::drawDetectedCornersCharuco(imageCopyLeft, charucoCornersLeft, charucoIdsLeft, cv::Scalar(0, 255, 255));
    char markFilenameL[256];
    sprintf_s(markFilenameL, "markersLeft.bmp");
    cv::imwrite(markFilenameL, imageCopyLeft);

    cv::Mat imageCopyRight = leftImage.clone();
    if (markerIdsRight.size() > 0)
        cv::aruco::drawDetectedMarkers(imageCopyRight, markerCornersRight, markerIdsRight);
    if (charucoIdsRight.size() > 0)
        cv::aruco::drawDetectedCornersCharuco(imageCopyRight, charucoCornersRight, charucoIdsRight, cv::Scalar(0, 255, 255));
    char markFilenameR[256];
    sprintf_s(markFilenameR, "markersRight.bmp");
    cv::imwrite(markFilenameR, imageCopyRight);
#endif

    //Marker配对
    std::vector<_MarkerPairing> stereoMarks;
    for (int i = 0, i_max = (int)charucoIdsLeft.size(); i < i_max; i++)
    {
        for (int j = 0, j_max = (int)charucoIdsRight.size(); j < j_max; j++)
        {
            if ( (charucoIdsLeft[i] == charucoIdsRight[j]) && (charucoCornersLeft[i].size() == charucoCornersRight[j].size()))
            {
                _MarkerPairing a_pair;
                a_pair.markID = charucoIdsLeft[i];
                a_pair.left.insert(a_pair.left.end(), charucoCornersLeft[i].begin(), charucoCornersLeft[i].end());
                a_pair.right.insert(a_pair.right.end(), charucoCornersRight[j].begin(), charucoCornersRight[j].end());
                stereoMarks.push_back(a_pair);
                break;
            }
        }
    }

    for (int i = 0, i_max = (int)stereoMarks.size(); i < i_max; i++)
    {
        //双目校正
        std::vector<cv::Point2f> rectCornersLeft;
        cv::undistortPoints(stereoMarks[i].left, rectCornersLeft, cameraMatrixL, distCoeffsL, R1, P1);
        std::vector<cv::Point2f> rectCornersRight;
        cv::undistortPoints(stereoMarks[i].right, rectCornersRight, cameraMatrixR, distCoeffsR, R2, P2);

        std::vector<SVzNL3DPoint> corners;
        for (int m = 0; m < rectCornersLeft.size(); m++)
        {
            double abs_diff = abs(rectCornersLeft[m].y - rectCornersRight[m].y);
            if (abs_diff < 1.0) //极线误差小于1个像素
            {
                SVzNL3DPoint a_pt3d = _getStereoPoints3d(rectCornersLeft[m], rectCornersRight[m], Q, disparityOffset);
                corners.push_back(a_pt3d);
            }
        }
        if (corners.size() == 4)
        {
            SWD_charuco3DMark a_3dMark;
            a_3dMark.markID = stereoMarks[i].markID;
            a_3dMark.mark3D.x = (corners[0].x + corners[1].x + corners[2].x + corners[3].x) / 4;
            a_3dMark.mark3D.y = (corners[0].y + corners[1].y + corners[2].y + corners[3].y) / 4;
            a_3dMark.mark3D.z = (corners[0].z + corners[1].z + corners[2].z + corners[3].z) / 4;
            marks.push_back(a_3dMark);
        }
    }
    return;
}