#include <pcl/visualization/cloud_viewer.h>
#include<list>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/obj_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/io.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/region_growing.h>


#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>  
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
//SYJ新增

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/extract_indices.h>

#include <limits>
#include <vector>

//去除雜點
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/common/transforms.h>

//讀寫檔案
#include<iostream>
#include<fstream>
#include <filesystem>
#include <iostream>  
#include <algorithm> 
#include <windows.h>
using namespace std;
// 使用 Eigen 庫的常數定義 M_PI
#define _USE_MATH_DEFINES
#include <cmath>
#include <pcl/registration/icp.h>
//#define SIZE 20

//struct callback_args {
//	bool* isShow;
//	pcl::PointCloud<pcl::PointXYZ>::Ptr orgin_points;
//	pcl::visualization::PCLVisualizer::Ptr viewerPtr;
//	pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d;
//};
//void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
//{
//	struct callback_args* data = (struct callback_args*)args;
//	if (event.getPointIndex() == -1)
//		return;
//	int index = event.getPointIndex();
//	std::cout << "index: " << index << std::endl;
//	pcl::PointXYZ current_point;
//	event.getPoint(current_point.x, current_point.y, current_point.z);
//	data->clicked_points_3d->points.push_back(current_point);
//	// Draw clicked points in red:
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(data->clicked_points_3d, 255, 0, 0);
//	data->viewerPtr->removePointCloud("clicked_points");
//	data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
//	data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
//	std::cout << current_point.x << "  " << current_point.y << "  " << current_point.z << std::endl;
//}
// 根據文件修改時間排序
bool compareFileTime(const std::filesystem::directory_entry& a, const std::filesystem::directory_entry& b) {
	return a.last_write_time() > b.last_write_time();
}
void adjustVoxelGridLeafSize(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int target_min_points, int target_max_points, float& leaf_size) {
	int original_count = cloud->points.size();
	float adjustment_factor = 0.01f;  // 每次調整的因子

	pcl::VoxelGrid<pcl::PointXYZ> voxelGrid;

	// 自動調整體素大小以達到目標點雲數量範圍
	while (true) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr downsampledCloud(new pcl::PointCloud<pcl::PointXYZ>);
		voxelGrid.setInputCloud(cloud);
		voxelGrid.setLeafSize(leaf_size, leaf_size, leaf_size);
		voxelGrid.filter(*downsampledCloud);

		int downsampled_count = downsampledCloud->points.size();

		if (downsampled_count >= target_min_points && downsampled_count <= target_max_points) {
			break;  // 點數在範圍內，停止調整
		}
		else if (downsampled_count > target_max_points) {
			leaf_size += adjustment_factor;  // 增加體素大小，減少點雲數量
		}
		else {
			leaf_size -= adjustment_factor;  // 減小體素大小，增加點雲數量
		}
	}
}

int main(int argc, char** argv)
{
	// 定義.pcd檔案的路徑
	//string s = "C:\\c#\\WindowsFormsApp1\\PCDemo\\";
	std::string file_path_1 = "C:\\c#\\cam\\金樣本\\Gold Standard.pcd";
	//std::string file_path_2 = "C:\\c#\\cam\\比對樣本2024_04_23_14_09_29\\Compare samples.pcd";
	// 定義目錄路徑
	std::string cam_path = "C:\\c#\\cam\\";

	// 創建一個存儲目錄中所有 "比對樣本" 文件夾的向量
	std::vector<std::filesystem::directory_entry> subdirs;

	// cam目錄中的所有子目錄
	for (const auto& entry : std::filesystem::directory_iterator(cam_path)) {
		if (entry.is_directory() && entry.path().filename().string().find("比對樣本") != std::string::npos) {
			subdirs.push_back(entry);
		}
	}

	// 按照文件夾修改時間排序
	std::sort(subdirs.begin(), subdirs.end(), compareFileTime);

	if (!subdirs.empty())
	{
		// 獲取最新的比對樣本文件夾
		auto latest_subdir = subdirs.front();
		std::string file_path_2 = latest_subdir.path().string() + "\\Compare samples.pcd";

		std::cout << "最新比對樣本的路徑是: " << file_path_2 << std::endl;
	

	// 創建點雲對象
	// 初始化點雲對象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
	pcl::PointCloud<pcl::Normal>::Ptr Normals(new pcl::PointCloud<pcl::Normal>);
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_and_normal(new pcl::PointCloud<pcl::PointXYZINormal>);
	// 從.pcd檔案中讀取點雲數據
	pcl::io::loadPCDFile(file_path_1, *cloud1);
	pcl::io::loadPCDFile(file_path_2, *cloud2);

	// 設定點雲數量範圍
	int target_min_points = 1000;  // 最小點數
	int target_max_points = 1500;  // 最大點數

	// 自動調整體素大小
	float leaf_size_1 = 1.0f;  // 初始體素大小
	float leaf_size_2 = 1.0f;

	adjustVoxelGridLeafSize(cloud1, target_min_points, target_max_points, leaf_size_1);
	adjustVoxelGridLeafSize(cloud2, target_min_points, target_max_points, leaf_size_2);

	std::cout << "體素大小 (cloud1): " << leaf_size_1 << std::endl;
	std::cout << "體素大小 (cloud2): " << leaf_size_2 << std::endl;

	// 向下採樣後的點雲
	pcl::PointCloud<pcl::PointXYZ>::Ptr downsampledCloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr downsampledCloud2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> voxelGrid;

	voxelGrid.setInputCloud(cloud1);
	voxelGrid.setLeafSize(leaf_size_1, leaf_size_1, leaf_size_1);
	voxelGrid.filter(*downsampledCloud1);

	voxelGrid.setInputCloud(cloud2);
	voxelGrid.setLeafSize(leaf_size_2, leaf_size_2, leaf_size_2);
	voxelGrid.filter(*downsampledCloud2);

	std::cout << "點雲1 向下採樣後的點數: " << downsampledCloud1->points.size() << std::endl;
	std::cout << "點雲2 向下採樣後的點數: " << downsampledCloud2->points.size() << std::endl;

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; // 使用完整類名
	icp.setInputSource(cloud2);
	icp.setInputTarget(cloud1);

	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);

	if (icp.hasConverged())
	{
		// 確認配準成功
		std::cout << "ICP 配準成功。" << std::endl;
		std::cout << "得分: " << icp.getFitnessScore() << std::endl;

		// 獲取最終的變換矩陣
		Eigen::Matrix4f transformation = icp.getFinalTransformation();

		// 提取位移並轉換為毫米
		Eigen::Vector3f translation = transformation.block<3, 1>(0, 3);  // 轉換為毫米
		float x = translation.x();
		float y = translation.y();
		float z = translation.z();
		std::cout << "XYZ 位移 (毫米): X=" << translation.x() << ", Y=" << translation.y() << ", Z=" << translation.z() << std::endl;
		
		// 提取旋轉部分
		Eigen::Matrix3f rotation = transformation.block<3, 3>(0, 0);
		Eigen::Quaternionf quaternion(rotation);

		// 將四元數轉換為歐拉角
		Eigen::Vector3f euler = quaternion.toRotationMatrix().eulerAngles(0, 1, 2);

		// 將弧度轉換為度數
		Eigen::Vector3f euler_degrees = euler * (180.0 / M_PI);
		float Rx = euler_degrees.x();
		float Ry = euler_degrees.y();
		float Rz = euler_degrees.z();
		// 顯示RX, RY, RZ
		std::cout << "旋轉 (RX, RY, RZ) 度數: RX=" << euler_degrees.x() << ", RY=" << euler_degrees.y() << ", RZ=" << euler_degrees.z() << std::endl;
		// 指定輸出文件的路徑
		std::string outputFilePath = "C:\\c#\\cam\\output.txt";

		std::ofstream outputFile(outputFilePath);
		outputFile << std::fixed << std::setprecision(2);
		outputFile << x << "," << y << "," << z << "," << Rx << "," << Ry << "," << Rz << std::endl;
		outputFile.close();
	}
	else
	{
		std::cout << "ICP 配準失敗。" << std::endl;
	}
		

	// 創建可視化器
	/*pcl::visualization::PCLVisualizer viewer("Cloud Viewer");*/


	////找到最淺的点
	//pcl::PointXYZ deepestPoint;
	//double deepestZ = std::numeric_limits<double>::max(); // 初始设为最大值
	//for (size_t i = 0; i < cloud2->size(); ++i) {
	//	if (cloud2->points[i].z < deepestZ) {
	//		deepestZ = cloud2->points[i].z;
	//		deepestPoint = cloud2->points[i];
	//	}
	//}

	//
	//// 找到最深的点
	//pcl::PointXYZ shallowestPoint;
	//double shallowestZ = std::numeric_limits<double>::lowest(); // 初始设为最小值
	//for (size_t i = 0; i < cloud2->size(); ++i) {
	//	if (cloud2->points[i].z > shallowestZ) {
	//		shallowestZ = cloud2->points[i].z;
	//		shallowestPoint = cloud2->points[i];
	//	}
	//}
	 
	// 找出點雲資料的 y 軸最小值和最大值
	double yMin = std::numeric_limits<double>::max();
	double yMax = std::numeric_limits<double>::lowest();
	for (size_t i = 0; i < cloud2->size(); ++i) {
		if (cloud2->points[i].y < yMin) {
			yMin = cloud2->points[i].y;
		}
		if (cloud2->points[i].y > yMax) {
			yMax = cloud2->points[i].y;
		}
	}

	// 根據 y 軸的範圍來定義矩形框的大小
	double boxSize = (yMax - yMin) / 5;  // 以五行為主
	double boxMin = yMin;
	double boxMax = boxMin + boxSize;

	// 在定義的矩形框中找到最淺的點和最深的點
	pcl::PointXYZ shallowestPoint, deepestPoint;
	double shallowestZ = std::numeric_limits<double>::lowest();
	double deepestZ = std::numeric_limits<double>::max();
	for (size_t i = 0; i < cloud2->size(); ++i) {
		if (cloud2->points[i].y >= boxMin && cloud2->points[i].y <= boxMax) {
			if (cloud2->points[i].z > shallowestZ) {
				shallowestZ = cloud2->points[i].z;
				shallowestPoint = cloud2->points[i];
			}
			if (cloud2->points[i].z < deepestZ) {
				deepestZ = cloud2->points[i].z;
				deepestPoint = cloud2->points[i];
			}
		}
	}
	std::cout << "最深點=" << shallowestPoint << ", 最淺點=" << deepestPoint  << std::endl;





	//// 将最淺点添加到点云中
	/*pcl::PointCloud<pcl::PointXYZ>::Ptr deepestPointCloud(new pcl::PointCloud<pcl::PointXYZ>);
	deepestPointCloud->push_back(deepestPoint);*/
	
	//// 將最深點添加到点云中
	/*pcl::PointCloud<pcl::PointXYZ>::Ptr shallowestPointCloud(new pcl::PointCloud<pcl::PointXYZ>);
	shallowestPointCloud->push_back(shallowestPoint);*/


	//// 渲染最淺點為藍色
	/*viewer.addPointCloud(deepestPointCloud, "deepestPoint");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 1.0, "deepestPoint");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "deepestPoint");*/

	// 渲染最深點為紅色
	/*viewer.addPointCloud(shallowestPointCloud, "shallowestPoint");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 0.0, "shallowestPoint");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "shallowestPoint");*/


	//使用下採樣後的點雲進行視覺化
	//viewer.addPointCloud(downsampledCloud1, "downsampledCloud1");
	//viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "downsampledCloud1");  // 藍色

	//viewer.addPointCloud(downsampledCloud2, "downsampledCloud2");
	//viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "downsampledCloud2");  // 紅色

	//// 顯示坐標系
	//viewer.addCoordinateSystem(10, "cloud");
	/*viewer.addCoordinateSystem(10, "cloud");
	viewer.setCameraPosition(0, 0, -100, 0, 1, 0, 0, 0, -1);*/
	 
	 //初始化参数
	/*bool isShow = true;
	struct callback_args kb_args;
	kb_args.isShow = &isShow;
	kb_args.orgin_points = cloud1;*/


	/*viewer.spin();*/
	}
	else {
		std::cout << "沒有找到比對樣本文件夾。" << std::endl;
	}
	return 0;
}