#include "Steger.hpp"
#include <cuda.h>
#include <cuda_runtime.h>

#include <pcl/common/common.h>
#include <pcl/filters//voxel_grid.h>
//#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>



bool init_cuda()
{
	static bool bInit = false;
	if (bInit)
		return true;
	int count;

	cudaGetDeviceCount(&count);
	if (count == 0) {
		fprintf(stderr, "There is no device./n");
		return false;
	}

	int i;
	for (i = 0; i < count; i++) {
		cudaDeviceProp prop;
		if (cudaGetDeviceProperties(&prop, i) == cudaSuccess) {
			if (prop.major >= 1) {
				break;
			}
		}
	}

	if (i == count) {
		fprintf(stderr, "There is no device supporting CUDA 1.x./n");
		return false;
	}

	cudaSetDevice(i);
	bInit = true;
	printf("CUDA Init success...........\n");
	return true;
}

int main()
{
	if (!init_cuda())
	{
		return 0;
	}
	
	cv::Mat src = cv::imread("D:\\code\\steger\\map.bmp", 0);
	cv::Mat image=src.clone();
	for (int i = 0; i < image.rows; i++)
	{
		for (int j = 0; j < image.cols; j++)
		{
			if (image.at<uchar>(i, j) < 10)
				image.at<uchar>(i, j) = 0;
		}
	}


	cv::Mat fimage, fout;
	image.convertTo(fimage, CV_32FC1);
	fout = cv::Mat::zeros(fimage.size(), CV_32FC1);
	Steger steger(3.1, 0.5, 3.5);
	// LARGE_INTEGER freq, t0, t1;
	// QueryPerformanceFrequency(&freq);

	// QueryPerformanceCounter(&t0);
	std::vector<cv::Point2f> points;
	steger.detect(fimage,points);

	// QueryPerformanceCounter(&t1);

	// printf("time=%lf\n", (((t1.QuadPart - t0.QuadPart) * 1000.0) / freq.QuadPart));
	float resolution = 0.01;
	pcl::PointCloud<pcl::PointXYZ> cloud;
	for (int i = 0; i < points.size(); ++i)
	{
		pcl::PointXYZ point((points[i].x-fimage.cols/2)*resolution, 
			(points[i].y-fimage.rows/2)*resolution, 0.0);
		cloud.push_back(point);
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_upground_downsample(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
	vgfilter.setInputCloud(cloud.makeShared());
	vgfilter.setLeafSize(0.01f, 0.01f, 0.01f);
	vgfilter.filter(*cloud_upground_downsample);

	pcl::PCDWriter writer;
	writer.write("./map.pcd", *cloud_upground_downsample);

	getchar();
}