// camCalib.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <vector>
#include "sourceCode/MonoLaserCalibrate.h"
#include "sourceCode/FitMapParam.h"

#define _DO_CAMERA_CALIB    1

void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
{
    std::ofstream sw(fileName);

    for (int i = 0; i < fitMap.rows; i++)
    {
        // 存储拟合结果
        double K[FittingOrder + 1];
        for (int _c = 0; _c <= FittingOrder; _c++)
            K[_c] = fitMap.ptr<float>(i)[_c];
        char dataStr[250];
        //sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g, %g, %g, %g", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sw << dataStr << std::endl;
    }
    sw.close();
    return;
}

void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
{
    std::ofstream sw(fileName);

    char dataStr[250];
    for (int i = 0; i < K.rows; i++)
    {
        // 存储旋转矩阵
        double temp[3];
        for (int _c = 0; _c < 3; _c++)
            temp[_c] = K.ptr<double>(i)[_c];
       
        //sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sprintf_s(dataStr, 250, "%g, %g, %g", temp[0], temp[1], temp[2]);
        sw << dataStr << std::endl;
    }
    double temp[5];
    for (int _c = 0; _c < 5; _c++)
        temp[_c] = D.ptr<double>(0)[_c];
    sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]);
    sw << dataStr << std::endl;
    sw.close();
    return;
}

void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
{
    K = cv::Mat::zeros(3, 3, CV_64FC1);
    D = cv::Mat::zeros(1, 5, CV_64FC1);
    std::ifstream inputFile(fileName);
    std::string linedata;

    if (inputFile.is_open() == false)
        return;

    int line = 0;
    while (getline(inputFile, linedata))
    {
        if (line < 3)
        {
            double data[3];
            sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &data[0], &data[1], &data[2]);
            for (int _c = 0; _c < 3; _c++)
                K.ptr<double>(line)[_c] = data[_c];
        }
        else
        {
            double data[5];
            sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf", &data[0], &data[1], &data[2], &data[3], &data[4]);
            for (int _c = 0; _c < 5; _c++)
                D.ptr<double>(0)[_c] = data[_c];
        }
        line++;
    }
    inputFile.close();
    return;
}

typedef struct
{
    int nMin;	//< 最小值
    int nMax;	//< 最大值
} SWdNLRange;

#define CALIB_CHESS_BOARD   1
#define CALIB_CIRCLE_GRID   2
#define CALIB_CHARUCO       3

#define CALIB_TEST_GROUP 4
int main()
{
    std::cout << "Hello World!\n";

    const char* calibDataPath[CALIB_TEST_GROUP] = {
        "D:\\Work\\camalgo\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
        "D:\\Work\\camalgo\\camCalib\\camCalibData\\chessboard\\",   //1
        "D:\\Work\\camalgo\\camCalib\\camCalibData\\circlePoint\\", //2
        "D:\\Work\\camalgo\\camCalib\\camCalibData\\charuCo\\", //3
    };
    const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
        {3,39},{1,200},{1,166},{122,141}
    };
    const int boardType[CALIB_TEST_GROUP] =
    {
        CALIB_CHESS_BOARD,
        CALIB_CHESS_BOARD,
        CALIB_CIRCLE_GRID,
        CALIB_CHARUCO
    };

    for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
    {
        grp = 2;
        int calibType = boardType[grp];
        cv::Size cbPattern;
        float cbSquareSize;
        if (CALIB_CHESS_BOARD == calibType)
        {
            cbPattern = cv::Size(8, 11); // 10);
            cbSquareSize = 40.0f; // 10.f;
        }
        else if (CALIB_CIRCLE_GRID == calibType)
        {
            cbPattern = cv::Size(7, 7); // 10);
            cbSquareSize = 50.0f; // 10.f;
        }
        else if (CALIB_CHARUCO == calibType)
        {
            cbPattern = cv::Size(20, 46); // 10);
            cbSquareSize = 50.0f; // 10.f;
        }
        else
            continue;

        std::vector<std::vector<cv::Point2f>> cbCornersList;
        cv::Size imageSize;

        int startIndex = fileIdx[grp].nMin;
        int endIndex = fileIdx[grp].nMax;
        int index;
#if _DO_CAMERA_CALIB
        for (index = startIndex; index <= endIndex; index++) {
            char filename[256];
            sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
            cv::Mat srcImg = cv::imread(filename);
            if (srcImg.empty())
                break;

            cv::Mat img;
            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
            char rotateFilename[256];
            sprintf_s(rotateFilename, "%scalib_%03d_rotate.bmp", calibDataPath[grp], index);
            cv::String rFileName(rotateFilename);
            cv::imwrite(rFileName, img);

            std::vector<cv::Point2f> corners;
            if (CALIB_CHESS_BOARD == calibType)
            {
                detectCorners(img, cbPattern, corners);
#if ENABLE_DEBUG
                if (corners.size() > 0)
                {
                    cv::Mat color = img.clone();
                    cv::drawChessboardCorners(color, cbPattern, corners, true);
                    //cv::resize(color, color, cv::Size(), 0.5, 0.5);
                    sprintf_s(filename, "%scalib_%03d_corner.bmp", calibDataPath[grp], index);
                    cv::imwrite(filename, color);
                }
#endif
            }
            else if (CALIB_CIRCLE_GRID == calibType)
            {
                detectCirclePoints(img, cbPattern, corners);
#if ENABLE_DEBUG
                if (corners.size() > 0)
                {
                    cv::Mat color = img.clone();
                    cv::drawChessboardCorners(color, cbPattern, corners, true);
                    //cv::resize(color, color, cv::Size(), 0.5, 0.5);
                    sprintf_s(filename, "%scalib_%03d_center.bmp", calibDataPath[grp], index);
                    cv::imwrite(filename, color);
                }
#endif

            }
            else if (CALIB_CHARUCO == calibType)
            {
            }
            if (corners.empty())
                continue;

            imageSize = cv::Size(img.cols, img.rows);
            cbCornersList.push_back(corners);
        }

        cv::Mat K, D;
        std::vector<double> reprojectionError;
        monocularCalibration(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        for(int i = 0; i < reprojectionError.size(); i ++)
            std::cout << reprojectionError[i] << std::endl;

        // 输出映射类型，通常使用CV_32FC1或CV_16SC2
        cv::Mat map_x, map_y;
        cv::Mat newCamMatrix;
        // 生成畸变矫正映射
#if ENABLE_FISH_EYE
        cv::fisheye::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map1, map2);
#else
        double alpha = 0.4; // 0.4;
        newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map_x, map_y);
#endif

        // 生成系数表
        cv::Mat fitMap_x = GetFitParamMap(map_x, 1);
        cv::Mat fitMap_y = GetFitParamMap(map_y, 1);
        //输出系数文件
        char calibParamName[256];
        sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_x);
        sprintf_s(calibParamName, "%scalib_param_y.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_y);

        //比较误差
        cv::Mat mapGen_x = GetMapFromFitMap(fitMap_x, imageSize, 1);
        cv::Mat mapGen_y = GetMapFromFitMap(fitMap_y, imageSize, 1);
        //搜索最大和平均误差
            // 计算绝对差异
        cv::Mat diff_x, diff_y;
        cv::absdiff(map_x, mapGen_x, diff_x);
        cv::absdiff(map_y, mapGen_y, diff_y);
        // 查找最大值和最小值
        double minVal_x, maxVal_x;
        cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x);
        double minVal_y, maxVal_y;
        cv::minMaxLoc(diff_y, &minVal_y, &maxVal_y);
        // 计算平均值
        cv::Scalar meanVal_x = cv::mean(diff_x);
        cv::Scalar meanVal_y = cv::mean(diff_y);

        std::cout << "X Max_difference: " << maxVal_x << std::endl;
        std::cout << "X Mean difference: " << meanVal_x[0] << std::endl;
        std::cout << "Y Max difference: " << maxVal_y << std::endl;
        std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;

        //生成矫正图像
        index = 3;
        for (;; index++) {

            char filename[256];
            sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
            cv::Mat srcImg = cv::imread(filename);
            if (srcImg.empty())
                break;

            cv::Mat img;
            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
            cv::Mat calibImg;
            remap(img,
                calibImg,
                mapGen_x,
                mapGen_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, calibImg);
        }

        //output K and D
        char calibKDName[256];
        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
        sg_outputCalibKD(calibKDName, K, D);
#else
        char calibKDName[256];
        sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath);
        cv::Mat K, D;
        sg_readCalibKD(calibKDName, K, D);
#endif

 #if ENABLE_GEN_IMAGE
        cv::Vec4f laserPE;
        generateLaserLine(10.f, 5.f, laserPE);
        std::cout << "generateLaserLine pe: " << laserPE << std::endl;

        index = 3;
        for (;; index++) {

            char filename[256];
            sprintf_s(filename, "%s%03d.bmp", cbImagePath, index);
            cv::Mat img = cv::imread(filename);
            if (img.empty())
                break;

            std::vector<cv::Point2f> corners;
            detectCorners(img, cbPattern, corners);
            if (corners.empty())
                continue;

            cv::Vec4f pe;
            fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);

            cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize);

    #if ENABLE_DEBUG
            cv::Mat color = image.clone();
            cv::resize(color, color, cv::Size(), 0.5, 0.5);
            cv::imshow("image", color);
            cv::waitKey(10);·
#endif

            sprintf_s(filename, "%s%d.bmp", laserImagePath, index);
            cv::imwrite(filename, image);
        }
    
#endif

        {
            std::vector<cv::Point3f> all_pts3d;

            index = 3;
            for (;; index++) {

                char filename[256];
                sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
                cv::Mat srcImg = cv::imread(filename);
                if (srcImg.empty())
                    break;

                cv::Mat img;
                cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
                std::vector<cv::Point2f> corners;
                detectCorners(img, cbPattern, corners);
                if (corners.empty())
                    continue;

                // 创建棋盘格区域的掩码
                cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                // 使用多边形近似来填充角点之间的区域
                // 棋盘格区域需要比角点区域大一圈
                std::vector<cv::Point2f> contour_line[4];
                for (int i = 0; i < cbPattern.width; i++)
                {
                    cv::Point2f pt_c = corners[i];
                    cv::Point2f pt_2 = corners[cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[0].push_back(pt_1);
                }
                for (int i = 0; i < cbPattern.height; i++)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[1].push_back(pt_1);
                }
                for (int i = cbPattern.width - 1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
                    cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[2].push_back(pt_1);
                }
                for (int i = cbPattern.height-1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[3].push_back(pt_1);
                }
                std::vector<cv::Point> contours;
                //生成轮廓点
                for (int n = 0; n < 4; n++)
                {
                    int num = contour_line[n].size();
                    for (int i = 0; i < num; i++)
                        contours.push_back(contour_line[n][i]);
                    cv::Point2f pt_c = contour_line[n][num - 1];
                    cv::Point2f pt_2 = contour_line[n][num - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contours.push_back(pt_1);
                }
                // 使用 fillPoly 填充多边形
                cv::Scalar color(255); // 红色
                cv::fillPoly(chessMask, contours, color);
#if 1
                sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
                cv::imwrite(filename, chessMask);
#endif

                cv::Vec4f pe;
                fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);

                sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
                cv::Mat srcLaserImg = cv::imread(filename);
                if (srcLaserImg.empty())
                    break;

                cv::Mat laserImg_unMask;
                cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
                //与Mask相与，保证待处理的激光线在标定板上
                cv::Mat laserImg;
                cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
    #if 1
                sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
                cv::imwrite(filename, laserImg);
    #endif
                std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
                //显示亚像素点
                cv::Mat enlargeImg;
                if (laserImg.channels() == 1)
                    laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
                else
                    enlargeImg = laserImg.clone();
                cv::Size objSize = laserImg.size();
                objSize.width = objSize.width * 5;
                cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);

                for (int i = 0, i_max = pts2d.size(); i < i_max; i++)
                {
                    cv::Point2f a_subPix = pts2d[i];
                    a_subPix.x += 0.5;
                    int row = (int)(a_subPix.y + 0.5);
                    int col = (int)(a_subPix.x * 5 + 0.5);
                    enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
                    enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
                    enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
                }
                sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
                cv::imwrite(filename, enlargeImg);
                std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);

                all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
            }

            cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
            std::cout << "pe: " << pe << std::endl;
        }
    }

    return 0;
}