#include "ArithPmd.h"
#include <fstream>
#define _USE_MATH_DEFINES
#include <math.h>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <pcl/filters/voxel_grid.h>

ArithPmd::ArithPmd()
{
}

ArithPmd::~ArithPmd()
{
}


// 扩边
static void expand_border(cv::InputOutputArray _src)
{
    cv::Mat src = cv::Mat::zeros(_src.size() + cv::Size(2, 2), _src.type());
    _src.getMat().copyTo(src(cv::Rect(1, 1, _src.size().width, _src.size().height)));
    _src.assign(src);
}

// 去除边界
static void remove_border(cv::InputOutputArray _src)
{
    cv::Mat src;
    _src.getMat()(cv::Rect(1, 1, _src.size().width - 2, _src.size().height - 2)).copyTo(src);
    _src.assign(src);
}
// 填充算法（漫水填充）
static void fill_hole(cv::InputArray _src, cv::OutputArray _dst) {
    cv::Mat src = _src.getMat();
    cv::Mat dst = src.clone();
    expand_border(dst);
    cv::floodFill(dst, cv::Point(0, 0), cv::Scalar(255));
    remove_border(dst);
    dst = src | (~dst);
    _dst.assign(dst);
}

// 小面积剔除
static void remove_small_region(cv::InputArray _src, cv::OutputArray _dst, int area_limit, int check_mode = 1, int neighbor_mode = 1) {
    cv::Mat src = _src.getMat();
    cv::Mat dst = _dst.getMat();
    int remove_count = 0;
    cv::Mat point_label = cv::Mat::zeros(src.size(), CV_8UC1);
    // 去除小连通区域的白色点
    if (check_mode == 1) {
        for (int i = 0; i < src.rows; ++i) {
            for (int j = 0; j < src.cols; ++j) {
                if (src.at<uchar>(i, j) < 10) {
                    point_label.at<uchar>(i, j) = 3;
                }
            }
        }
    }
    else
    {
        // 去除孔洞
        for (int i = 0; i < src.rows; i++)
        {
            for (int j = 0; j < src.cols; j++)
            {
                if (src.at<uchar>(i, j) > 10)
                {
                    point_label.at<uchar>(i, j) = 3;
                }
            }
        }
    }
    std::vector<cv::Point2i>neighbor_pos;
    neighbor_pos.push_back(cv::Point2i(-1, 0));
    neighbor_pos.push_back(cv::Point2i(1, 0));
    neighbor_pos.push_back(cv::Point2i(0, -1));
    neighbor_pos.push_back(cv::Point2i(0, 1));
    if (neighbor_mode == 1) {
        neighbor_pos.push_back(cv::Point2i(-1, -1));
        neighbor_pos.push_back(cv::Point2i(-1, 1));
        neighbor_pos.push_back(cv::Point2i(1, -1));
        neighbor_pos.push_back(cv::Point2i(1, 1));
    }
    int neighbor_count = 4 + 4 * neighbor_mode;
    int currx = 0, curry = 0;
    for (int i = 0; i < src.rows; i++)
    {
        for (int j = 0; j < src.cols; j++)
        {
            if (point_label.at<uchar>(i, j) == 0)
            {
                std::vector<cv::Point2i>grow_buffer;
                grow_buffer.push_back(cv::Point2i(j, i));
                point_label.at<uchar>(i, j) = 1;
                int check_result = 0;
                for (int z = 0; z < grow_buffer.size(); z++)
                {
                    for (int q = 0; q < neighbor_count; q++)
                    {
                        currx = grow_buffer.at(z).x + neighbor_pos.at(q).x;
                        curry = grow_buffer.at(z).y + neighbor_pos.at(q).y;
                        // 防越界
                        if (currx >= 0 && currx < src.cols && curry >= 0 && curry < src.rows)
                        {
                            if (point_label.at<uchar>(curry, currx) == 0)
                            {
                                grow_buffer.push_back(cv::Point2i(currx, curry));
                                point_label.at<uchar>(curry, currx) = 1;
                            }
                        }
                    }
                }
                if (grow_buffer.size() > area_limit)
                    check_result = 2;
                else
                {
                    check_result = 1;
                    remove_count++;
                }
                for (int z = 0; z < grow_buffer.size(); z++)
                {
                    currx = grow_buffer.at(z).x;
                    curry = grow_buffer.at(z).y;
                    point_label.at<uchar>(curry, currx) += check_result;
                }
            }
        }
    }
    check_mode = 255 * (1 - check_mode);
    // 反转面积过小的区域
    for (int i = 0; i < src.rows; ++i)
    {
        for (int j = 0; j < src.cols; ++j)
        {
            if (point_label.at<uchar>(i, j) == 2)
            {
                dst.at<uchar>(i, j) = check_mode;
            }
            else if (point_label.at<uchar>(i, j) == 3)
            {
                dst.at<uchar>(i, j) = src.at<uchar>(i, j);

            }
        }
    }
    _dst.assign(dst);
}

void static preprocess(cv::InputArray _src, cv::OutputArray _dst, int area_threshold)
{
    cv::Mat gray_image = _src.getMat();
    cv::Mat thresh_image, little_image, fill_image;
    // cvtColor(src, gray_image, cv::COLOR_BGR2GRAY);
    threshold(gray_image, thresh_image, 85, 255, cv::THRESH_BINARY);
    little_image = cv::Mat(thresh_image.size(), CV_8UC1, cv::Scalar(0));
    remove_small_region(thresh_image, little_image, area_threshold);

    fill_hole(little_image, fill_image);
    // cv::Mat mask;
    // threshold(fill_image, mask, 0, 1, cv::THRESH_BINARY);
    // cv::imwrite("mask.bmp", mask * 255);
    _dst.assign(fill_image);
}


bool save_matrix_as_img(const MatrixXf& m, std::string path) {
    MatrixXf t = (m.array() - m.minCoeff()).matrix();
    t = t * 255 / t.maxCoeff();
    Mat img;
    cv::eigen2cv(t, img);
    cv::imwrite(path, img);

    return true;
}

bool save_matrix_as_txt(const MatrixXf& m, std::string path, bool add_index, int orientation) {
    std::ofstream fout(path);

    MatrixXf m2 = m;
    if (add_index) {
        if (orientation == 0) {
            VectorXf t(m2.cols());
            for (uint32_t i = 0; i < t.rows(); ++i)
                t(i) = i;
            m2.row(0) = t.transpose();
        }
        else {
            VectorXf t(m.rows());
            for (uint32_t i = 0; i < t.rows(); ++i)
                t(i) = i;
            m2.col(0) = t;
        }
    }
    fout << m2;
    fout.close();
    return true;
}

bool save_matrix_as_ply(const MatrixXf& m, std::string path) {
    std::ofstream fout(path);
    fout << "ply\n"
        "format ascii 1.0\n"
        "element vertex " << m.cols() << "\n"
        "property float32 x\n"
        "property float32 y\n"
        "property float32 z\n"
        "end_header\n";

    for (uint32_t j = 0; j < m.cols(); j++)
        fout << m(0, j) << " " << m(1, j) << " " << m(2, j) << "\n";
    fout.close();
    return true;
}

MatrixXf matrix_to_home(const Eigen::MatrixXf& origin) {
    MatrixXf m(origin.rows() + 1, origin.cols());
    m.block(0, 0, origin.rows(), origin.cols()) = origin;
    m.row(origin.rows()) = VectorXf::Constant(origin.cols(), 1).transpose();
    return m;
}

bool configCameraRay(const Eigen::Matrix3f& cameraMatrix, const cv::Size& imageSize, float Z, MatrixXf& camera_rays) {
    const uint16_t width = imageSize.width;
    const uint16_t height = imageSize.height;

    // 初始化camera_rays,初始值为归一化后的像素坐标(第三行为1)
    camera_rays.resize(3, height * width);
    {
        // row 1
        VectorXf t1(width);
        for (uint16_t i = 0; i < width; ++i)
            t1(i) = i;
        for (uint16_t i = 0; i < height; ++i)
            camera_rays.block(0, i * width, 1, width) = t1.transpose();

        // row 2
        for (uint16_t i = 0; i < height; i++)
            camera_rays.block(1, i * width, 1, width) = VectorXf::Constant(width, i).transpose();

        // row 3
        camera_rays.row(2) = VectorXf::Constant(height * width, 1).transpose();
    }

    // 反归一化
    if (Z != 1.0)
        camera_rays *= Z;

    // 忽略镜头畸变，反相机内参
    camera_rays = cameraMatrix.inverse() * camera_rays;

    return true;
}

bool configRefPlane(const Eigen::Vector3f& plane_point, const Eigen::Vector3f& plane_normal, const Eigen::MatrixXf& camera_rays,
    Eigen::MatrixXf& refPlane) {
    Vector3f M(0, 0, 0);
    float t1 = (plane_point - M).transpose() * plane_normal;

    ArrayXXf t = t1 / (plane_normal.transpose() * camera_rays).array();
    refPlane.resize(camera_rays.rows(), camera_rays.cols());
    for (int i = 0; i < camera_rays.rows(); ++i)
        refPlane.row(i) = camera_rays.row(i).array() * t.array();
    if (M(0) != 0 || M(1) != 0 || M(2) != 0)
        refPlane = refPlane.colwise() + M;

    return true;
}

bool configScreenPixelPos(const Screen& S, const Matrix4f& WST, MatrixXf& screen_pix_pos) {
    screen_pix_pos.resize(3, S.rows * S.cols);

    // 每个像素去中心位置，如第一个像素取(0.5,0.5)
    // row 1
    VectorXf t1(S.cols);
    for (uint16_t i = 0; i < S.cols; ++i)
        t1(i) = i + 0.5;
    for (uint16_t i = 0; i < S.rows; ++i)
        screen_pix_pos.block(0, i * S.cols, 1, S.cols) = t1.transpose();
    screen_pix_pos.row(0) = S.width / S.cols * screen_pix_pos.row(0);

    // row 2
    for (uint16_t i = 0; i < S.rows; ++i)
        screen_pix_pos.block(1, i * S.cols, 1, S.cols) = VectorXf::Constant(S.cols, (i + 0.5) * S.height / S.rows).transpose();

    // row 3
    screen_pix_pos.row(2) = VectorXf::Constant(screen_pix_pos.cols(), 0).transpose();

    // 从屏幕坐标系转到世界坐标系
    MatrixXf t2 = matrix_to_home(screen_pix_pos);
    screen_pix_pos = (WST * t2).block(0, 0, 3, t2.cols());
    return true;
}

bool phase_shifting(const std::vector<MatrixXf>& imgs, std::vector<MatrixXf>& wraped_ps_maps) {
    // atan2((I1=I3)/(I0-I2))
    // 循环四组，而非每组四张图片
    for (uint16_t i = 0; i < 4; i++) {
        wraped_ps_maps[i] = MatrixXf(imgs[0].rows(), imgs[0].cols());
        MatrixXf tem0, tem1;
        tem0 = imgs[i * 4 + 1] - imgs[i * 4 + 3];
        tem1 = imgs[i * 4] - imgs[i * 4 + 2];
        wraped_ps_maps[i] = tem0.binaryExpr(tem1, [](float a, float b) {
            return atan2(a, b);
            });
    }

    return true;
}

bool phase_unwrapping(const vector<MatrixXf>& wraped_ps_maps, uint16_t period_width, uint16_t period_height, vector<MatrixXf>& unwrap_ps_maps) {
    // 由于制作条纹时相位减了pi，因此加上一个pi(参照低频条纹，不减pi相位是分段的)，来作为真实相位
    for (int i = 0; i < 2; ++i) {
        uint16_t period = i == 0 ? period_width : period_height;
        unwrap_ps_maps[i] = ((period * (wraped_ps_maps[i + 2].array() + M_PI).matrix() - (wraped_ps_maps[i].array() + M_PI).matrix()) /
            (2 * M_PI)).array().round().matrix(); // 计算得到 K
        unwrap_ps_maps[i] = (wraped_ps_maps[i].array() + M_PI).matrix() + 2 * M_PI * unwrap_ps_maps[i]; // phi + 2*pi*k
    }
    return true;
}

bool screen_camera_phase_match(const vector<MatrixXf>& upm, const Screen& s, uint16_t pw, uint16_t ph, const MatrixXf& spp, MatrixXf& res) {
    uint32_t img_size = upm[0].cols() * upm[0].rows();
    MatrixXfR coor_x_f = upm[0] * s.cols / (2 * pw * M_PI);
    MatrixXfR coor_y_f = upm[1] * s.rows / (2 * ph * M_PI);
    coor_x_f.resize(img_size, 1);
    coor_y_f.resize(img_size, 1);
    VectorXUI coor_x = coor_x_f.array().round().matrix().cast<uint32_t>();
    VectorXUI coor_y = coor_y_f.array().round().matrix().cast<uint32_t>();

    res.resize(3, img_size);
    for (uint32_t i = 0; i < coor_x.rows(); ++i) {
        if (coor_x(i) >= s.cols)
            coor_x(i) = s.cols - 1;
        if (coor_y(i) >= s.rows)
            coor_y(i) = s.rows - 1;
        res.col(i) = spp.col(coor_y(i) * s.cols + coor_x(i));
    }

    return true;
}

bool slope_calculate(const Vector3f& camera_world, const MatrixXf& refPlane, const MatrixXf& screen_camera_phase_match_pos,
    std::vector<MatrixXfR>& slope) {
    ArrayXXf dm2c, dm2s, M_C(3, refPlane.cols()), M_S;

    if (camera_world.cols() == 1) {
        M_C.row(0) = refPlane.row(0).array() - camera_world(0, 0);
        M_C.row(1) = refPlane.row(1).array() - camera_world(1, 0);
        M_C.row(2) = refPlane.row(2).array() - camera_world(2, 0);
    }
    else
        M_C = (refPlane - camera_world).array();
    M_S = (refPlane - screen_camera_phase_match_pos).array();

    dm2c = M_C.matrix().colwise().norm().array();
    dm2s = M_S.matrix().colwise().norm().array();

    ArrayXXf denominator = -(M_C.row(2) / dm2c + M_S.row(2) / dm2s);

    for (uint32_t i = 0; i < 2; ++i)
        slope[i] = ((M_C.row(i) / dm2c + M_S.row(i) / dm2s) / denominator).matrix();

    return true;
}

bool modal_reconstruction(const MatrixXf& sx, const MatrixXf& sy, MatrixXfR& Z, const std::vector<float>& rg, uint32_t terms, const MatrixXf& mask) {
    // resize 斜率，2*M*N x 1
    MatrixXfR s(sx.rows() * 2, sx.cols());
    s.block(0, 0, sx.rows(), sx.cols()) = sx;
    s.block(sx.rows(), 0, sy.rows(), sy.cols()) = sy;
    s.resize(s.rows() * s.cols(), 1);

    // 多项式矩阵
    MatrixXfR slope_p(s.rows(), terms * terms);   // 斜率多项式矩阵
    MatrixXfR Z_p(s.rows() / 2, terms * terms); // 高度多项式矩阵
    double xa = rg[0], xb = rg[1], ya = rg[2], yb = rg[3];
    for (uint32_t i = 0; i < sx.rows(); ++i) {
        for (uint32_t j = 0; j < sx.cols(); ++j) {
            if (mask(i, j) == 0) continue;  // 跳过 mask 为 0 的部分
            double y = (ya + yb - 2 * (yb - (yb - ya) / (sx.rows() - 1) * i)) / (ya - yb);
            double x = (xa + xb - 2 * ((xb - xa) / (sx.cols() - 1) * j + xa)) / (xa - xb);

            // 多项式有terms * terms 项，两层循环，先x后y, Dy_2 指 D_(n-2)
            float Dy_2 = 1, Dy_1 = y, dy_1 = 0;
            for (uint32_t n = 0; n < terms; ++n) {
                float Dy, dy;
                if (n == 0) {
                    Dy = 1;
                    dy = 0;
                }
                else if (n == 1) {
                    Dy = y;
                    dy = n * Dy_1 + y * dy_1;
                }
                else {
                    Dy = ((2 * n - 1) * y * Dy_1 - (n - 1) * Dy_2) / n;
                    dy = n * Dy_1 + y * dy_1;
                }

                float Dx_2 = 1, Dx_1 = x, dx_1 = 0;
                for (uint32_t m = 0; m < terms; ++m) {
                    float Dx, dx;
                    if (m == 0) {
                        Dx = 1;
                        dx = 0;
                    }
                    else if (m == 1) {
                        Dx = x;
                        dx = m * Dx_1 + x * dx_1;
                    }
                    else {
                        Dx = ((2 * m - 1) * x * Dx_1 - (m - 1) * Dx_2) / m;
                        dx = m * Dx_1 + x * dx_1;
                    }
                    slope_p(i * sx.cols() + j, n * terms + m) = dx * Dy;
                    slope_p(sx.cols() * sx.rows() + i * sx.cols() + j, n * terms + m) = Dx * dy;

                    Z_p(i * sx.cols() + j, n * terms + m) = Dx * Dy;

                    Dx_2 = Dx_1;
                    Dx_1 = Dx;
                    dx_1 = dx;
                }

                Dy_2 = Dy_1;
                Dy_1 = Dy;
                dy_1 = dy;
            }
        }
    }

    // 计算系数
    VectorXf C = slope_p.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(s);

    // 恢复高度
    Z = Z_p * C;
    Z.resize(sx.rows(), sx.cols());
    /*
    float Z_max = Z.maxCoeff();
    float Z_min = Z.minCoeff();
    Z = (Z.array() - Z_min) / (Z_max - Z_min);  // 归一化到 [0, 1]
    Z = 2 * Z.array() - 1;  // 再归一化到 [-1, 1]

    Z /= 1000;
    */
    return true;
}

double computeReprojectionErrors(
    const vector<vector<cv::Point3f>>& objectPoints,
    const vector<vector<cv::Point2f>>& imagePoints,
    const vector<Mat>& rvecs, const vector<Mat>& tvecs,
    const Mat& cameraMatrix, const Mat& distCoeffs,
    vector<float>& perViewErrors) {
    vector<cv::Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for (i = 0; i < (int)objectPoints.size(); i++) {
        projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i],
            cameraMatrix, distCoeffs, imagePoints2);
        err = cv::norm(Mat(imagePoints[i]), Mat(imagePoints2), cv::NORM_L2);
        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float)std::sqrt(err * err / n);
        totalErr += err * err;
        totalPoints += n;
    }

    return std::sqrt(totalErr / totalPoints);
}

/*
bool aruco_analyze(const vector<Mat>& imgs_board, const ArucoBoard& board, const Mat& cameraMatrix, const Mat& distCoeffs, vector<Mat>& Rmats,
    vector<cv::Vec3f>& Tvecs, vector<vector<int>>& Ids, vector<vector<vector<cv::Point2f>>>& corners,
    vector<vector<vector<cv::Point2f>>>& rejectedCandidates, std::string output_dir) {
    for (uint16_t i = 0; i < imgs_board.size(); i++) {
        cv::Mat img = imgs_board[i].clone();
        auto parameters = cv::aruco::DetectorParameters::create();
        cv::aruco::detectMarkers(img, board.dictionary, corners[i], Ids[i], parameters, rejectedCandidates[i], cameraMatrix, distCoeffs);

        Mat output_img;
        cvtColor(imgs_board[i], output_img, cv::COLOR_GRAY2RGB);
        cv::aruco::drawDetectedMarkers(output_img, corners[i], Ids[i], cv::Scalar(0, 255, 0));

        // transforms points from the board coordinate system to the camera coordinate system
        cv::Ptr<cv::aruco::GridBoard> aruco_board = cv::aruco::GridBoard::create(board.markers_size.width, board.markers_size.height, board.markerLength,
            board.markerSeparation, board.dictionary, 0);
        cv::Vec3f rvec;
        int boardNum = cv::aruco::estimatePoseBoard(corners[i], Ids[i], aruco_board, cameraMatrix, distCoeffs, rvec, Tvecs[i]);

        // 检测到标记板
        if (boardNum > 0) {
            Rodrigues(rvec, Rmats[i]);
            cv::drawFrameAxes(output_img, cameraMatrix, distCoeffs, rvec, Tvecs[i], 0.04, 1);
            imwrite(output_dir + "/sys_calib_output/img_poseBoard_" + std::to_string(i) + ".bmp", output_img);
        }
        else
            return false;
    }

    return true;
}
*/

bool system_calib(vector<Matrix3f>& CVR, vector<Vector3f>& CVTL, Matrix3f& CSR, VectorXf& CSTL_D, vector<Vector3f>& n) {
    vector<Vector3f> m(3);  // m12,m23,m31
    for (uint16_t i = 0; i < CVR.size(); i++) {
        Eigen::EigenSolver<Matrix3f> es(CVR[i] * CVR[(i + 1) % 3].transpose(), true);
        auto eval = es.eigenvalues();

        // get eigenvector whos corresponfing eigenvalue is 1
        for (int j = 0; j < eval.rows(); j++) {
            std::complex<double> evalj = eval(j);
            if (fabs(1 - evalj.real()) <= 0.0001) {
                auto evec = es.eigenvectors();
                for (int t = 0; t < evec.rows(); t++) {
                    m[i](t) = evec(t, j).real();
                }
                break;
            }
        }
    }

    // sovling three normal vector(n)
    n[0] = m[0].cross(m[2]);
    n[1] = m[0].cross(m[1]);
    n[2] = m[1].cross(m[2]);
    for (int i = 0; i < 3; i++) {
        if (n[i](2) > 0)
            n[i] = -n[i];
        n[i].normalize();
    }

    // sovling CSR
    Matrix3f I3 = Matrix3f::Identity(3, 3);
    Matrix3f CSR0 = (I3 - 2 * n[0] * n[0].transpose()).inverse() * CVR[0];
    Matrix3f CSR1 = (I3 - 2 * n[1] * n[1].transpose()).inverse() * CVR[1];
    Matrix3f CSR2 = (I3 - 2 * n[2] * n[2].transpose()).inverse() * CVR[2];
    CSR = (CSR0 + CSR1 + CSR2) / 3;

    MatrixXf T_A(9, 6);
    MatrixXf T_B(9, 1);
    Vector3f Zero3 = Vector3f::Zero(3);
    T_A << (I3 - 2 * n[0] * n[0].transpose()), -2 * n[0], Zero3, Zero3,
        (I3 - 2 * n[1] * n[1].transpose()), Zero3, -2 * n[1], Zero3,
        (I3 - 2 * n[2] * n[2].transpose()), Zero3, Zero3, -2 * n[2];
    T_B << CVTL[0],
        CVTL[1],
        CVTL[2];
    CSTL_D = T_A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(T_B);

    return true;
}


int ArithPmd::Initlization(const nlohmann::json& param)
{
    Matrix3f cameraMatrix0, cameraMatrix1, cameraMatrix2, cameraMatrix3; // 相机内参
    MatrixXf camera_rays0, camera_rays1, camera_rays2, camera_rays3; // 相机光线,相机坐标系下
    cv::Mat distcoeffs0, distcoeffs1, distcoeffs2, distcoeffs3; // 相机镜头畸变
    Matrix4f CWT0, CWT1, CWT2, CWT3; // 世界坐标系(参考平面坐标系)向量转到相机坐标系下的变换矩阵
    Matrix4f CST0, CST1, CST2, CST3; // screen 坐标系向量转到相机坐标系下的变换矩阵

    std::vector<uint32_t> t0 = param.at("ROI");
    roi0 = { t0[0], t0[1], t0[2], t0[3] };
    roi1 = { t0[0], t0[1], t0[2], t0[3] };
    roi2 = { t0[0], t0[1], t0[2], t0[3] };
    roi3 = { t0[0], t0[1], t0[2], t0[3] };

    std::vector<float> t1 = param.at("cameraMatrix0");
    MatrixXfR t2(1, t1.size());
    t2.row(0) = VectorXf::Map(&t1[0], t1.size());
    t2.resize(3, 3);
    cameraMatrix0 = t2;

    std::vector<float> t1_1 = param.at("cameraMatrix1");
    MatrixXfR t2_1(1, t1_1.size());
    t2_1.row(0) = VectorXf::Map(&t1_1[0], t1_1.size());
    t2_1.resize(3, 3);
    cameraMatrix1 = t2_1;

    std::vector<float> t1_2 = param.at("cameraMatrix2");
    MatrixXfR t2_2(1, t1_2.size());
    t2_2.row(0) = VectorXf::Map(&t1_2[0], t1_2.size());
    t2_2.resize(3, 3);
    cameraMatrix2 = t2_2;

    std::vector<float> t1_3 = param.at("cameraMatrix3");
    MatrixXfR t2_3(1, t1_3.size());
    t2_3.row(0) = VectorXf::Map(&t1_3[0], t1_3.size());
    t2_3.resize(3, 3);
    cameraMatrix3 = t2_3;

    std::vector<float> t3 = param.at("distCoeffs0");
    Mat t5 = Mat(t3);
    distcoeffs0 = t5.clone();

    std::vector<float> t3_1 = param.at("distCoeffs1");
    Mat t5_1 = Mat(t3_1);
    distcoeffs1 = t5_1.clone();

    std::vector<float> t3_2 = param.at("distCoeffs2");
    Mat t5_2 = Mat(t3_2);
    distcoeffs2 = t5_2.clone();

    std::vector<float> t3_3 = param.at("distCoeffs3");
    Mat t5_3 = Mat(t3_3);
    distcoeffs3 = t5_3.clone();

    std::vector<uint16_t> t4 = param.at("image_size");
    img_size.width = t4[0];
    img_size.height = t4[1];

    Screen screen0 = { param.at("screenWidth"), param.at("screenHeight"), param.at("screenCols"), param.at("screenRows") };
    screen = screen0;
    period_width = param.at("period_width");
    period_height = param.at("period_height");

    configCameraRay(cameraMatrix0, img_size, 1.0, camera_rays0);
    configCameraRay(cameraMatrix1, img_size, 1.0, camera_rays1);
    configCameraRay(cameraMatrix2, img_size, 1.0, camera_rays2);
    configCameraRay(cameraMatrix3, img_size, 1.0, camera_rays3);

    // ----------------计算--------------------
    // 读取CWT，计算参考平面与相机光线交点
    std::vector<float> t7 = param.at("CWT0");
    MatrixXfR t8(1, t7.size());
    t8.row(0) = VectorXf::Map(&t7[0], t7.size());
    t8.resize(4, 4);
    CWT0 = t8;
    Vector4f O = matrix_to_home(Vector3f::Constant(3, 0));
    camera_origin_world0 = (CWT0.inverse() * O).head(3);
    // camera_origin_world.push_back(&camera_origin_world0);

    configRefPlane(CWT0.block(0, 3, 3, 1), CWT0.block(0, 2, 3, 1), camera_rays0, refPlane0);
    camera_rays0.resize(0, 0);
    // 上面计算的是再相机坐标系下的参考平面，将其转换到世界坐标系下
    MatrixXf ref_homo = matrix_to_home(refPlane0);
    ref_homo = CWT0.inverse() * ref_homo;
    refPlane0 = ref_homo.block(0, 0, 3, ref_homo.cols());
    // refPlane.push_back(&refPlane0);

    // 读取屏幕信息，条纹信息，投影条纹
    std::vector<float> t9 = param.at("CST0");
    MatrixXfR t10(1, t9.size());
    t10.row(0) = VectorXf::Map(&t9[0], t9.size());
    t10.resize(4, 4);
    CST0 = t10;
    Matrix4f WST = CWT0.inverse() * CST0;
    configScreenPixelPos(screen, WST, screen_pix_pos0);
    // screen_pix_pos.push_back(&screen_pix_pos0);

    // 读取CWT，计算参考平面与相机光线交点
    std::vector<float> t7_1 = param.at("CWT1");
    MatrixXfR t8_1(1, t7_1.size());
    t8_1.row(0) = VectorXf::Map(&t7_1[0], t7_1.size());
    t8_1.resize(4, 4);
    CWT1 = t8_1;
    Vector4f O_1 = matrix_to_home(Vector3f::Constant(3, 0));
    camera_origin_world1 = (CWT1.inverse() * O_1).head(3);
    // camera_origin_world.push_back(&camera_origin_world1);

    configRefPlane(CWT1.block(0, 3, 3, 1), CWT1.block(0, 2, 3, 1), camera_rays1, refPlane1);
    camera_rays1.resize(0, 0);
    // 上面计算的是再相机坐标系下的参考平面，将其转换到世界坐标系下
    MatrixXf ref_homo_1 = matrix_to_home(refPlane1);
    ref_homo_1 = CWT1.inverse() * ref_homo_1;
    refPlane1 = ref_homo_1.block(0, 0, 3, ref_homo_1.cols());
    // refPlane.push_back(&refPlane1);

    // 读取屏幕信息，条纹信息，投影条纹
    std::vector<float> t9_1 = param.at("CST1");
    MatrixXfR t10_1(1, t9_1.size());
    t10_1.row(0) = VectorXf::Map(&t9_1[0], t9_1.size());
    t10_1.resize(4, 4);
    CST1 = t10_1;
    Matrix4f WST_1 = CWT1.inverse() * CST1;
    configScreenPixelPos(screen, WST_1, screen_pix_pos1);
    // screen_pix_pos.push_back(&screen_pix_pos1);

    // 读取CWT，计算参考平面与相机光线交点
    std::vector<float> t7_2 = param.at("CWT2");
    MatrixXfR t8_2(1, t7_2.size());
    t8_2.row(0) = VectorXf::Map(&t7_2[0], t7_2.size());
    t8_2.resize(4, 4);
    CWT2 = t8_2;
    Vector4f O_2 = matrix_to_home(Vector3f::Constant(3, 0));
    camera_origin_world2= (CWT2.inverse() * O_2).head(3);
    // camera_origin_world.push_back(&camera_origin_world2);

    configRefPlane(CWT2.block(0, 3, 3, 1), CWT2.block(0, 2, 3, 1), camera_rays2, refPlane2);
    camera_rays2.resize(0, 0);
    // 上面计算的是再相机坐标系下的参考平面，将其转换到世界坐标系下
    MatrixXf ref_homo_2 = matrix_to_home(refPlane2);
    ref_homo_2 = CWT2.inverse() * ref_homo_2;
    refPlane2 = ref_homo_2.block(0, 0, 3, ref_homo_2.cols());
    // refPlane.push_back(&refPlane2);

    // 读取屏幕信息，条纹信息，投影条纹
    std::vector<float> t9_2 = param.at("CST2");
    MatrixXfR t10_2(1, t9_2.size());
    t10_2.row(0) = VectorXf::Map(&t9_2[0], t9_2.size());
    t10_2.resize(4, 4);
    CST2 = t10_2;
    Matrix4f WST_2 = CWT2.inverse() * CST2;
    configScreenPixelPos(screen, WST_2, screen_pix_pos2);
    // screen_pix_pos.push_back(&screen_pix_pos2);

    // 读取CWT，计算参考平面与相机光线交点
    std::vector<float> t7_3 = param.at("CWT3");
    MatrixXfR t8_3(1, t7_3.size());
    t8_3.row(0) = VectorXf::Map(&t7_3[0], t7_3.size());
    t8_3.resize(4, 4);
    CWT3 = t8_3;
    Vector4f O_3 = matrix_to_home(Vector3f::Constant(3, 0));
    camera_origin_world3 = (CWT3.inverse() * O_3).head(3);
    // camera_origin_world.push_back(&camera_origin_world3);

    configRefPlane(CWT3.block(0, 3, 3, 1), CWT3.block(0, 2, 3, 1), camera_rays3, refPlane3);
    camera_rays3.resize(0, 0);
    // 上面计算的是再相机坐标系下的参考平面，将其转换到世界坐标系下
    MatrixXf ref_homo_3 = matrix_to_home(refPlane3);
    ref_homo_3 = CWT3.inverse() * ref_homo_3;
    refPlane3 = ref_homo_3.block(0, 0, 3, ref_homo_3.cols());
    // refPlane.push_back(&refPlane3);

    // 读取屏幕信息，条纹信息，投影条纹
    std::vector<float> t9_3 = param.at("CST3");
    MatrixXfR t10_3(1, t9_3.size());
    t10_3.row(0) = VectorXf::Map(&t9_3[0], t9_3.size());
    t10_3.resize(4, 4);
    CST3 = t10_3;
    Matrix4f WST_3 = CWT3.inverse() * CST3;
    configScreenPixelPos(screen, WST_3, screen_pix_pos3);
    // screen_pix_pos.push_back(&screen_pix_pos3);

    std::vector<float> t11 = param.at("CTC1");
    MatrixXfR t12(1, t11.size());
    t12.row(0) = VectorXf::Map(&t11[0], t11.size());
    t12.resize(4, 4);
    CTC0 = t12;
    // CTC.push_back(&CTC0);

    std::vector<float> t11_1 = param.at("CTC2");
    MatrixXfR t12_1(1, t11_1.size());
    t12_1.row(0) = VectorXf::Map(&t11_1[0], t11_1.size());
    t12_1.resize(4, 4);
    CTC1 = t12_1; 
    // CTC.push_back(&CTC1);

    std::vector<float> t11_2 = param.at("CTC3");
    MatrixXfR t12_2(1, t11_2.size());
    t12_2.row(0) = VectorXf::Map(&t11_2[0], t11_2.size());
    t12_2.resize(4, 4);
    CTC2 = t12_2;
    // CTC.push_back(&CTC2);

    return 0;
}

static int product_mask(const MatrixXf& x, MatrixXf& y,  MatrixXf& roi_m)
{
    // 检查输入矩阵的尺寸是否匹配
    if (x.rows() != roi_m.rows() || x.cols() != roi_m.cols()) {
        return -1; // 返回错误代码，表示尺寸不匹配
    }
    // 初始化输出矩阵y，大小与x相同
    y = MatrixXf::Zero(x.rows(), x.cols());

    // 逐元素相乘
    y = x.array() * roi_m.array();

    return 0; // 返回0表示成功
    
}

int ArithPmd::Preprocess(const std::vector<cv::Mat>& images, int area_threshold, int32_t camera_index)
{
    cv::Mat input = cv::Mat::zeros(images[0].size(), images[0].type());
    for (const auto& img : images) {
        cv::add(input, img, input);
    }
    cv::Mat fill_image;
    preprocess(input, fill_image, area_threshold);

    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(fill_image, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    cv::Rect largest_bbox;
    double max_area = 0;
    for (const auto& contour : contours) {
        double area = cv::contourArea(contour);
        if (area > max_area) {
            max_area = area;
            largest_bbox = cv::boundingRect(contour);
        }
    }

    // cv::Mat mask;
    if (camera_index == 0)
    {
        threshold(fill_image, mask0, 0, 1, cv::THRESH_BINARY);
        // cv::imwrite("mask.bmp", mask * 255);
        cv::Mat roi_mask = mask0(largest_bbox);
        cv::cv2eigen(roi_mask, roi_m0);
        roi0.startRow = largest_bbox.y;
        roi0.startCol = largest_bbox.x;
        roi0.blockRows = largest_bbox.height;
        roi0.blockCols = largest_bbox.width;
    }
    else if (camera_index == 1)
    {
        threshold(fill_image, mask1, 0, 1, cv::THRESH_BINARY);
        // cv::imwrite("mask.bmp", mask * 255);
        cv::Mat roi_mask = mask1(largest_bbox);
        cv::cv2eigen(roi_mask, roi_m1);
        roi1.startRow = largest_bbox.y;
        roi1.startCol = largest_bbox.x;
        roi1.blockRows = largest_bbox.height;
        roi1.blockCols = largest_bbox.width;
    }
    else if (camera_index == 2)
    {
        threshold(fill_image, mask2, 0, 1, cv::THRESH_BINARY);
        // cv::imwrite("mask.bmp", mask * 255);
        cv::Mat roi_mask = mask2(largest_bbox);
        cv::cv2eigen(roi_mask, roi_m2);
        roi2.startRow = largest_bbox.y;
        roi2.startCol = largest_bbox.x;
        roi2.blockRows = largest_bbox.height;
        roi2.blockCols = largest_bbox.width;
    }
    else
    {
        threshold(fill_image, mask3, 0, 1, cv::THRESH_BINARY);
        // cv::imwrite("mask.bmp", mask * 255);
        cv::Mat roi_mask = mask3(largest_bbox);
        cv::cv2eigen(roi_mask, roi_m3);
        roi3.startRow = largest_bbox.y;
        roi3.startCol = largest_bbox.x;
        roi3.blockRows = largest_bbox.height;
        roi3.blockCols = largest_bbox.width;
    }
    
    return 0;
}



int ArithPmd::Predict(const std::vector<Eigen::MatrixXf>& img_pats, MatrixXfR& pcl, int32_t camera_index)
{
    ROI roi;
    MatrixXf roi_m;
    if (camera_index == 0)
    {
        roi = roi0;
        roi_m = roi_m0;
    }
    else if (camera_index == 1)
    {
        roi = roi1;
        roi_m = roi_m1;
    }
    else if (camera_index == 2)
    {
        roi = roi2;
        roi_m = roi_m2;
    }
    else
    {
        roi = roi3;
        roi_m = roi_m3;
    }

    std::vector<MatrixXf> ps_maps(2);
    {
        std::vector<MatrixXf> wraped_ps_maps(4);
        phase_shifting(img_pats, wraped_ps_maps);
        phase_unwrapping(wraped_ps_maps, period_width, period_height, ps_maps);
    }

    // save_matrix_as_img(ps_maps[0], "phase_unwrap_x.bmp");
    // save_matrix_as_img(ps_maps[1], "phase_unwrap_y.bmp");
    // 屏幕像素与像素相位匹配，获取屏幕像素对应三维坐标,世界坐标系下
    MatrixXf screen_camera_phase_match_pos; // 尺寸为 roi
    {
        std::vector<MatrixXf> ps_maps_roi0(2);
        std::vector<MatrixXf> ps_maps_roi(2);
        for (uint32_t i = 0; i < 2; ++i)
        {
            ps_maps_roi0[i] = ps_maps[i].block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);
            ps_maps_roi[i] = MatrixXf::Zero(ps_maps_roi0[i].rows(), ps_maps_roi0[i].cols());
            int r = product_mask(ps_maps_roi0[i], ps_maps_roi[i],roi_m);
        }
        if (camera_index == 0)
        {
            screen_camera_phase_match(ps_maps_roi, screen, period_width, period_height, screen_pix_pos0,
                screen_camera_phase_match_pos);
        }
        else if (camera_index == 1)
        {
            screen_camera_phase_match(ps_maps_roi, screen, period_width, period_height, screen_pix_pos1,
                screen_camera_phase_match_pos);
        }
        else if (camera_index == 2)
        {
            screen_camera_phase_match(ps_maps_roi, screen, period_width, period_height, screen_pix_pos2,
                screen_camera_phase_match_pos);
        }
        else
        {
            screen_camera_phase_match(ps_maps_roi, screen, period_width, period_height, screen_pix_pos3,
                screen_camera_phase_match_pos);
        }
        // screen_camera_phase_match(ps_maps_roi, screen, period_width, period_height, *screen_pix_pos[camera_index],screen_camera_phase_match_pos);
        save_matrix_as_img(ps_maps_roi[0], "ps_maps_roi_x.bmp");
        save_matrix_as_img(ps_maps_roi[1], "ps_maps_roi_y.bmp");
    }
    
    // 斜率计算
    std::vector<MatrixXfR> slope(2);
    {
        // 取 refPlane 的 roi
        std::vector<MatrixXf> refP(2);
        MatrixXfR temp;
        MatrixXf ref_roi(3, roi.blockRows * roi.blockCols);
        for (uint32_t i = 0; i < 3; ++i) {
            if (camera_index == 0)
            {
                refP[0] = refPlane0.row(i);
            }
            else if (camera_index == 1)
            {
                refP[0] = refPlane1.row(i);
            }
            else if (camera_index == 2)
            {
                refP[0] = refPlane2.row(i);
            }
            else
            {
                refP[0] = refPlane3.row(i);
            }
            refP[0].resize(img_size.height, img_size.width);
            
            temp  = refP[0].block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);
            refP[1] = MatrixXf::Zero(temp.rows(), temp.cols());
            int r1 = product_mask(temp, refP[1], roi_m);
            /*
            refP[1] = refP[0].block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);
            */
            refP[1].resize(1, roi.blockRows * roi.blockCols);
            ref_roi.row(i) = refP[1];
        }

        if (camera_index == 0)
        {
            slope_calculate(camera_origin_world0, ref_roi, screen_camera_phase_match_pos, slope);
        }
        else if (camera_index == 1)
        {
            slope_calculate(camera_origin_world1, ref_roi, screen_camera_phase_match_pos, slope);
        }
        else if (camera_index == 2)
        {
            slope_calculate(camera_origin_world2, ref_roi, screen_camera_phase_match_pos, slope);
        }
        else
        {
            slope_calculate(camera_origin_world3, ref_roi, screen_camera_phase_match_pos, slope);
        }
        // slope_calculate(*camera_origin_world[camera_index], ref_roi, screen_camera_phase_match_pos, slope);
        for (uint32_t i = 0; i < 2; ++i)
            slope[i].resize(roi.blockRows, roi.blockCols);
    }

    MatrixXfR Z;
    MatrixXf refPlane_t;
    if (camera_index == 0)
    {
        refPlane_t = refPlane0;
    }
    else if (camera_index == 1)
    {
        refPlane_t = refPlane1;
    }
    else if (camera_index == 2)
    {
        refPlane_t = refPlane2;
    }
    else
    {
        refPlane_t = refPlane3;
    }

    MatrixXfR X_ref = refPlane_t.row(0), Y_ref = refPlane_t.row(1);
    X_ref.resize(img_size.height, img_size.width);
    Y_ref.resize(img_size.height, img_size.width);

    MatrixXfR temp_1;
    temp_1 = X_ref.block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);
    MatrixXf X_roi = MatrixXf::Zero(temp_1.rows(), temp_1.cols());
    int r_x = product_mask(temp_1, X_roi, roi_m);
    MatrixXfR temp_2;
    temp_2 = Y_ref.block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);
    MatrixXf Y_roi = MatrixXf::Zero(temp_2.rows(), temp_2.cols());
    int r_y = product_mask(temp_2, Y_roi, roi_m);
    // MatrixXfR Y_roi = Y_ref.block(roi.startRow, roi.startCol, roi.blockRows, roi.blockCols);

    std::vector<float> range = { temp_1(0, 0) * 1000, temp_1(0, roi.blockCols - 1) * 1000,
                                temp_2(0, 0) * 1000, temp_2(roi.blockRows - 1, 0) * 1000,
    };

    modal_reconstruction(slope[0], slope[1], Z, range, 4, roi_m);
    roi_m.resize(0, 0);

    // 第一种做法
    X_roi.resize(1, roi.blockCols * roi.blockRows);
    Y_roi.resize(1, roi.blockCols * roi.blockRows);
    Z.resize(1, roi.blockCols * roi.blockRows);
    

    // Temporary vectors to store non-zero elements
    vector<float> new_X, new_Y, new_Z;

    // Traverse the matrices and add non-zero elements to the new vectors
    for (int i = 0; i < roi.blockRows * roi.blockCols; ++i) {
        if (X_roi(0, i) != 0 || Y_roi(0, i) != 0) {
            new_X.push_back(X_roi(0, i));
            new_Y.push_back(Y_roi(0, i));
            new_Z.push_back(Z(0, i));
        }
    }

    // Resize the matrices
    MatrixXf X_roi_, Y_roi_, Z_;
    X_roi_.resize(1, new_X.size());
    Y_roi_.resize(1, new_Y.size());
    Z_.resize(1, new_Z.size());

    // Copy the non-zero elements back to the matrices
    for (int i = 0; i < new_X.size(); ++i) {
        X_roi_(0, i) = new_X[i];
        Y_roi_(0, i) = new_Y[i];
        Z_(0, i) = new_Z[i];
    }


    float X_max = X_roi_.maxCoeff();
    float X_min = X_roi_.minCoeff();
    X_roi_ = (X_roi_.array() - X_min) / (X_max - X_min);  // 归一化到 [0, 1]
    X_roi_ = 2 * X_roi_.array() - 1;  // 再归一化到 [-1, 1]
    // X_roi_ = X_roi_ / 10;

    float Y_max = Y_roi_.maxCoeff();
    float Y_min = Y_roi_.minCoeff();
    Y_roi_ = (Y_roi_.array() - Y_min) / (Y_max - Y_min);  // 归一化到 [0, 1]
    Y_roi_ = 2 * Y_roi_.array() - 1;  // 再归一化到 [-1, 1]
    // Y_roi_ = Y_roi_ / 10;

    float Z_max = Z_.maxCoeff();
    float Z_min = Z_.minCoeff();
    Z_ = (Z_.array() - Z_min) / (Z_max - Z_min);  // 归一化到 [0, 1]
    Z_ = 2 * Z_.array() - 1;  // 再归一化到 [-1, 1]
    Z_ = Z_ / 1000;

    pcl.resize(3, new_X.size());
    pcl.row(0) = X_roi_;
    pcl.row(1) = Y_roi_;
    pcl.row(2) = Z_;
    
    roi.startRow = 0;
    roi.startCol = 0;
    roi.blockRows = 0;
    roi.blockCols = 0;
    /*
    X_roi.resize(1, roi.blockCols * roi.blockRows);
    Y_roi.resize(1, roi.blockCols * roi.blockRows);
    Z.resize(1, roi.blockCols * roi.blockRows);

    pcl.resize(3, roi.blockCols * roi.blockRows);
    pcl.row(0) = X_roi;
    pcl.row(1) = Y_roi;
    pcl.row(2) = Z;
    */
    
    /*
    // 第二种做法
    Eigen::Map<Eigen::MatrixXf> flatX(X_roi.data(), X_roi.size(), 1);
    Eigen::Map<Eigen::MatrixXf> flatY(Y_roi.data(), X_roi.size(), 1);
    Eigen::Map<Eigen::MatrixXf> flatZ(Z.data(), X_roi.size(), 1);
    Eigen::Map<Eigen::MatrixXf> flatMask(roi_m.data(), roi_m.size(), 1);

    Eigen::MatrixXf product_x = flatX.array() * flatMask.array();
    Eigen::MatrixXf product_y = flatY.array() * flatMask.array();
    Eigen::MatrixXf product_z = flatZ.array() * flatMask.array();
    std::vector<float> filtered_x, filtered_y, filtered_z;
    for (int i = 0; i < product_x.size(); ++i) {
        if (product_x(i) != 0) {
            filtered_x.push_back(product_x(i));
            filtered_y.push_back(product_y(i));
            filtered_z.push_back(product_z(i));
        }
    }

    // X_roi.resize(1, roi.blockCols * roi.blockRows);
    // Y_roi.resize(1, roi.blockCols * roi.blockRows);
    // Z.resize(1, roi.blockCols * roi.blockRows);

    pcl.resize(3, filtered_x.size());
    for (int i = 0; i < filtered_x.size(); ++i) {
        pcl(0, i) = filtered_x[i];  // X坐标
        pcl(1, i) = filtered_y[i];  // Y坐标
        pcl(2, i) = filtered_z[i];  // Z坐标
    }
    */
    return 0;
}

int ArithPmd::Stitch(const pcl::PointCloud<pcl::PointXYZ>& cloud1, const pcl::PointCloud<pcl::PointXYZ>& cloud2, pcl::PointCloud<pcl::PointXYZ>& cloud_output, int32_t camera_index)
{
    pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter;
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (camera_index == 0)
    {
        pcl::transformPointCloud(cloud1, *transformed_cloud, CTC0);
    }
    else if (camera_index == 1)
    {
        pcl::transformPointCloud(cloud1, *transformed_cloud, CTC1);
    }
    else if (camera_index == 2)
    {
        pcl::transformPointCloud(cloud1, *transformed_cloud, CTC2);
    }
    else
    {
        pcl::transformPointCloud(cloud1, *transformed_cloud, CTC2);
    }
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud3(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> cloud_all = (*transformed_cloud) + cloud2;
    cloud3 = cloud_all.makeShared();
    double Res = 0.4;
    downSizeFilter.setLeafSize(Res, Res, Res);
    downSizeFilter.setInputCloud(cloud3);
    downSizeFilter.filter(cloud_output);
    return 0;
}