#ifndef _EXPORTING 
#define _EXPORTING 
#endif 

#include "WaferRebuild.h"
#include "ArithPmd.h"

WaferRebuild::WaferRebuild()
{
    arith_pmd = new ArithPmd();
}

WaferRebuild::~WaferRebuild()
{
    if (arith_pmd != nullptr)
    {
        delete arith_pmd;
        arith_pmd = nullptr;
    }
}

std::tuple<int, std::string> WaferRebuild::initParam(const nlohmann::json& param)
{
    try
    {
        int retcode = arith_pmd->Initlization(param);
        return std::make_tuple(0, "sucess");
    }
    catch (const std::exception& e)
    {
        return std::make_tuple(-1, e.what());
    }
	
}


std::tuple<int, std::string> WaferRebuild::calcCloud(const std::vector<std::shared_ptr<infra::Image>>& image_list, std::shared_ptr<infra::CloudPoint<double>>& cloud_points, int32_t camera_index)
{
    try
    {
        if (!cloud_points) {
            throw std::runtime_error("Error: cloud_points is nullptr!");
        }
        std::vector<Eigen::MatrixXf> img_pats;
        std::vector<cv::Mat> gray_images;
        img_pats.resize(image_list.size());
        for (int i = 0; i < image_list.size(); ++i) {
            int type = (image_list[i]->type() == -1) ? CV_8UC1 : CV_8UC3; // 假设 -1 是灰度图，其他都是三通道图像
            cv::Mat mat;
            cv::Mat grayMat;
            if (type == CV_8UC1) {
                grayMat = cv::Mat(arith_pmd->img_size.height, arith_pmd->img_size.width, type, image_list[i]->data());
                gray_images.push_back(grayMat);
            }
            else {
                mat = cv::Mat(arith_pmd->img_size.height, arith_pmd->img_size.width, type, image_list[i]->data());
                cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY);
                gray_images.push_back(grayMat);
            }
            // cv::cv2eigen(grayMat, img_pats[i]);
        }
        std::vector<cv::Mat> gray_temp{ gray_images[gray_images.size() - 4], gray_images[gray_images.size() - 3], gray_images[gray_images.size() - 2], gray_images[gray_images.size() - 1] };
        for (int i = 0; i < gray_images.size(); ++i)
        {
            cv::Mat temp;
            if (camera_index == 0)
            {
                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask0);
            }
            else if (camera_index == 1)
            {
                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask1);
            }
            else if (camera_index == 2)
            {
                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask2);
            }
            else if (camera_index == 3)
            {
                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask3);
            }
            // cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask);
            cv::imwrite("temp.bmp", temp);
            cv::cv2eigen(temp, img_pats[i]);
        }
        int retcode0 = arith_pmd->Preprocess(gray_temp, 500, camera_index);
        MatrixXfR pcl;
        int retcode = arith_pmd->Predict(img_pats, pcl, camera_index);

        // std::vector<infra::Point3d<float>> points;

        // 预留足够空间
        int numPoints = pcl.cols();
        cloud_points->cloud_point.resize(numPoints);
        // cloud_points->cloud_point.reserve(numPoints);
        // cloud_points.reserve(numPoints);
        // 遍历每一个点
        for (int i = 0; i < numPoints; ++i) {
            // 创建一个新的 Point3d 对象并添加到 vector
            infra::Point3d<double> point = infra::Point3d<double>(
                pcl(0, i), // X 坐标
                pcl(1, i), // Y 坐标 m  
                pcl(2, i)  // Z 坐标
            );
            cloud_points->cloud_point[i] = point;
        }
        save_matrix_as_ply(pcl, "surface.ply");
        return std::make_tuple(0, "sucess");
    }
    catch (const std::exception& e) {
        return std::make_tuple(-1, e.what());
    }

}

pcl::PointCloud<pcl::PointXYZ> convertToPCLPointCloud(const std::shared_ptr<infra::CloudPoint<double>>& cloud_points) {
    pcl::PointCloud<pcl::PointXYZ> cloud;

    if (!cloud_points) {
        std::cerr << "Error: Input cloud_points is nullptr." << std::endl;
        return cloud;
    }

    // Resize the PCL PointCloud to the size of cloud_points
    cloud.points.resize(cloud_points->size());

    // Convert each point from infra::CloudPoint<double> to pcl::PointXYZ
    for (size_t i = 0; i < cloud_points->size(); ++i) {
        const infra::Point3d<double>& point = cloud_points->cloud_point[i];
        cloud.points[i].x = static_cast<float>(point.x);
        cloud.points[i].y = static_cast<float>(point.y);
        cloud.points[i].z = static_cast<float>(point.z);
    }

    return cloud;
}

static std::shared_ptr<infra::CloudPoint<double>> convertToInfraCloudPoint(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
    // Create a new infra::CloudPoint<double> object
    auto cloud_points = std::make_shared<infra::CloudPoint<double>>();

    // Resize the cloud_points->cloud_point vector to the size of cloud
    cloud_points->cloud_point.resize(cloud.points.size());

    // Convert each pcl::PointXYZ point to infra::Point3d<double> and store in cloud_points
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        cloud_points->cloud_point[i].x = static_cast<double>(cloud.points[i].x);
        cloud_points->cloud_point[i].y = static_cast<double>(cloud.points[i].y);
        cloud_points->cloud_point[i].z = static_cast<double>(cloud.points[i].z);
    }

    return cloud_points;
}

std::tuple<int, std::string> WaferRebuild::mergeCloud(const std::map<int32_t, std::shared_ptr<infra::CloudPoint<double>>>& cloud_points_list,
    std::shared_ptr<infra::CloudPoint<double>>& cloud_points)
{
    try
    {
        int error_code = 0;
        std::string error_message = "Success";
        pcl::PointCloud<pcl::PointXYZ> cloud_output;
        auto it = cloud_points_list.begin();
        if (it == cloud_points_list.end()) {
            error_code = -1;
            error_message = "Error: Input cloud_points_list is empty.";
            return std::make_tuple(error_code, error_message);
        }
       cloud_output = convertToPCLPointCloud(it->second);
        ++it;
        // Merge each subsequent cloud in the list
        while (it != cloud_points_list.end()) {
            pcl::PointCloud<pcl::PointXYZ> cloud_to_merge = convertToPCLPointCloud(it->second);

            // Call Stitch function to merge cloud_output and cloud_to_merge
            int stitch_result = arith_pmd->Stitch(cloud_output, cloud_to_merge, cloud_output, it->first);
            if (stitch_result != 0) {
                error_code = stitch_result;
                error_message = "Error: Stitching failed for camera index " + std::to_string(it->first);
                break;
            }
            ++it;
        }
        cloud_points = convertToInfraCloudPoint(cloud_output);

        return std::make_tuple(error_code, error_message);
    }
    catch (const std::exception& e) {
        return std::make_tuple(-1, e.what());
    }
}


static infra::Point3d<double> calculateCentroid(const pcl::PointCloud<pcl::PointXYZ>& pcl_cloud) {
    infra::Point3d<double> centroid;

    // Create an Eigen vector to store the centroid
    Eigen::Vector4f pcl_centroid;

    // Compute centroid using PCL function
    pcl::compute3DCentroid(pcl_cloud, pcl_centroid);

    // Convert the centroid from Eigen vector to infra::Point3d<double>
    centroid.x = pcl_centroid[0];
    centroid.y = pcl_centroid[1];
    centroid.z = pcl_centroid[2];

    return centroid;
}


std::tuple<int, std::string> WaferRebuild::warpage(const std::shared_ptr<infra::CloudPoint<double>>& cloud_points, int32_t filterMM, nlohmann::json& result)
{
    try {
        if (!cloud_points) {
            throw std::runtime_error("Input cloud_points is nullptr.");
        }

        // Convert infra::CloudPoint<double> to pcl::PointCloud<pcl::PointXYZ>
        pcl::PointCloud<pcl::PointXYZ> pcl_cloud = convertToPCLPointCloud(cloud_points);

        // Calculate centroid (assuming centroid calculation is implemented in math_tool.h)
        infra::Point3d<double> centroid = calculateCentroid(pcl_cloud);  // Example function, replace with actual implementation

        // Define x and y axes
        infra::Point3d<double> axis_x(1.0, 0.0, 0.0);  // Assuming x-axis direction
        infra::Point3d<double> axis_y(0.0, 1.0, 0.0);  // Assuming y-axis direction

        // Calculate heights and populate result JSON
        for (auto& axis : { "x", "y" }) {
            std::vector<double> heights;
            double bow = 0.0;
            double warpage = 0.0;

            infra::Point3d<double> axis_vector = (axis == "x") ? axis_x : axis_y;

            // Calculate heights and populate heights vector
            for (size_t i = 0; i < pcl_cloud.points.size(); ++i) {
                infra::Point3d<double>& point = cloud_points->cloud_point[i];
                double height = point.x * axis_vector.x + point.y * axis_vector.y + point.z * axis_vector.z;
                heights.push_back(height);
            }

            // Calculate bow (average height)
            if (!heights.empty()) {
                bow = std::accumulate(heights.begin(), heights.end(), 0.0) / heights.size();
            }

            // Calculate warpage (standard deviation of heights)
            if (heights.size() > 1) {
                double sum_sq_diff = std::inner_product(heights.begin(), heights.end(), heights.begin(), 0.0);
                double mean = bow;
                double variance = sum_sq_diff / heights.size() - mean * mean;
                warpage = std::sqrt(variance);
            }

            // Fill JSON result
            result[axis]["height"] = heights;
            result[axis]["bow"] = bow;
            result[axis]["warpage"] = warpage;
        }

        /*
        // Calculate max bow and max warpage
        double max_bow = 0.0;
        double max_warpage = 0.0;
        double angle_max_bow = 0.0;
        double angle_max_warpage = 0.0;

        // Calculate max_bow and max_warpage values
        // (example calculation, replace with actual implementation)
        // Assume you have functions to find maximum bow and warpage and their angles
        max_bow = calculateMaxBow(result);
        max_warpage = calculateMaxWarpage(result);
        angle_max_bow = calculateAngleMaxBow(result);
        angle_max_warpage = calculateAngleMaxWarpage(result);

        // Fill max bow and max warpage in JSON result
        result["max_bow"]["angle"] = angle_max_bow;
        result["max_bow"]["height"] = result[angle_max_bow]["height"];
        result["max_bow"]["max_bow"] = max_bow;

        result["max_warpage"]["angle"] = angle_max_warpage;
        result["max_warpage"]["height"] = result[angle_max_warpage]["height"];
        result["max_warpage"]["max_warpage"] = max_warpage;
        */
        return std::make_tuple(0, "Success");
    }
    catch (const std::exception& e) {
        return std::make_tuple(-1, e.what());
    }
}