//#include <sstream>
//#include <iostream>
#include <fstream>
//#include <ctime> //This doesn't seem to be needed for running this code

#include "OctreeTestCode.h"


using namespace std;

// Global Parameters
int 	_loadPCD 				= 0; 	//=1 if we have a pcd file to load instead of .txt file
int 	_savePCD 				= 0; 	//=1 if we want to save a pcd file
int 	_normalizeMap 			= 0; 	//=1 if we want to anchor the map (x,y) to (0,0)
int 	_saveVTK 				= 0; 	//=1 if we want to save a VTK file with the mesh
int 	_buildMesh	 			= 0; 	//=1 if we want to build the mesh
int 	_loadMesh 				= 0; 	//=1 if we want to load the mesh from VTK file
int 	_display				= 0; 	//=1 if we want to display the ray trace on the octree map (with normals)
int 	_displayMesh 			= 0; 	//=1 if we want to display the mesh (with normals)

float 	deg2rad 				= M_PI/180.f;


void octreeTest (void)
{
	/*/////////////////////////////////////////////////////
	 * POINT CLOUD GENERATION  --> Read in data
	 */
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

	if (!_loadPCD) 	/* Load from .txt */
	{
		// Read XYZ points from file
		//string inputFilename = "ReducedMap.txt";
		//string inputFilename = "/home/shouts/camelot/Projects/mbari/mbariMaps/PortugueseLedge/PortugueseLedge3dPointsDEM.txt";
		//string inputFilename = "/home/shouts/camelot/Projects/mbari/mbariMaps/PortugueseLedge/PortLedgeLongShort.txt";
		string inputFilename = "/home/shouts/camelot/Projects/mbari/mbariMaps/NearshoreWall/NearshoreWall3dPointsNEDUTM.txt";

		ifstream inputFile (inputFilename.c_str());
		string line;

		vector<float> xVec, yVec, zVec;
		float x, y, z;
		float xmin, ymin, zmin;
		printf("octreeTest::found input file, reading in data\n");
		// Read input data
		if (inputFile.is_open()){
			while ( inputFile.good() ){

				getline (inputFile,line);
				stringstream sline;
				sline<<line;
				sline >> x  >> y >> z;

				xVec.push_back(x);
				yVec.push_back(y);
				zVec.push_back(z);
			}
		}

		int NumPoints = xVec.size(); 	// Number of points in map

		// Set cloud properties
		cloud->is_dense 	= false;
		cloud->height 		= 1;
		cloud->width 		= NumPoints;
		cloud->points.resize (cloud->width * cloud->height);

		if (_normalizeMap)
		{
			/* /////////////////////////////////////////////////////////////
			 * Normalize DEM coordinates such that it contains (x,y) = (0,0)
			 */

			xmin = 2*xVec[0];
			ymin = 2*yVec[0];
			zmin = 2*zVec[0];

			// Find min values
			for (int ii=0; ii<NumPoints; ii++)
			{
				x = xVec[ii];
				y = yVec[ii];
				z = zVec[ii];

				if (x < xmin) { xmin = x; }
				if (y < ymin) { ymin = y; }
				if (z < zmin) { zmin = z; }
			}

			for (int ii=0; ii<NumPoints; ii++)
			{
				cloud->points[ii].x = xVec[ii] - xmin;
				cloud->points[ii].y = yVec[ii] - ymin;
				cloud->points[ii].z = zVec[ii] - zmin;
			}
			/* /////////////////////////////////////
			 * End Map Normalization
			 */
		}
		else
		{
			for (int ii=0; ii<NumPoints; ii++)
			{
				cloud->points[ii].x = xVec[ii];
				cloud->points[ii].y = yVec[ii];
				cloud->points[ii].z = zVec[ii];
			}
		}

		if (_savePCD)
		{
			// Save PCD file
			pcl::io::savePCDFileASCII("points.pcd", *cloud);
		}

	} else 	/* Load data from PCD file */
	{
		// Load input file into a PointCloud<T> with an appropriate type
		sensor_msgs::PointCloud2 cloud_blob;
		pcl::io::loadPCDFile ("points.pcd", cloud_blob);

		// make the data available in cloud
		pcl::fromROSMsg (cloud_blob, *cloud);
	}

	/*//////////////////////////////////////////////////
	 * end POINT CLOUD GENERATION
	 */



	/*//////////////////////////////////////////////////
	 * NORMAL ESTIMATION
	 */
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud (cloud);
	n.setInputCloud (cloud);
	n.setSearchMethod (tree); 	// Set Method
	n.setKSearch (6); 			// Set the k-neighbors to use
	n.compute (*normals); 		// Compute the normals

	// Force terrain normals to point upward (-Z)
	for (int nn=0; nn<normals->points.size(); nn++) {
		if (normals->points[nn].normal_z > 0){
			normals->points[nn].normal_x = -normals->points[nn].normal_x;
			normals->points[nn].normal_y = -normals->points[nn].normal_y;
			normals->points[nn].normal_z = -normals->points[nn].normal_z;
		}
	}

	// Concatenate the XYZ and normal fields
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

	/*///////////////////////////////////////////////////
	 * end NORMAL ESTIMATION
	 */



	/*///////////////////////////////////////////////////
	 * TRIANGLE MESH GENERATION
	 */
	// Create search tree
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud (cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh triangles;

	if (_buildMesh)
	{
		// BUILD MESH

		// Set the maximum distance between connected points (maximum edge length)
		gp3.setSearchRadius (3.0);

		// Set typical values for the parameters
		gp3.setMu (1.7);
		gp3.setMaximumNearestNeighbors (100);
		gp3.setMaximumSurfaceAngle(M_PI); // 180 degrees
		gp3.setMinimumAngle(M_PI/72); // 5 degrees
		gp3.setMaximumAngle(170*deg2rad); // 170 degrees
		gp3.setNormalConsistency(false);

		// Get result
		gp3.setInputCloud (cloud_with_normals);
		gp3.setSearchMethod (tree2);
		gp3.reconstruct (triangles);

		// Additional vertex information
		std::vector<int> parts = gp3.getPartIDs();
		std::vector<int> states = gp3.getPointStates();

		if (_saveVTK)
		{
			// SAVE VTK FILE
			pcl::io::saveVTKFile("mesh.vtk", triangles, 10);
		}

	} else if (_loadMesh)
	{
		// Load triangle mesh from VTK file
		pcl::io::loadPolygonFileVTK( "mesh.vtk", triangles );
	}

	/*/////////////////////////////////////////////////////
	 * end TRIANGLE MESH GENERATION
	 */


	/*/////////////////////////////////////////////////////
	 * OCTREE GENERATION
	 */
	float 	resolution 	= 4.0f; 	// Octree resolution

	printf("about to build octree\n");
	// Build Octree
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud (cloud);  		// octree.setInputCloud(octree_with_normals) if you want normals as well
	octree.addPointsFromInputCloud ();
	/*//////////////////////////////////////////////////////
	 * end OCTREE GENERATION
	 */


	/*//////////////////////////////////////////////////////
	 * SIMULATE VEHICLE SENSING
	 */
	float Altitude 			= 15.f; 			// METERS

	//define an origin and direction of vehicle side scan rays
	Eigen::Vector3f sensorOrigin, direction; 		// startPoint=sensor origin, direction=beam direction
	Eigen::Vector3f downVector; 				// global down vector (toward center of earth)
	downVector << 0., 0., 1.;

	/* Generate the Sensor Origin Position:
	 * --> Pick middle map point of cloud
	 * --> Sensor Origin is Altitude above this middle map point
	 */
	int cell = floor((float)cloud->points.size()/2);
	printf("octreeTest:: cell = %d\n", cell);
	sensorOrigin << cloud->points[cell].x, cloud->points[cell].y, cloud->points[cell].z - Altitude;

	// Set Beam Direction: For now it is set to straight down
	direction = downVector;

	// Allocate
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::AlignedPointTVector alignedPoints;
	std::vector<int> intersectIndices;
	pcl::Normal terrainNormal;
	int 	ret 		= 0; 		// return variable (dummy)

	// ****** CALCULATE VOXEL INTERSECTION ********
	ret = octree.getIntersectedVoxelCenters(sensorOrigin, direction, alignedPoints);
	printf("octreeTest:: ret = %d\n",ret);
	printf("octreeTest:: sensorOrigin = %f, %f, %f\n", sensorOrigin[0], sensorOrigin[1], sensorOrigin[2]);
	if(ret>0) printf("octreeTest:: alignedPoints = %f, %f, %f\n", alignedPoints[0].x, alignedPoints[0].y, alignedPoints[0].z);
	printf("pointscellsurroundingsx = %f, %f, %f\n", cloud->points[cell+1].x, cloud->points[cell].x, cloud->points[cell-1].x);
	printf("pointscellsurroundingsy = %f, %f, %f\n", cloud->points[cell+1].y, cloud->points[cell].y, cloud->points[cell-1].y);
	/*///////////////////////////////////////////////////////////////
	 * end SIMULATE VEHICLE SENSING
	 */


	/*////////////////////////////////////////////////////////////////
	 * DISPLAY
	 */
	if (_display)
	{
		/*///////////////////////////////////////////////////////////////
		 * DISPLAY OCTREE
		 */
		pcl::visualization::PCLVisualizer *cloud_viewer = new pcl::visualization::PCLVisualizer("3D Viewer");
		cloud_viewer->addPointCloud(cloud, "Cloud", 0);
		cloud_viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 1, 0.5, "normals");

		// Display candy
		cloud_viewer->addCoordinateSystem(100.0, 0);
		cloud_viewer->setCameraPose((double)(sensorOrigin[0]+90.), (double)(sensorOrigin[1]+90.), (double)(sensorOrigin[2]), 1., 0.,0., 1., 1., -0.1);

		// Draw ray trace line
		for (int ii=0; ii < alignedPoints.size(); ii++)
		{
			std::string lineID;
			std::string tmp = "line";
			char numstr[21]; // enough to hold all numbers up to 64-bits
			sprintf(numstr, "%d", ii);
			lineID = tmp + numstr;

			// Intersected voxel point
			pcl::PointXYZ destPt = alignedPoints[ii];

			// Sensor Origin cast into PointXYZ format
			pcl::PointXYZ sensorPoint = pcl::PointXYZ(sensorOrigin[0], sensorOrigin[1], sensorOrigin[2]);

			// Draw the line from sensor to voxel
			cloud_viewer->addLine<pcl::PointXYZ>(sensorPoint, destPt, 255., 0., 0., lineID);
		}
		cloud_viewer->spin();
	}

	/*/////////////////////////////////////////////////////
	 * end DISPLAY OCTREE
	 */

	/*/////////////////////////////////////////////////////
	 * DISPLAY MESH
	 */
	if (_displayMesh)
	{
		// Display Mesh
		pcl::visualization::PCLVisualizer * mesh_viewer_ = new pcl::visualization::PCLVisualizer("Mesh View");
		mesh_viewer_->addPolygonMesh(triangles, "mesh");
		mesh_viewer_->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetProperty()->SetInterpolationToPhong();
		mesh_viewer_->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 1, 0.5, "normals");
		mesh_viewer_->setCameraPose((double)sensorOrigin[0], (double)sensorOrigin[1], (double)sensorOrigin[2], 0., 0., 1., 1., 0., 0., 0);

		// Loop mesh viewer
		while (!mesh_viewer_->wasStopped ())
		{
			mesh_viewer_->spinOnce (100);
			sleep(1);
		}
	}
	/*//////////////////////////////////////////////////////////
	 * end DISPLAY MESH
	 */

	// Finish
	//return (0);
}