#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 <vector>

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

#include <pcl/common/transforms.h>

//讀寫檔案
#include<iostream>
#include<fstream>

#include <iostream>  
#include <algorithm> 
#include <windows.h>
using namespace std;
#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;
}
int main(int argc, char** argv)
{
	string s = "D:\\ROBOT_Project\\Robot\\ABB\\RealSence_PathMove\\bin\\x64\\Debug\\";
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(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>);

	pcl::io::loadPCDFile(s+"allpoint.pcd", *cloud);

	//計算完整大底法向量
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> neOMP;
	neOMP.setNumberOfThreads(3);
	neOMP.setInputCloud(cloud);
	neOMP.setRadiusSearch(30);
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	neOMP.setViewPoint(centroid[0], centroid[1], centroid[2]);
	neOMP.compute(*Normals);

	//法向量與點雲結合
	pcl::concatenateFields(*cloud, *Normals, *cloud_and_normal);

	double e = 1;
	int jump = 1;
	int Startj = 0;
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudN(new pcl::PointCloud<pcl::PointXYZINormal>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr edgeR_BF(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile(s+"processpoint.pcd", *edgeR_BF);

	int count = 0, count1 = 0, ccc = 1000;

	//使用轉折處點雲與完整大底點雲進行匹配
	for (size_t i = 0; i < edgeR_BF->size(); i++)
	{
		bool flag = false;
		for (size_t j = Startj; j < cloud_and_normal->size(); j += jump)
		{
			int aa = cloud_and_normal->points[j].x - edgeR_BF->points[i].x;
			int bb = cloud_and_normal->points[j].y - edgeR_BF->points[i].y;
			int cc = cloud_and_normal->points[j].z - edgeR_BF->points[i].z;
			if (sqrt(aa * aa + bb * bb + cc * cc) < e)
			{
				count += 1;
				cloudN->push_back(cloud_and_normal->points[j]);
				//Startj = j;
				flag = true;
				e = 1;
				break;
			}
		}
		if (!flag)
		{
			e += 1;
			i--;
		}
	}
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr tempcloudNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
	tempcloudNormal = cloudN;

	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudNormalL0(new pcl::PointCloud<pcl::PointXYZINormal>);
	Eigen::Vector3f planeVec;
	Eigen::Vector3f origenVec;
	float dotResult, radian, n, a;
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudNormalL(new pcl::PointCloud<pcl::PointXYZINormal>);
	for (size_t i = 0; i < tempcloudNormal->size(); i++)
	{
		cout << tempcloudNormal->points[i].normal_x << endl;
		//將所有法向量轉換為同樣長度
		n = 1.5 / tempcloudNormal->points[i].normal_z;
		tempcloudNormal->points[i].normal_x *= n;
		tempcloudNormal->points[i].normal_y *= n;
		tempcloudNormal->points[i].normal_z *= n;
		cloudNormalL->push_back(tempcloudNormal->points[i]);
		cout << cloudNormalL->points[i].normal_x << endl;
	}
	for (size_t i = 0; i < cloudNormalL->size(); i++)
	{
		cout << cloudNormalL->points[i].x << "  " << cloudNormalL->points[i].y << "  " << cloudNormalL->points[i].z << endl;
	}
#pragma endregion

	pcl::io::savePCDFile(s+"edge_normal.pcd", *cloudNormalL);//寫入檔案
	pcl::visualization::PCLVisualizer viewer;
	viewer.addPointCloudNormals<pcl::PointXYZINormal>(cloudNormalL, 1, -50, "cloudNormalL");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloudNormalL");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "cloudNormalL");
	viewer.addPointCloud(cloud, "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 = cloud;

	/*struct callback_args cb_args;
	pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d(new pcl::PointCloud<pcl::PointXYZ>);
	cb_args.clicked_points_3d = clicked_points_3d;
	cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
	viewer.registerPointPickingCallback(pp_callback, (void*)&cb_args);
	pcl::console::print_highlight("Shift+click on three floor points, then press 'Q'...\n");*/

	viewer.spin();
	return 0;
}