#include "Libapi3d.h"
#include "IniHelper.h"
#include <iostream>
#include <vector>
#include <cmath>
#include <chrono>
#include "Log.h"	
#include "LidarHelper.h"
#include "Log.h"
#include "LidarHelper.h"


static void delay_second(double second)
{
	clock_t start_time;
	start_time = clock();
	for (; (clock() - start_time) < second * CLOCKS_PER_SEC;);
}

#ifndef _WIN32
static bool bCouldIsRecv = false;
static bool bCouldIsGet = false;
static uint32_t need_points_size = 30 * 10000;
static float points_data[40 * 10000 * 4];
//static uint32_t cur_point_num = 0;
static std::atomic<uint32_t> cur_point_num(0);
static void GetLidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data)
{
	if (data) {
		/** Parsing the timestamp and the point cloud data. */		uint64_t cur_timestamp = *((uint64_t*)(data->timestamp));
		if (data->data_type == kCartesian) {
			LivoxRawPoint* p_point_data = (LivoxRawPoint*)data->data;
		}
		else if (data->data_type == kSpherical) {
			LivoxSpherPoint* p_point_data = (LivoxSpherPoint*)data->data;
		}
		else if (data->data_type == kExtendCartesian) {
			LivoxExtendRawPoint* p_point_data = (LivoxExtendRawPoint*)data->data;
#ifdef _WIN32
			LivoxExtendRawPoint point_array[10 * 10000];
#else
			LivoxExtendRawPoint point_array[data_num];
#endif
			//uint32_t* converted_data = (uint32_t*)malloc(data_num * 4 * sizeof(uint32_t));
			if (!bCouldIsRecv)
			{
				uint32_t _cur_point_num = cur_point_num++;
				//cur_point_num += data_num;
				_cur_point_num = _cur_point_num * data_num;
				need_points_size = G(cloud_need_points_size);
				uint32_t _offset = _cur_point_num * 4;

				for (uint32_t i = 0; i < data_num; ++i) 
				{
					_offset = _offset + i * 4;
					points_data[_offset] = p_point_data[i].x / 1000.0;
					points_data[_offset + 1] = p_point_data[i].y / 1000.0;
					points_data[_offset + 2] = p_point_data[i].z / 1000.0;
					points_data[_offset + 3] = (uint32_t)(p_point_data[i].reflectivity << 8 | p_point_data[i].tag);
				}

				uint32_t _cur_count = _cur_point_num + data_num;
				if ((_cur_count % 100000) <= 100)
					std::cout << _cur_count << "  need data size : " << need_points_size  << std::endl;
				if (_cur_count < need_points_size)
					return;
				else
					bCouldIsRecv = true;
			}
			else
				return;
			
			// 等待点云拷贝结束
			//__p_point_data = p_point_data;
			//__data_num = data_num;
			while (true)
			{
				if (!bCouldIsGet)
				{
					delay_second(0.01);
					continue;
				}
				cur_point_num = 0;
				uint32_t* p = (uint32_t*)points_data;
				memset(p , 0, need_points_size * 4);
				bCouldIsGet = false;
				break;
			}
		}
		else if (data->data_type == kExtendSpherical) {
			LivoxExtendSpherPoint* p_point_data = (LivoxExtendSpherPoint*)data->data;
		}
		else if (data->data_type == kDualExtendCartesian) {
			LivoxDualExtendRawPoint* p_point_data = (LivoxDualExtendRawPoint*)data->data;
		}
		else if (data->data_type == kDualExtendSpherical) {
			LivoxDualExtendSpherPoint* p_point_data = (LivoxDualExtendSpherPoint*)data->data;
		}
		else if (data->data_type == kImu) {
			LivoxImuPoint* p_point_data = (LivoxImuPoint*)data->data;
		}
		else if (data->data_type == kTripleExtendCartesian) {
			LivoxTripleExtendRawPoint* p_point_data = (LivoxTripleExtendRawPoint*)data->data;
		}
		else if (data->data_type == kTripleExtendSpherical) {
			LivoxTripleExtendSpherPoint* p_point_data = (LivoxTripleExtendSpherPoint*)data->data;
		}
		// printf("data_type %d\n", data->data_type);
	}
}
#else
/** Extend cartesian coordinate format. */
typedef struct {
	int32_t x;            /**< X axis, Unit:mm */
	int32_t y;            /**< Y axis, Unit:mm */
	int32_t z;            /**< Z axis, Unit:mm */
	uint8_t reflectivity; /**< Reflectivity */
	uint8_t tag;          /**< Tag */
} LivoxExtendRawPoint;
typedef struct {
	uint8_t version;              /**< Packet protocol version. */
	uint8_t slot;                 /**< Slot number used for connecting LiDAR. */
	uint8_t id;                   /**< LiDAR id. */
	uint8_t rsvd;                 /**< Reserved. */
	uint32_t err_code;      /**< Device error status indicator information. */
	uint8_t timestamp_type;       /**< Timestamp type. */
	/** Point cloud coordinate format, refer to \ref PointDataType . */
	uint8_t data_type;
	uint8_t timestamp[8];         /**< Nanosecond or UTC format timestamp. */
	uint8_t data[1];              /**< Point cloud data. */
} LivoxEthPacket;
static bool bCouldIsRecv = false;
static bool bCouldIsGet = false;
static uint32_t need_points_size = 30 * 10000;
static float points_data[40 * 10000];
//static uint32_t cur_data_num = 0;
static std::atomic<uint32_t> cur_point_num(0);
typedef void (*FuncGetLidarData)(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data);
FuncGetLidarData GetLidarData;
#endif // !_WIN32


// 采集点云数据
static int lidar_indxx = 0;
int Cal3D::LibapiCapCloudPoints(std::shared_ptr<geometry::PointCloud>& cloud_ptr)
{
	int ret = CAL_OK;

	if (G(lidar_cap_fake) == "true")
	{
		if (G(fake_lidar_fpath) == "")
			assert(0);
		else
		{
			// 读取fake文件，获取点云数据
			ret = LibapiReadCloudPointsPly(G(fake_lidar_fpath), cloud_ptr);
			if (ret != CAL_OK)
			{
				return ret;
			}
			if (cloud_ptr == nullptr)
			{
				return CAL_NONE_PTR;
			}
			if (cloud_ptr.get()->points_.size() == 0)
			{
				return CAL_3D_CLOUD_POINTS_IS_0;
			}
		}
	}
	else
	{
		// cloud_ptr = cap demo ... 
		if (lidar_indxx == 0)
		{
			Loger(LEVEL_INFOR, "CLidarHelper::Get()->Init start");
			ret = CLidarHelper::Get()->Init((void*)GetLidarData);
			Loger(LEVEL_INFOR, "CLidarHelper::Get()->Init end");
			if (ret != CAL_OK)
			{
				Loger(LEVEL_INFOR, "CLidarHelper::Get()->Init() %d", ret);
				return ret;
			}
			lidar_indxx = 1;
		}
		else
		{
			Loger(LEVEL_INFOR, "CLidarHelper::Get()->Start start");
			ret = CLidarHelper::Get()->Start();
			Loger(LEVEL_INFOR, "CLidarHelper::Get()->Start end");
			if (ret != CAL_OK)
			{
				Loger(LEVEL_INFOR, "CLidarHelper::Get()->Start() %d", ret);
				return ret;
			}
		}
		
		// 写入点云对象
		Loger(LEVEL_INFOR, "cloud_ptr.get()->points_. get data start");
		geometry::PointCloud* pCloud = cloud_ptr.get();
		// int size = cloud_ptr.get()->points_.size() + __data_num ;
		// cloud_ptr.get()->points_.resize(size * 3 * sizeof(float));
		// wait数据
		while (true)
		{
			if (!bCouldIsRecv)
			{
				delay_second(0.05);
				continue;
			}
			delay_second(0.01);
			break;
		}
		pCloud->points_.resize(need_points_size);
		for (uint32_t i = 0; i < need_points_size; ++i) {
			pCloud->points_[i][0] = points_data[i * 4];
			pCloud->points_[i][1] = points_data[i * 4 + 1];
			pCloud->points_[i][2] = points_data[i * 4 + 2];
			//pCloud->points_.push_back({ (float)point[0], (float)point[1], (float)point[2] });
		}
		if (G(save_cload) == "true")
		{
			io::WritePointCloud("output.ply", *pCloud);
		}
			
		bCouldIsGet = true;
		delay_second(0.01);
		bCouldIsRecv = false; //开始接收数据
		
		Loger(LEVEL_INFOR, "cloud_ptr.get()->points_. get data end, data size is %d", pCloud->points_.size());

		Loger(LEVEL_INFOR, "CLidarHelper::Get()->Stop start");
		ret = CLidarHelper::Get()->Stop();
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "CLidarHelper::Get()->Stop() %d", ret);
			return ret;
		}
		Loger(LEVEL_INFOR, "CLidarHelper::Get()->Stop end");
	}

	return CAL_OK;
}

// 读取点云数据
int Cal3D::LibapiReadCloudPointsPly(const std::string plyPath, std::shared_ptr<geometry::PointCloud>& cloud_ptr)
{
	int ret = CAL_OK;

	cloud_ptr = io::CreatePointCloudFromFile(plyPath);

	/*if (!io::ReadPointCloud(plyPath, *cloud_ptr))*/
	if (cloud_ptr == nullptr)
	{
		Loger(LEVEL_INFOR, "Cal3D::LibapiReadCloudPointsPly Failed to read  %s", plyPath.c_str());
		return CAL_READFILE_FAILED;
	}

	if (cloud_ptr->IsEmpty())
	{
		Loger(LEVEL_INFOR, "cloud_ptr->IsEmpty() cloud_ptr : %s", plyPath.c_str());
		return CAL_PRAMA_EMPUTY;
		assert(0);
	}

	Loger(LEVEL_INFOR, "cloud_ptr read success: %s", plyPath.c_str());
	return ret;
}

// 保存点云数据
int Cal3D::LibapiSaveCloudPointsPly(std::string savaPath, std::shared_ptr<geometry::PointCloud>& cloud_ptr)
{
	if (cloud_ptr->IsEmpty())
		assert(0);

	int ret = CAL_OK;

	if (!io::WritePointCloud(savaPath, *cloud_ptr))
	{
		return CAL_WRITEFILE_FAILED;
	}

	return ret;
}

// 点云数据筛选
// 按照 point[0],point[1],point[2] = x, y, z
int Cal3D::LibapiFilterCloudPointsByDist(std::shared_ptr<geometry::PointCloud>& src_cloud_ptr,
	std::shared_ptr<geometry::PointCloud>& dst_cloud_ptr, float minusx, float x, float minusy, float y, float minusz, float z)
{
	if (src_cloud_ptr->IsEmpty())
		assert(0);

	int ret = CAL_OK;

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	// 遍历点云中的每个点
	for (int i = 0; i < src_cloud_ptr->points_.size(); ++i) {
		Eigen::Vector3d point_3d = src_cloud_ptr->points_[i];
		if (minusx < point_3d.x() && point_3d.x() < x
			&& minusy < point_3d.y() && point_3d.y() < y
			&& point_3d.z() < z)
		{
			Eigen::Vector3d dst_point_3d = Eigen::Vector3d(point_3d.x(), point_3d.y(), point_3d.z());
			dst_cloud_ptr->points_.push_back(dst_point_3d);
		}
	}

	if (dst_cloud_ptr->IsEmpty())
		assert(0);

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D::LibapiFilterCloudPointsByDist time is : " << elapsex.count() << std::endl;
	return ret;
}

// 点云修正
int Cal3D::LibapiCorrectCloudPoint(std::shared_ptr<geometry::PointCloud>& src_cloud_ptr,
	std::shared_ptr<geometry::PointCloud>& dst_cloud_ptr, int node)
{
	if (src_cloud_ptr->IsEmpty())
		assert(0);

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	int ret = CAL_OK;

	float kk = G(kk);
	float r;
	for (int i = 0; i < src_cloud_ptr->points_.size(); ++i)
	{
		Eigen::Vector3d point_3d = src_cloud_ptr->points_[i];
		//r = sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2));
		//point_3d[0] = point_3d[0] + point_3d[0] / r * kk;
		//point_3d[1] = point_3d[1] + point_3d[1] / r * kk;
		//point_3d[2] = point_3d[2] + point_3d[2] / r * kk;
		point_3d[2] -= kk;
		dst_cloud_ptr->points_.push_back(point_3d);
	}

	if (dst_cloud_ptr->IsEmpty())
		assert(0);

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D::LibapiCorrectCloudPoint time is : " << elapsex.count() << std::endl;
	return ret;
}

// 点云转数组
int Cal3D::LibapiCloudPointToVec(std::shared_ptr<geometry::PointCloud>& src_cloud_ptr,
	std::vector<cv::Point3d>& dst_3d)
{
	if (src_cloud_ptr->IsEmpty())
	{
		Loger(LEVEL_INFOR, "src_cloud_ptr->IsEmpty()");
		return CAL_PRAMA_EMPUTY;
		//assert(0);
	}

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	int ret = CAL_OK;

	for (int i = 0; i < src_cloud_ptr->points_.size(); ++i)
	{
		Eigen::Vector3d point_3d = src_cloud_ptr->points_[i];
		dst_3d.push_back(cv::Point3d(point_3d[0], point_3d[1], point_3d[2]));
	}

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D::LibapiCloudPointToVec time is : " << elapsex.count() << std::endl;

	return ret;
}

// 点云数据矩阵变换
int Cal3D::LibapiRotAndTrans(std::shared_ptr<geometry::PointCloud>& cloud_ptr)
{
	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	int ret = CAL_OK;

	//float y = 1.91032;
	//float p = -1.5938;
	//float r = -0.321605;

	Eigen::Matrix3d Rz = cloud_ptr->GetRotationMatrixFromXYZ({ 0, 0, G(y) });
	Eigen::Matrix3d Ry = cloud_ptr->GetRotationMatrixFromXYZ({ 0, G(p), 0 });
	Eigen::Matrix3d Rx = cloud_ptr->GetRotationMatrixFromXYZ({ G(r), 0, 0 });

	cloud_ptr->Rotate(Rx, { 1, 0, 0 });
	cloud_ptr->Rotate(Ry, { 0, 1, 0 });
	cloud_ptr->Rotate(Rz, { 0, 0, 1 });

	Eigen::Vector3d T_vector = Eigen::Vector3d({ G(tx), G(ty), G(tz) });

	cloud_ptr->Translate(T_vector);

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D::LibapiRotAndTrans time is : " << elapsex.count() << std::endl;

	return ret;
}

// 点云数据投影
// im 必须是去畸变图像 undistort image
int Cal3D::LibapiCloudPointsPorject(std::vector<cv::Point3d>& src_points3D,
	cv::Mat& cameraMatrix, std::vector<cv::Point2d>& dst_points2D, cv::Mat* im_2D3D, bool doTest, cv::Mat& test_src_im)
{
	if (src_points3D.size() == 0 || cameraMatrix.empty())
		assert(0);

	if (doTest && test_src_im.empty()) assert(0);

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	int ret = CAL_OK;

	cv::Mat rvec, tvec;
	rvec.create(1, 3, cv::DataType<float>::type);
	rvec.at<float>(0) = 0.0;
	rvec.at<float>(1) = 0.0;
	rvec.at<float>(2) = 0.0;
	tvec.create(3, 1, cv::DataType<float>::type);
	tvec.at<float>(0) = 0.0;
	tvec.at<float>(1) = 0.0;
	tvec.at<float>(2) = 0.0;
	cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << 0.0, 0.0, 0.0, 0.0, 0.0);

	// 投影点
	cv::projectPoints(src_points3D, rvec, tvec, cameraMatrix, distCoeffs, dst_points2D);

	if (dst_points2D.size() == 0)
		assert(0);

	cv::Mat test_im = cv::Mat::zeros(im_2D3D->size(), im_2D3D->type());
	int ch = im_2D3D->channels();;
	float* im_2D3D_row_ptr;
	uchar* test_im_row_ptr;
	for (int i = 0; i < dst_points2D.size(); ++i)
	{
		if (0 <= dst_points2D[i].x && dst_points2D[i].x < im_2D3D->cols && 0 <= dst_points2D[i].y && dst_points2D[i].y < im_2D3D->rows)
		{
			im_2D3D_row_ptr = (float*)im_2D3D->ptr<float>(int(dst_points2D[i].y));
			im_2D3D_row_ptr[(int)dst_points2D[i].x * ch] = src_points3D[i].x;
			im_2D3D_row_ptr[(int)dst_points2D[i].x * ch + 1] = src_points3D[i].y;
			im_2D3D_row_ptr[(int)dst_points2D[i].x * ch + 2] = src_points3D[i].z;

			//std::cout << im_2D3D_row_ptr[(int)dst_points2D[i].x * ch] << std::endl;
			//std::cout << im_2D3D_row_ptr[(int)dst_points2D[i].x * ch+1] << std::endl;
			//std::cout << im_2D3D_row_ptr[(int)dst_points2D[i].x * ch+2] << std::endl;

			if (doTest)
			{
				test_im_row_ptr = (uchar*)test_im.ptr<uchar>(int(dst_points2D[i].y));
				test_im_row_ptr[(int)dst_points2D[i].x * ch] = 255;
			}
		}
	}

	// for test
	if (doTest)
	{
		cv::Mat image_test_red = cv::Mat::zeros(test_src_im.size(), test_src_im.type());
		cv::Mat image_test_mask;
		image_test_red.setTo(cv::Scalar(0, 0, 255));
		cv::cvtColor(test_im, test_im, cv::COLOR_RGB2GRAY);
		cv::threshold(test_im, image_test_mask, 1, 255, cv::THRESH_BINARY);
		cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(9, 9), cv::Point(-1, -1));
		cv::dilate(image_test_mask, image_test_mask, kernel);
		cv::bitwise_and(image_test_red, test_src_im, test_src_im, image_test_mask);
#ifdef WIN32
		cv::resize(test_src_im, test_src_im, cv::Size(int(934), int(700)));
		cv::imshow("test_src_im", test_src_im);
		cv::waitKey(0);
#endif
	}

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D::LibapiCloudPointsPorject time is : " << elapsex.count() << std::endl;

	return ret;
}

int Cal3D::LibapiCloudPointsPorject(
	std::vector<cv::Point3d>& src_points3D,
	cv::Mat& cameraMatrix,
	std::vector <std::vector<cv::Point2f>>& vecConners,
	cv::Mat* im2D3D,
	std::vector<std::vector<cv::Point2f>>& vecInConners2D,
	std::vector<std::vector<cv::Point3f>>& vecInConners3D)
{
	if (src_points3D.size() == 0 || cameraMatrix.empty())
	{
		Loger(LEVEL_INFOR, "src_points3D.size() == 0 || cameraMatrix.empty()");
		return CAL_PRAMA_EMPUTY;
		//assert(0);
	}


	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	int ret = CAL_OK;

	cv::Mat rvec, tvec;
	rvec.create(1, 3, cv::DataType<float>::type);
	rvec.at<float>(0) = 0.0;
	rvec.at<float>(1) = 0.0;
	rvec.at<float>(2) = 0.0;
	tvec.create(3, 1, cv::DataType<float>::type);
	tvec.at<float>(0) = 0.0;
	tvec.at<float>(1) = 0.0;
	tvec.at<float>(2) = 0.0;
	cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << 0.0, 0.0, 0.0, 0.0, 0.0);

	// 投影点
	std::vector<cv::Point2d> dstPoints2D;
	cv::projectPoints(src_points3D, rvec, tvec, cameraMatrix, distCoeffs, dstPoints2D);

	if (dstPoints2D.empty())
	{
		Loger(LEVEL_INFOR, "dstPoints2D.empty()");
		return CAL_3D_PROJECT_POINTS_IS_0;
		//assert(0);
	}

	int ch = 3;
	float* im2D3DRowPtr;
	cv::Point2f p2d;
	std::vector<int> counts(vecConners.size());
	for (int r = 0; r < vecConners.size(); ++r)
		counts[r] = 0;
	for (int i = 0; i < dstPoints2D.size(); ++i)
	{
		if (0 <= dstPoints2D[i].x && dstPoints2D[i].x < im2D3D->cols && 0 <= dstPoints2D[i].y && dstPoints2D[i].y < im2D3D->rows)
		{
			im2D3DRowPtr = (float*)im2D3D->ptr<float>(int(dstPoints2D[i].y));
			im2D3DRowPtr[(int)dstPoints2D[i].x * ch] = src_points3D[i].x;
			im2D3DRowPtr[(int)dstPoints2D[i].x * ch + 1] = src_points3D[i].y;
			im2D3DRowPtr[(int)dstPoints2D[i].x * ch + 2] = src_points3D[i].z;
			p2d.x = dstPoints2D[i].x;
			p2d.y = dstPoints2D[i].y;

			for (int r = 0; r < vecConners.size(); ++r)
			{
				// 在ROI检测中，未正常检测到4个角点
				if (vecConners[r].size() != 4)
					continue;

				// 待优化
				// 收缩4个角点
				std::vector<cv::Point2f> vecConnersTemp;
				vecConnersTemp.push_back({ vecConners[r][0].x + 100.0f,vecConners[r][0].y + 100.0f });
				vecConnersTemp.push_back({ vecConners[r][1].x - 100.0f,vecConners[r][1].y + 100.0f });
				vecConnersTemp.push_back({ vecConners[r][2].x - 100.0f,vecConners[r][2].y - 100.0f });
				vecConnersTemp.push_back({ vecConners[r][3].x + 100.0f,vecConners[r][3].y - 100.0f });

				// 返回正值：点在多边形内部
				// 返回零：点在多边形上
				// 返回负值：点在多边形外部
				if (cv::pointPolygonTest(vecConnersTemp, p2d, false) >= 0)
				{
					counts[r]++;
					if ((counts[r] % 5) == 0)
					{
						cv::Point3f p3d = { (float)src_points3D[i].x, (float)src_points3D[i].y, (float)src_points3D[i].z };
						vecInConners3D[r].push_back(p3d);
						vecInConners2D[r].push_back(p2d);
					}
				}
			}
		}
	}
	return ret;
}

//int main() {
//	// 假设 img 是一个 CV_32FC3 类型的图像
//	cv::Mat img = cv::Mat::ones(480, 640, CV_32FC3) * 0.0001;  // 示例数据，值很小
//
//	// 获取最小值和最大值
//	double minVal, maxVal;
//	cv::minMaxLoc(img.reshape(1), &minVal, &maxVal); // 将图像展平到单通道以获得总体 min/max
//
//	// 如果 minVal 和 maxVal 接近，可以直接设置一个范围（例如 0 到 1）
//	if (maxVal - minVal < 1e-6) {
//		maxVal = minVal + 1.0;
//	}
//
//	// 手动进行归一化，将图像值扩展到 0 到 255 的范围
//	cv::Mat img_normalized;
//	img.convertTo(img_normalized, CV_32FC3, 1.0 / (maxVal - minVal), -minVal / (maxVal - minVal));
//	img_normalized.convertTo(img_normalized, CV_8UC3, 255.0);  // 转换为 8 位图像
//
//	// 显示图像
//	cv::imshow("Image", img_normalized);
//	cv::waitKey(0);
//
//	return 0;
//}
// 用于测试点云数据投影产生的2D3D数据
int Cal3D::LibapiCloudPointProjectTest(cv::Mat& im_2D3D)
{
	int ret = CAL_OK;

#ifdef WIN32
	cv::Mat tmp;
	im_2D3D.convertTo(tmp, CV_8UC3, 255.0);
	cv::resize(tmp, tmp, cv::Size(int(934), int(700)));
	tmp = tmp * 1000;
	cv::imshow("im_2D3D", tmp);
	cv::waitKey(0);
#endif

	return ret;
}

// 点云数据转换为2D3D点对数据
int Cal3D::LibapiCloudPointsToCMeasureInfo(cv::Mat& im_2D3D, CMeasureInfo& CMeasureInfo)
{
	if (im_2D3D.empty())
		assert(0);

	int ret = CAL_OK;

	//CMeasureInfo.im = &im_2D3D;

	return 0;
}

static Eigen::Matrix3f cvMatToEigenMap(const cv::Mat& mat)
{
	//if (mat.type() != CV_32FC1) {
	//	throw std::invalid_argument("输入矩阵必须是 CV_32FC1 类型");
	//}
	Eigen::Matrix3f eigenMat(mat.rows, mat.cols);

	// 将数据逐元素拷贝到 Eigen 矩阵
	for (int i = 0; i < mat.rows; ++i) {
		for (int j = 0; j < mat.cols; ++j) {
			eigenMat(i, j) = mat.at<double>(i, j);
		}
	}

	return eigenMat;
}

// 通过H，计算2D坐标的3D坐标
int Cal3D::LibapiCalConner3D(const std::vector<cv::Point2f>& vec2D_2, cv::Mat& H,
	std::vector<cv::Point3f> vec3d_n, std::vector<cv::Point3f>& vec3D_4)
{
	int ret = CAL_OK;

	open3d::geometry::PointCloud _3pts_cloud = open3d::geometry::PointCloud();
	for (const auto& point : vec3d_n) {
		_3pts_cloud.points_.push_back(Eigen::Vector3d(point.x, point.y, point.z));
	}
	//distance_threshold=0.01, ransac_n=3, num_iterations=500
	float A, B, C, D;
	std::vector<size_t> inliers;
	std::tuple<Eigen::Vector4d, std::vector<size_t>> result = _3pts_cloud.SegmentPlane(0.01, 3, 500);
	Eigen::Vector4d abcd = std::get<0>(result);
	A = abcd[0], B = abcd[1], C = abcd[2], D = abcd[3];
	inliers = std::get<1>(result);

	float x, y, t;
	Eigen::Matrix3f H_inv = cvMatToEigenMap(H).inverse();
	Eigen::Matrix3f xy1;
	Eigen::MatrixXf pts3d;
	cv::Point3f p3d(3);
	cv::Point3f p3d_next(3);
	for (int i = 0; i < 4; ++i)
	{
		cv::Point2f p2d = vec2D_2[i];
		x = p2d.x, y = p2d.y;
		xy1(0) = x, xy1(1) = y, xy1(2) = 1;
		pts3d = H_inv * xy1.conjugate();
		pts3d(0) = pts3d(0) / pts3d(2);
		pts3d(1) = pts3d(1) / pts3d(2);
		pts3d(2) = pts3d(2) / pts3d(2);
		t = -D / (A * pts3d(0) + B * pts3d(1) + C * pts3d(2));
		pts3d(0) = t * pts3d(0);
		pts3d(1) = t * pts3d(1);
		pts3d(2) = t * pts3d(2);
		p3d.x = pts3d(0);
		p3d.y = pts3d(1);
		p3d.z = pts3d(2);
		vec3D_4.push_back(p3d);
	}

	for (int i = 0; i < 4; ++i)
	{
		p3d = vec3D_4[i];
		p3d_next = vec3D_4[(i + 1) % 4];
		float edges = sqrt(pow((p3d.x - p3d_next.x), 2)
			+ pow((p3d.y - p3d_next.y), 2)
			+ pow((p3d.z - p3d_next.z), 2));
		std::cout << edges << std::endl;
	}

	return ret;
}

//
//int main()
//{
//	int ret = CAL_OK;
//
//	utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);
//
//	auto cloud_ptr = std::make_shared<geometry::PointCloud>();
//	auto cloud_ptr_by_dist = std::make_shared<geometry::PointCloud>();
//	auto cloud_ptr_by_correct = std::make_shared<geometry::PointCloud>();
//
//	ret = Cal3D::LibapiCapCloudPoints(cloud_ptr);
//	if (ret != CAL_OK) assert(0);
//
//	ret = Cal3D::LibapiRotAndTrans(cloud_ptr);
//
//	ret = Cal3D::LibapiFilterCloudPointsByDist(cloud_ptr, cloud_ptr_by_dist, 
//		G(dminx), G(dmaxx), G(dminy), G(dmaxy), G(dminz), G(dmaxz));
//	if (ret != CAL_OK) assert(0);
//	if (cloud_ptr_by_dist->IsEmpty()) assert(0);
//
//	ret = Cal3D::LibapiCorrectCloudPoint(cloud_ptr_by_dist, cloud_ptr_by_correct, 0);
//
//	std::vector<cv::Point3d> vec_point_3d;
//	ret = Cal3D::LibapiCloudPointToVec(cloud_ptr_by_correct, vec_point_3d);
//
//	cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << G(fx), 0, G(cx), 0, G(fy), G(cy), 0, 0, 1);
//	std::vector<cv::Point2d> vec_point_2d;
//	//cv::Mat test_src_im = cv::imread("output.png");
//	cv::Mat test_src_im;
//
//	//cv::Mat test_src_im;
//	cv::Mat* im_2D3D = new cv::Mat(cv::Size(G(w), G(h)), CV_32FC3);
//	im_2D3D->setTo(cv::Scalar(0, 0, 0)); // 设置为全黑背景
//	ret = Cal3D::LibapiCloudPointsPorject(vec_point_3d, cameraMatrix, vec_point_2d, im_2D3D, false, test_src_im);
//
//	// for test
//	//ret = Cal3D::LibapiCloudPointProjectTest(*im_2D3D);
//	//cv::FileStorage file("d:\\im_2D3D.yml", cv::FileStorage::WRITE);
//	//std::string tag = "im_2D3D";
//	//file << tag << *im_2D3D;
//	//file.release();
//
//	// 创建CMeasureInfo数据结构
//	long id;
//	ret = CalUtils::LibapiGenRandomId(id);
//	std::string info = "abc";
//	CMeasureInfo* CMeasureInfo_ptr = new CMeasureInfo(id, info, im_2D3D);
//
//	visualization::DrawGeometries({ cloud_ptr_by_correct }, "TestFileFormat", 1920, 1080);
//
//	return 0;
//}