// System includes
#include <stdio.h>
#include <assert.h>

// CUDA runtime
#include <cuda_runtime.h>
#include <nvrtc_helper.h>

// helper functions and utilities to work with CUDA
#include <helper_functions.h>

// ¾ØÕóÔËËã¿â
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>

// pcl
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.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/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/gp3.h>
#include <pcl/console/time.h>
#include <pcl/filters/random_sample.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/gpu/containers/initialization.h>
#include <pcl/gpu/octree/octree.hpp>
#include <pcl/gpu/features/features.hpp>
#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>

// custom
#include "kdTree.h"
//#include "kernel.h"

using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;

#define SourcePointSize_1 20000
#define SourcePointSize_2 200000

extern "C" bool get_distance(
	TreeElement *tree_array, 
	const int *size, 
	const coordinate *points, 
	const coordinate *normals, 
	coordinate *nearest, 
	double *distance);

extern "C" bool get_distance_icp(
	TreeElement *tree_array, 
	const int *size, 
    coordinate *points,
	const coordinate *normals, 
	coordinate *nearest,
	double *distance);

extern "C" bool rotate_points_host(
	coordinate *points,
	double *rotate_matrix);

extern "C" bool transfer_points_host(
	coordinate *points,
	coordinate *dev_transfer_vector);

bool next_iteration = true;

coordinate getBarycenter(coordinate *point,int size)
{
	coordinate barycenter;

	double sumX = 0;
	double sumY = 0;
	double sumZ = 0;

	for (int i = 0; i < size; i++)
	{
		sumX += point[i].x;
		sumY += point[i].y;
		sumZ += point[i].z;
	}

	barycenter.x = sumX / size;
	barycenter.y = sumY / size;
	barycenter.z = sumZ / size;

	return barycenter;
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
	void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

// icp
void step1()
{
	TreeNode *tree = new TreeNode;
	cudaError_t cudaStatus;

	coordinate sourceBarycenter;
	coordinate targetBarycenter;

#pragma region µãÔÆÔØÈë

	//===============================µãÔÆÔØÈë======================================//
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_trans(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>());

	pcl::io::loadPLYFile("D:\\test\\Mesh.ply", *cloud_in);
	pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target);

	pcl::PointXYZ min_target;
	pcl::PointXYZ max_target;

	pcl::PointXYZ min_in;
	pcl::PointXYZ max_in;

	pcl::getMinMax3D(*cloud_in, min_in, max_in);
	pcl::getMinMax3D(*cloud_target, min_target, max_target);

	
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_filter;

	//for (int i = 0; i < cloud_in->points.size(); i++)
	//{
	//	cloud_in->points[i].z *= 100;
	//}

	//radius_filter.setInputCloud(cloud_in);
	//radius_filter.setRadiusSearch(7);
	//radius_filter.setMinNeighborsInRadius(30); //ÉèÖòéѯµãµÄÁÚÓòµã¼¯ÊýСÓÚ2µÄɾ³ý
	//radius_filter.filter(*cloud_in);

	//for (int i = 0; i < cloud_in->points.size(); i++)
	//{
	//	cloud_in->points[i].z *= 0.01;
	//}

	//pcl::CropBox<pcl::PointXYZ> cropper;
	//Vector4f min_point;
	//Vector4f max_point;
	//min_point(0) = min_in.x + 0.5;// ((max_in.x - min_in.x) - (max_target.x - min_target.x)) / 2;
	//min_point(1) = min_in.y + 0.5;// ((max_in.y - min_in.y) - (max_target.y - min_target.y)) / 2;
	//min_point(2) = min_in.z;
	//min_point(3) = 1.0;
	//max_point(0) = max_in.x - 0.5;// ((max_in.x - min_in.x) - (max_target.x - min_target.x)) / 2;
	//max_point(1) = max_in.y - 0.5;// ((max_in.y - min_in.y) - (max_target.y - min_target.y)) / 2;
	//max_point(2) = max_in.z;
	//max_point(3) = 1.0;
	//cropper.setMin(min_point);
	//cropper.setMax(max_point);
	//cropper.setInputCloud(cloud_in);
	//cropper.filter(*cloud_in);

	pcl::RandomSample<pcl::PointXYZ> rs;
	rs.setSample(SourcePointSize_1);

	rs.setInputCloud(cloud_in);
	rs.filter(*cloud);

	rs.setInputCloud(cloud_target);
	rs.filter(*cloud_target);

#pragma endregion

#pragma region ·¨Ïß¹ÀËã

	//===============================·¨Ïß¹ÀËã====================================//
	pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud));
	pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
	pcl::gpu::Octree::PointCloud cloud_device;
	cloud_device.upload(cloud->points);
	normals_device.upload(normals_p->points);

	pcl::gpu::NormalEstimation normal_estimator;
	normal_estimator.setRadiusSearch(2.0, 50);
	normal_estimator.setInputCloud(cloud_device);
	normal_estimator.compute(normals_device);
	normals_device.download(normals_p->points);

	for (int i = 0; i < normals_p->size(); i++)
	{
		pcl::Normal normal;

		normal.normal_x = normals_p->points[i].x;
		normal.normal_y = normals_p->points[i].y;
		normal.normal_z = normals_p->points[i].z;

		source_normals->push_back(normal);
	}

	//=======================================================================//

#pragma endregion

	clock_t start, end;
	start = clock();

#pragma region kdTree½¨Á¢
	//==============================kdTree½¨Á¢===========================//

	coordinate *points_target = new coordinate[cloud_target->points.size()];

	for (int i = 0; i < cloud_target->points.size(); i++)
	{
		points_target[i].x = cloud_target->points[i].x;
		points_target[i].y = cloud_target->points[i].y;
		points_target[i].z = cloud_target->points[i].z;
	}

	tree = build_kdtree(points_target, cloud_target->points.size(), tree);



	int height = getHeight(tree);
	int arraySize = pow(2, height) - 1;

	TreeElement *tree_array = new TreeElement[arraySize];
	coordinate *points = new coordinate[SourcePointSize_1];
	coordinate *point_normals = new coordinate[SourcePointSize_1];
	coordinate *nearest = new coordinate[SourcePointSize_1];
	double *distance = new double[SourcePointSize_1];

	for (int i = 0; i < arraySize; i++)
	{
		tree_array[i].dom_elt = *(new coordinate);
		tree_array[i].split = 0;
		tree_array[i].isEmpty = true;
	}

	TreeToArray(tree, tree_array);

	for (int i = 0; i < SourcePointSize_1; i++)
	{
		points[i].x = cloud->points[i].x;
		points[i].y = cloud->points[i].y;
		points[i].z = cloud->points[i].z;

		point_normals[i].x = normals_p->points[i].x;
		point_normals[i].y = normals_p->points[i].y;
		point_normals[i].z = normals_p->points[i].z;
	}

	targetBarycenter = getBarycenter(points_target, cloud_target->points.size());
	sourceBarycenter = getBarycenter(points, cloud->points.size());

	delete[] points_target;

	coordinate delta;
	delta.x = targetBarycenter.x - sourceBarycenter.x;
	delta.y = targetBarycenter.y - sourceBarycenter.y;
	delta.z = targetBarycenter.z - sourceBarycenter.z;

#pragma endregion

#pragma region GPU×î½üÁÚ

	double rotate_matrix_array[9] = { 1,0,0,0,1,0,0,0,1 };
	Eigen::Matrix<double, 3, 3> matrix;
	Eigen::Matrix3d matrix_u;
	Eigen::Matrix3d matrix_s;
	Eigen::Matrix3d matrix_v;
	Eigen::Matrix4d matrix_temp = Matrix4d::Identity(4, 4);
	Eigen::Matrix4d matrix_final = Matrix4d::Identity(4, 4);

	Matrix<double, SourcePointSize_1, 3> source_m;
	Matrix<double, SourcePointSize_1, 3> target_m;

	double distance_sum;
	double distance_avg;

	TreeElement *dev_tree_array;
	coordinate *dev_points;
	coordinate *dev_point_normals;
	coordinate *dev_nearest;
	double *dev_rotate_matrix;
	coordinate *dev_transfer_vector;
	double *dev_distance;
	int *dev_arraySize;

	pcl::PointCloud<pcl::PointXYZ>::Ptr s_points(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr t_points(new pcl::PointCloud<pcl::PointXYZ>);

	for (int i = 0; i < SourcePointSize_1; i++)
	{
		pcl::PointXYZ point;
		point.x = 0;
		point.y = 0;
		point.z = 0;
		s_points->points.push_back(point);
		t_points->points.push_back(point);
	}

	/*pcl::visualization::PCLVisualizer viewer;
	viewer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_s_color_h(s_points, 20, 180, 20);
	viewer.addPointCloud<pcl::PointXYZ>(s_points, cloud_s_color_h, "s_cloud");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_t_color_h(t_points, 180, 180, 180);
	viewer.addPointCloud<pcl::PointXYZ>(t_points, cloud_t_color_h, "t_cloud");
	viewer.addCoordinateSystem(20);
	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);*/

	cudaMalloc((void**)&dev_tree_array, sizeof(TreeElement) * arraySize);
	cudaMalloc((void**)&dev_points, sizeof(coordinate) * SourcePointSize_1);
	cudaMalloc((void**)&dev_point_normals, sizeof(coordinate) * SourcePointSize_1);
	cudaMalloc((void**)&dev_rotate_matrix, sizeof(double) * 9);
	cudaMalloc((void**)&dev_transfer_vector, sizeof(coordinate));
	cudaMalloc((void**)&dev_nearest, sizeof(coordinate) * SourcePointSize_1);
	cudaMalloc((void**)&dev_distance, sizeof(double) * SourcePointSize_1);
	cudaMalloc((void**)&dev_arraySize, sizeof(int));

	cudaStatus = cudaMemcpy(dev_transfer_vector, &delta, sizeof(coordinate), cudaMemcpyHostToDevice);
	cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
	transfer_points_host(dev_points, dev_transfer_vector);
	cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);

	matrix_temp(0, 3) = delta.x;
	matrix_temp(1, 3) = delta.y;
	matrix_temp(2, 3) = delta.z;
	matrix_final = matrix_final * matrix_temp;

	for (int i = 0; i < 20; i++)
	{
		//viewer.spinOnce();

		if (next_iteration)
		{


#pragma region ×î½üÁÚ

			cudaStatus = cudaMemcpy(dev_tree_array, tree_array, sizeof(TreeElement) * arraySize, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_point_normals, point_normals, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_nearest, nearest, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_distance, distance, sizeof(double) * SourcePointSize_1, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_arraySize, &arraySize, sizeof(int), cudaMemcpyHostToDevice);

			get_distance_icp(
				dev_tree_array,
				dev_arraySize,
				dev_points,
				dev_point_normals,
				dev_nearest,
				dev_distance);

			cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
			cudaMemcpy(nearest, dev_nearest, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
			cudaMemcpy(distance, dev_distance, sizeof(double) * SourcePointSize_1, cudaMemcpyDeviceToHost);

#pragma endregion

#pragma region ¼ÆËãÐýת¾ØÕó

			distance_sum = 0;

			//sourceBarycenter = getBarycenter(points, SourcePointSize_1);
			//targetBarycenter = getBarycenter(nearest, SourcePointSize_1);

			/*int size = SourcePointSize_1;
			int threshold_index = (int)(size*0.1);
			double *distance_temp = new double[SourcePointSize_1];
			for (int i = 0; i < SourcePointSize_1; i++)
			{
				distance_temp[i] = distance[i] * distance[i];
			}
			std::nth_element(distance_temp, distance_temp + threshold_index, distance_temp + SourcePointSize_1);
			double threshold = sqrt(distance_temp[threshold_index - 1]);
			delete[] distance_temp;*/

			matrix = Eigen::Matrix3d::Zero();
			for (int i = 0; i < SourcePointSize_1; i++)
			{
				distance_sum += distance[i] * distance[i];

				/*matrix+= Eigen::Vector3d(points[i].x, points[i].y, points[i].z) * 
					Eigen::Vector3d(nearest[i].x, nearest[i].y, nearest[i].z).transpose();*/

				source_m(i, 0) = points[i].x - sourceBarycenter.x;
				source_m(i, 1) = points[i].y - sourceBarycenter.y;
				source_m(i, 2) = points[i].z - sourceBarycenter.z;
				target_m(i, 0) = nearest[i].x - targetBarycenter.x;
				target_m(i, 1) = nearest[i].y - targetBarycenter.y;
				target_m(i, 2) = nearest[i].z - targetBarycenter.z;

			}
			distance_avg = distance_sum / SourcePointSize_1;

			printf("Distance square sum average:%f\n", distance_avg);

			matrix = source_m.transpose() * target_m;
			Eigen::JacobiSVD<Eigen::Matrix3d> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
			matrix_u = svd.matrixU();
			matrix_v = svd.matrixV();
			Matrix3d rotate = matrix_v * matrix_u.transpose();

			if (rotate.determinant() < 0)
			{
				matrix_v(2, 0) = -matrix_v(2, 0);
				matrix_v(2, 1) = -matrix_v(2, 1);
				matrix_v(2, 2) = -matrix_v(2, 2);

				Matrix3d rotate = matrix_v * matrix_u.transpose();
			}

			cout << "Rotate=\n" << rotate << endl;

			rotate_matrix_array[0] = rotate(0, 0);
			rotate_matrix_array[1] = rotate(0, 1);
			rotate_matrix_array[2] = rotate(0, 2);

			rotate_matrix_array[3] = rotate(1, 0);
			rotate_matrix_array[4] = rotate(1, 1);
			rotate_matrix_array[5] = rotate(1, 2);

			rotate_matrix_array[6] = rotate(2, 0);
			rotate_matrix_array[7] = rotate(2, 1);
			rotate_matrix_array[8] = rotate(2, 2);

			cudaStatus = cudaMemcpy(dev_rotate_matrix, rotate_matrix_array, sizeof(double) * 9, cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
			rotate_points_host(dev_points, dev_rotate_matrix);
			cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);


#pragma endregion

#pragma region ¼ÆËãƽÒƾØÕó

			sourceBarycenter = getBarycenter(points, SourcePointSize_1);
			//targetBarycenter = getBarycenter(nearest, SourcePointSize_1);

			delta.x = targetBarycenter.x - sourceBarycenter.x;
			delta.y = targetBarycenter.y - sourceBarycenter.y;
			delta.z = targetBarycenter.z - sourceBarycenter.z;

			printf("transfer=[%f,%f,%f]\n", delta.x, delta.y, delta.z);

			cudaStatus = cudaMemcpy(dev_transfer_vector, &delta, sizeof(coordinate), cudaMemcpyHostToDevice);
			cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
			transfer_points_host(dev_points, dev_transfer_vector);
			cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);

#pragma endregion

			matrix_temp(0, 0) = rotate(0, 0);
			matrix_temp(0, 1) = rotate(0, 1);
			matrix_temp(0, 2) = rotate(0, 2);
			matrix_temp(1, 0) = rotate(1, 0);
			matrix_temp(1, 1) = rotate(1, 1);
			matrix_temp(1, 2) = rotate(1, 2);
			matrix_temp(2, 0) = rotate(2, 0);
			matrix_temp(2, 1) = rotate(2, 1);
			matrix_temp(2, 2) = rotate(2, 2);
			matrix_temp(0, 3) = delta.x;
			matrix_temp(1, 3) = delta.y;
			matrix_temp(2, 3) = delta.z;

			matrix_final = matrix_final * matrix_temp;

			/*for (int index = 0; index < SourcePointSize; index++)
			{
				t_points->points[index].x = nearest[index].x;
				t_points->points[index].y = nearest[index].y;
				t_points->points[index].z = nearest[index].z;
				s_points->points[index].x = points[index].x;
				s_points->points[index].y = points[index].y;
				s_points->points[index].z = points[index].z;
			}
			viewer.updatePointCloud(s_points, cloud_s_color_h, "s_cloud");
			viewer.updatePointCloud(t_points, cloud_t_color_h, "t_cloud");*/

			//next_iteration = false;
		}
	}

#pragma endregion

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setMaximumIterations(20);
	pcl::transformPointCloud(*cloud, *cloud, matrix_final);
	icp.setInputSource(cloud);
	icp.setInputTarget(cloud_target);
	icp.align(*cloud);

	if (icp.hasConverged())
	{
		matrix_final = icp.getFinalTransformation().cast<double>() * matrix_final;
		icp.setMaximumIterations(1);
		icp.align(*cloud);
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
	}

	end = clock();
	double costTime = (double)(end - start) / CLOCKS_PER_SEC;
	printf("Use Time:%f\n", costTime);


	delete[] tree_array;
	delete[] points;
	delete[] point_normals;
	delete[] nearest;
	delete[] distance;

	cudaFree(dev_tree_array);
	cudaFree(dev_points);
	cudaFree(dev_point_normals);
	cudaFree(dev_rotate_matrix);
	cudaFree(dev_transfer_vector);
	cudaFree(dev_nearest);
	cudaFree(dev_distance);
	cudaFree(dev_arraySize);

	pcl::transformPointCloud(*cloud_in, *cloud_trans, matrix_final);

	pcl::io::savePLYFile("D:\\test\\mesh_trans_gpu_2.ply", *cloud_trans);
}

// ¼ÆËãµãÔƵ½±ê×¼µãÔƵľàÀë
void step2()
{
	TreeNode *tree = new TreeNode;
	cudaError_t cudaStatus;

	#pragma region µãÔÆÔØÈë

	//===============================µãÔÆÔØÈë======================================//
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>());

		pcl::io::loadPLYFile("D:\\test\\mesh_trans_gpu_1.ply", *cloud);
		pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target);

		pcl::RandomSample<pcl::PointXYZ> rs;
		rs.setSample(SourcePointSize_2);

		rs.setInputCloud(cloud);
		rs.filter(*cloud);

		/*rs.setInputCloud(cloud_target);
		rs.filter(*cloud_target);*/

	#pragma endregion

		clock_t start, end;
		start = clock();

	#pragma region ·¨Ïß¹ÀËã

	//===============================·¨Ïß¹ÀËã====================================//
		pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud));
		pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
		pcl::gpu::Octree::PointCloud cloud_device;
		cloud_device.upload(cloud->points);
		normals_device.upload(normals_p->points);

		pcl::gpu::NormalEstimation normal_estimator;
		normal_estimator.setRadiusSearch(2.0, 50);
		normal_estimator.setInputCloud(cloud_device);
		normal_estimator.compute(normals_device);
		normals_device.download(normals_p->points);

		for (int i = 0; i < normals_p->size(); i++)
		{
			pcl::Normal normal;

			normal.normal_x = normals_p->points[i].x;
			normal.normal_y = normals_p->points[i].y;
			normal.normal_z = normals_p->points[i].z;

			source_normals->push_back(normal);
		}

		//=======================================================================//

	#pragma endregion

	#pragma region kdTree½¨Á¢
	//===============================kdTree½¨Á¢===============================//
		coordinate *points_target = new coordinate[cloud_target->points.size()];

		for (int i = 0; i < cloud_target->points.size(); i++)
		{
			points_target[i].x = cloud_target->points[i].x;
			points_target[i].y = cloud_target->points[i].y;
			points_target[i].z = cloud_target->points[i].z;
		}

		tree = build_kdtree(points_target, cloud_target->points.size(), tree);

		delete[] points_target;

		//====================================================================//
	#pragma endregion

	#pragma region ת»»³ÉÊý×é
	//==============================ת»»³ÉÊý×é===========================//

		int height = getHeight(tree);
		int arraySize = pow(2, height) - 1;

		TreeElement *tree_array = new TreeElement[arraySize];
		coordinate *points = new coordinate[SourcePointSize_2];
		coordinate *point_normals = new coordinate[SourcePointSize_2];
		coordinate *nearest = new coordinate[SourcePointSize_2];
		double *distance = new double[SourcePointSize_2];

		for (int i = 0; i < arraySize; i++)
		{
			tree_array[i].dom_elt = *(new coordinate);
			tree_array[i].split = 0;
			tree_array[i].isEmpty = true;
		}

		TreeToArray(tree, tree_array);

		for (int i = 0; i < SourcePointSize_2; i++)
		{
			points[i].x = cloud->points[i].x;
			points[i].y = cloud->points[i].y;
			points[i].z = cloud->points[i].z;

			point_normals[i].x = normals_p->points[i].x;
			point_normals[i].y = normals_p->points[i].y;
			point_normals[i].z = normals_p->points[i].z;
		}
	#pragma endregion

	#pragma region CPU×î½üÁÚ
		//==================================================================//
		//clock_t start, end;
		//start = clock();
		//coordinate target;
		//ofstream outFile;
		//outFile.open("D:\\data2.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
		//for (int i = 0; i < cloud->points.size(); i++)
		//{
		//	target.x = cloud->points[i].x;
		//	target.y = cloud->points[i].y;
		//	target.z = cloud->points[i].z;
		//	searchNearest(tree_array, cloud_target->points.size(), target, *nearest, *distance);
		//	float delta_x = target.x - nearest->x;
		//	float delta_y = target.y - nearest->y;
		//	float delta_z = target.z - nearest->z;
		//	*distance =
		//		source_normals->points[i].normal_x * delta_x +
		//		source_normals->points[i].normal_y * delta_y +
		//		source_normals->points[i].normal_z * delta_z;
		//	outFile << target.x << ',' << target.y << ',' << target.z << "," << distance << endl;
		//}
		//outFile.close();
		//end = clock();
		//double costTime = (double)(end - start) / CLOCKS_PER_SEC;
		//printf("Use Time:%f\n", costTime);
	#pragma endregion

	#pragma region GPU×î½üÁÚ

		TreeElement *dev_tree_array;
		coordinate *dev_points;
		coordinate *dev_point_normals;
		coordinate *dev_nearest;
		double *dev_distance;
		int *dev_arraySize;

		cudaMalloc((void**)&dev_tree_array, sizeof(TreeElement) * arraySize);
		cudaMalloc((void**)&dev_points, sizeof(coordinate) * SourcePointSize_2);
		cudaMalloc((void**)&dev_point_normals, sizeof(coordinate) * SourcePointSize_2);
		cudaMalloc((void**)&dev_nearest, sizeof(coordinate) * SourcePointSize_2);
		cudaMalloc((void**)&dev_distance, sizeof(double) * SourcePointSize_2);
		cudaMalloc((void**)&dev_arraySize, sizeof(int));

		cudaStatus = cudaMemcpy(dev_tree_array, tree_array, sizeof(TreeElement) * arraySize, cudaMemcpyHostToDevice);
		cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
		cudaStatus = cudaMemcpy(dev_point_normals, point_normals, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
		cudaStatus = cudaMemcpy(dev_nearest, nearest, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
		cudaStatus = cudaMemcpy(dev_distance, distance, sizeof(double) * SourcePointSize_2, cudaMemcpyHostToDevice);
		cudaStatus = cudaMemcpy(dev_arraySize, &arraySize, sizeof(int), cudaMemcpyHostToDevice);

		get_distance(dev_tree_array, dev_arraySize, dev_points, dev_point_normals, dev_nearest, dev_distance);

		cudaMemcpy(nearest, dev_nearest, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyDeviceToHost);
		cudaMemcpy(distance, dev_distance, sizeof(double) * SourcePointSize_2, cudaMemcpyDeviceToHost);

	#pragma endregion

		end = clock();
		double costTime = (double)(end - start) / CLOCKS_PER_SEC;
		printf("Use Time:%f\n", costTime);

	#pragma region Êý¾Ý±£´æ¼°Õ¹Ê¾

		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZI>);

		for (int i = 0; i < SourcePointSize_2; i++)
		{
			pcl::PointXYZI point;

			point.x = points[i].x;
			point.y = points[i].y;
			//point.z = points[i].z;
			point.z = distance[i] * 100;
			point.intensity = distance[i];

			cloud_result->push_back(point);

		}


		pcl::visualization::PCLVisualizer viewer;
		viewer.setBackgroundColor(0, 0, 0);
		pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud_result, "intensity");//°´ÕÕintensity×ֶνøÐÐäÖȾ
		viewer.addPointCloud<pcl::PointXYZI>(cloud_result, fildColor, "s_cloud");
		viewer.addCoordinateSystem(20);

		while (!viewer.wasStopped())
		{
			viewer.spinOnce();
		}

		ofstream outFile;
		outFile.open("D:\\data4.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
		for (int i = 0; i < SourcePointSize_2; i++)
		{
			outFile << points[i].x << ','
				<< points[i].y << ','
				<< points[i].z << ","
				<< nearest[i].x << ","
				<< nearest[i].y << ","
				<< nearest[i].z << ","
				<< distance[i] << endl;
		}
		outFile.close();

	#pragma endregion

	delete[] tree_array;
	delete[] points;
	delete[] point_normals;
	delete[] nearest;
	delete[] distance;

	cudaFree(dev_tree_array);
	cudaFree(dev_points);
	cudaFree(dev_point_normals);
	cudaFree(dev_nearest);
	cudaFree(dev_distance);
}

int main(int argc, char **argv)
{
	//step1();
	
	step2();
	
	return 0;
}