// BinocularMarkCam_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <VZNL_Types.h>
#include <string>
#include "binocularMarkCam_Export.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <Windows.h>

void _outputMarkData(char* fileName, std::vector<SWD_charuco3DMark>& marks)
{
    std::ofstream sw(fileName);
    char dataStr[250];
    for (int i = 0; i < (int)marks.size(); i++)
    {
        sprintf_s(dataStr, 250, "MarkID_%d: %g, %g, %g", marks[i].markID, marks[i].mark3D.x, marks[i].mark3D.y, marks[i].mark3D.z);
        sw << dataStr << std::endl;
    }
    sw.close();
}

#define TEST_GROUP 6
int main(int argc, char** argv)
{
    const char* dataPath[TEST_GROUP] = {
        "F:/标定采图/charucoMark图/MarkTest/",  //0
        "F:/标定采图/charucoMark图/Mark_13度/",
        "F:/ShangGu/项目/冠钦_博清科技/组装/数据/mark测试/",
        "F:/ShangGu/项目/冠钦_博清科技/组装/数据/现场Mark数据/",
        "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/",
        "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/",
    };

    SVzNLRange fileIdx[TEST_GROUP] = {
        {1,4}, {1,5}, {1,9}, {1, 1}, {2,33}, {2,33}
    };

	const char* ver = wd_charuco3DMarkVersion();
	printf("ver:%s\n", ver);

    for (int grp = 4; grp < 6; grp++)
    {
        SWD_BQ_CharucoMarkInfo markInfo;
        markInfo.patternSize = cv::Size(3, 3);
        markInfo.dictType = 1; //DICT_6x6
        markInfo.checkerSize = 60.0;
        markInfo.markerSize = 45.0;

        SWD_BQ_MarkBoardInfo boardInfo;
        boardInfo.totalBoardNum = 10;
        boardInfo.boardIdInterval = 8;
        boardInfo.boardChaucoIDNum = 4;

        // 1. 打开 XML 文件（READ 模式）
        char calibFile[250];
        sprintf_s(calibFile, sizeof(calibFile), "%sStereoCamera.xml", dataPath[grp]);
        cv::FileStorage fs(calibFile, cv::FileStorage::READ);

        // 检查文件是否成功打开
        if (!fs.isOpened()) {
            printf( "错误：无法打开 XML 文件！请检查路径是否正确。\n" );
            return -1;
        }
        cv::FileNode LeftCamera = fs["LeftCamera"];
        cv::Mat cameraMatrixL, distCoeffsL;
        LeftCamera["CameraMatrix"] >> cameraMatrixL;
        LeftCamera["DistCoeffs"] >> distCoeffsL;
        cv::FileNode RightCamera = fs["RightCamera"];
        cv::Mat cameraMatrixR, distCoeffsR;
        RightCamera["CameraMatrix"] >> cameraMatrixR;
        RightCamera["DistCoeffs"] >> distCoeffsR;
        cv::FileNode Rectification = fs["Rectification"];
        cv::Mat R1, R2,P1,P2,Q;
        Rectification["R1"] >> R1;
        Rectification["R2"] >> R2;
        Rectification["P1"] >> P1;
        Rectification["P2"] >> P2;
        Rectification["Q"] >> Q;

        int fidx1 = fileIdx[grp].nMin;
        int fidx2 = fileIdx[grp].nMax;
        for (int index = fidx1; index <= fidx2; index ++) 
        {
            char filename[256];
            sprintf_s(filename, "%s/left_%d.bmp", dataPath[grp], index);
            cv::Mat leftimg = cv::imread(filename, cv::IMREAD_GRAYSCALE);

            sprintf_s(filename, "%s/right_%d.bmp", dataPath[grp], index);
            cv::Mat rightimg = cv::imread(filename, cv::IMREAD_GRAYSCALE);

            if (leftimg.empty() || rightimg.empty())
                continue;

            double disparityOffset = 0;
            std::vector<SWD_charuco3DMark> marks;
            wd_BQ_getCharuco3DMark(
                leftimg, rightimg,
                cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R1, R2, P1, P2, Q,
                markInfo, boardInfo,
                disparityOffset,
                marks);

            sprintf_s(calibFile, sizeof(calibFile), "%sresult/marks_%d.txt", dataPath[grp], index);
            _outputMarkData(calibFile, marks);

            printf("%s:\n", filename);
            for (int i = 0; i < marks.size(); i++)
            {
                printf("markID:%d : (%f, %f, %f)\n", marks[i].markID, marks[i].mark3D.x, marks[i].mark3D.y, marks[i].mark3D.z);
            }
        }
    }
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件