/** @file
 *  @brief GPU-ICP Algorithm
 *  @author Deyuan Qiu, University of Applied Sciences Bonn-Rhein-Sieg, Sankt Augustin, Germany.
 *                      Fraunhofer IAIS, Sankt Augustin, Germany.
 */

#ifndef CICPGPUCUDA_H
#define CICPGPUCUDA_H

#include <cstdlib>						// C standard library
#include <cstdio>						// C I/O (for sscanf)
#include <cstring>						// string manipulation
#include <fstream>						// file I/O
#include <algorithm>					// min()
#include <time.h>

#include "ANN/ANN.h"					// ANN declarations
#include "ANN/ANNperf.h"				// kd-tree printing
#include "kd_tree.h"		  		// ANN node declaration

#include "newmat/newmat.h"
#include "newmat/newmatap.h"
using namespace NEWMAT;

#include "slam6d/cuda/CSystem.h"

/*
 * The maximum block size. For nVidia G80 architecture, 192 is suggested.
 */
//#define	BLOCKSIZE	512
//#define	BLOCKSIZE	256
#define	BLOCKSIZE	64
//#define	BLOCKSIZE	1


/*
 * The big enough size of the AoS that is going to allocate for the kd-tree.
 */
//#define	TREESIZE	262143
#define	TREESIZE  524288

#ifdef use_namespace
using namespace std;
#endif

/*
*	@class	CIcpGpuCuda
* 	@brief	Iterative Closest Point algorithm is implemented on a programmable graphic
* device. Kernels are implemented by CUDA (Compute Unified Device Architecture) GPGPU
* programming language (http://www.nvidia.com/object/cuda_home.html). To compile the code,
* nvcc compiler and related CUDA libraries must be installed. Kernel files are wrapped in
* CIcpGpuCuda_kernel.cuh and CIcpGpuCuda.cu. Attention: only NVidia GeForce G80 architecture
* and onwards graphic devices are garanteed to be supported.
* 	@author	Deyuan Qiu
* 	@date	2008.Nov.
*/
class CIcpGpuCuda{

public:

	/*
	*	standard constructor
	* @param	argc	passed from application main function.
	* @param	argv	passed from application main function.
	* @param	unWidth		the width of point cloud image.
	* @param	unHeight	the height of pint cloud image.
	*/
	CIcpGpuCuda(unsigned unWidth, unsigned unHeight, unsigned max_iter){
		init(unWidth, unHeight,max_iter);
	}

	/*
	*	standard destructor
	*/
	~CIcpGpuCuda();

	/*
	*	Set maximum iteration for ICP.
	* @param	unTimes	number of times ICP iterates maximally.
	*/
	void setMaxIteration(unsigned unTimes);

	/*
	*	Set maximum processing time for ICP, in milliseconds.
	* @param	dMilliseconds	ICP quits when elapsed time exceeds.
	*/
	void setMaxProcTime(double dMilliseconds);

	/*
	*	Set maximum deviation for ICP.
	* @param	dDeviation	ICP quits when specified deviation is achieved.
	*/
	void setMaxDeviation(double dDeviation);

	/*
	*	Set search radiuses for ICP. An iterative decreasing radius
	* is applied. Search radius decreases linearly from fRadiusMax to
	* fRadiusMin within unIterations iterations.
	* @param	fRadiusMax	Initial radius.
	* @param	fRadiusMin	Final radius.
	* @param	unIterations	Number of iterations, in which radius decreases.
	*/
	void setSearchRadius(float fRadiusMax, float fRadiusMin, unsigned unIterations);

	/*
	*	Get the number of points in point cloud. Model point cloud
	* and scene point cloud must have the same number of points.
	* @return	Number of points in point cloud.
	*/
	unsigned getSize(void);

	/*
	*	Get the the 2 dimensional pointer to the scene point cloud. Page-lock memory is
	* allocated and freed by the class. The data type must be casted to single pricision
	* float, and the array should be loaded as [3][N].
	* @return	2 dimensional pointer to scene point cloud.
	*/
	float** getScenePointer(void);

	/*
	*	Get the the 2 dimensional pointer to the model point cloud. Memory allocation
	* and freeing is handled by the class. The data type must be casted to double pricision
	* float, and the array should be loaded as [N][3]. Notice the difference to
	* getScenePointer().
	* @return	2 dimensional pointer to scene point cloud.
	*/
	double** getModelPointer(void);

	/*
	*	The method is called after point clouds are loaded, and before iteration() is called.
	*/
	void setPointClouds(void);

	/*
	*	ICP iterations.
	*/
	void iteration(void);

	/*
	*	Get the transformation matrix.
	* @return	The transformation matrix.
	*/
	Matrix* getMatrix(void);

	/**
     * These two functions are to set and get the tree pointer
     *  It is supposed to be created in scan file and passed to this class
	**/

	void setTreePointer(ANNkd_tree *);
     void getTreePointer(ANNkd_tree *&);

	double getTime(void);

        void setMinimums(float x, float y, float z);
        Matrix** getMatrices();

        void setTrans_Trans_inv(const double[], const double[]);

        void findNearestNeighbors(float, unsigned);

        Matrix fillHomoMatrix(Matrix* , double*);

        void computeCentroid(float*, float*, float*, float *&);
        
        Matrix computeHMatrix();

        void printMatrix(Matrix *);

     void setSize(unsigned int width, unsigned int height);	
	   
private:
	
	////////////////
	//cpu variables
	////////////////


	/*
	*	Initialization. Memories are allocated and default environmental state
	* is set. Called by standard constructor.
	* @param	unWidth		the width of point cloud image.
	* @param	unHeight	the height of pint cloud image.
	*/
	void init(unsigned unWidth, unsigned unHeight, unsigned max_iter);

	/*
	*	Initialization. Memories are allocated and default environmental state
	* is set. Called by init().
	* @param	unWidth		the width of point cloud image.
	* @param	unHeight	the height of pint cloud image.
	*/
	void setResolution(unsigned unWidth, unsigned unHeight);

	/*
	*	The internal search structure is set.
	*/
	void setTree(void);

	/*
	*	Set the model point cloud.
	*/
	void setModel(void);

	/*
	*	Set the scene point cloud.
	*/
	void setScene(void);

	/*
	*	Calculate the size of the search structure from its depth. The depth
	* is the number of levels of the tree. The search structure then is arraged
	* into a struture of arrays (SoA). A left-balanced binary tree is suggested.
	* @param	nDepth	Depth of the kd-tree.
	*/
	inline int depth2size(int nDepth);

	/*
	*	Rearrange the search structure into a structure of arrays (SoA). The rule
	* is: the N node's left child has the index of 2N, while the right child 2N+1.
	* Arrangement is fulfilled in recursion.
	* @param	root	pointer to the root node
	* @param	unStart	the currenet node
	*/
	void rearrange(ANNkd_ptr root, unsigned unStart);

	/*
	*	Get CUBLAS errors before this line.
	*/
	void getCublasErr(void);

	/*
	*	Get CUDA errors before this line.
	*/
	void getCudaErr(void);

	/*
	 * tree: structure of arrays (SoA)
	 */
	unsigned _unSizeTree;	//Size of the allocated memory.
	float* fSplit;
	unsigned* unIdx;
	unsigned* unAxis;
	bool* bIsLeaf;
	float* fLoBound;
	float* fHiBound;
	unsigned unSizeTree;	//Size of memory that the tree actually takes.

	/*
	 * transformation matrix
	 */
	float m[16];
        Matrix* trans;
        Matrix* trans_inv;

	/*
	 * kernel constants
	 */
	unsigned unSizeData;
	unsigned unNoThreads;
	unsigned unNoBlocks;
	unsigned _unWidth;
	unsigned _unHeight;

	/*
	 * search structure
	 */
	ANNkd_tree*	kdTree;
	ANNkdStats* st;

	/*
	 * data pointers
	 */
	float* fHstScn[3];
	float* fHstScnX;
	float* fHstScnY;
	float* fHstScnZ;
	double** h_idata;

	/*
	 * icp
	 */
	unsigned unMaxIteration;
	unsigned _unIterations;
	float fMaxProcTime;
	float fMaxDeviation;
	float _fSearchRadiusMax;
	float _fSearchRadiusMin;
	float _fRadiusStep;
	unsigned _unNoQSizeStep;
        float* temp_ones;
        float* ones;

	enum EnumIcpState { ICP_LIMIT 			= 0,
						ICP_PROCESSING 		= 1,
						ICP_NOTMATCHABLE 	= 2,
						ICP_MAXITERATIONS 	= 3,
						ICP_TIMEELAPSED 	= 4,
						ICP_SUCCESS 		= 5 };

	unsigned unPairs;
	unsigned* pNoPairs;
	Matrix* final_matrix;
	double _dElapsedTime;
        Matrix** matrices;

    /*
        Minimums of all values
    */
    float min_x;
    float min_y;
    float min_z;

	clock_t init_time; // To save the starting point of the timer Added by Shams

	////////////////
	//gpu variables
	////////////////

	float* fDevSplit;
	unsigned* unDevIdx;
	unsigned* unDevAxis;
	bool* bDevIsLeaf;
	float* fDevLoBound;
	float* fDevHiBound;
	float* fDevScnX;
	float* fDevScnY;
	float* fDevScnZ;
	float* fDist;
	float* fDistCpt; 			//compacted distance list
	unsigned* unMask;
	float* fDevMdlPairX;
	float* fDevMdlPairY;
	float* fDevMdlPairZ;
	float* fDevScnPairX;
	float* fDevScnPairY;
	float* fDevScnPairZ;

        /////////////
        float* cngfDevScnX;
        float* cngfDevScnY;
        float* cngfDevScnZ;
	float* cngfDevMdlPairX;
	float* cngfDevMdlPairY;
	float* cngfDevMdlPairZ;
	float* cngfDevScnPairX;
	float* cngfDevScnPairY;
	float* cngfDevScnPairZ;
        /////////////
	float* fCenModX;
	float* fCenModY;
	float* fCenModZ;
	float* fCenScnX;
	float* fCenScnY;
	float* fCenScnZ;
	unsigned* unNoPairs;

	//kd-tree based nearest neighbor search, using a priority queue.
	void class_nns_priority(
			float* fDevScnX,						//scene point cloud
			float* fDevScnY,
			float* fDevScnZ,
			float* fDist,							//squared distance between pairs, for deviation calculation
			float* fDevSplit, 						//kd-tree: position of splitting plain (inner node)
			unsigned* unDevIdx, 					//kd-tree: index of point (leaf node)
			unsigned* unDevAxis, 					//kd-tree: axis where splitting plain locates (inner node)
			bool* bDevIsLeaf, 						//kd-tree: node type (both nodes)
			float* fDevLoBound,						//kd-tree: lower bounding box (inner node)
			float* fDevHiBound,						//kd-tree: higher bounding box (inner node)
			unsigned* unMask,						//a 0-1 mask of pair and non-pairs.
			float* fDevMdlPairX,
			float* fDevMdlPairY,
			float* fDevMdlPairZ,
			float* fDevScnPairX,
			float* fDevScnPairY,
			float* fDevScnPairZ,
			float fSearchRadius,
			unsigned unSize,
			unsigned unWidth,
			unsigned unQStep);						//for dubugging thread


	//centralize a pointcloud
	void class_centralize(unsigned* unMask,
			float* fDevMdlPairX,
			float* fDevMdlPairY,
			float* fDevMdlPairZ,
			float* fDevScnPairX,
			float* fDevScnPairY,
			float* fDevScnPairZ,
			float fcm0,
			float fcm1,
			float fcm2,
			float fcs0,
			float fcs1,
			float fcs2,
			float* fCenteredModX,					//centered point cloud
			float* fCenteredModY,
			float* fCenteredModZ,
			float* fCenteredScnX,
			float* fCenteredScnY,
			float* fCenteredScnZ);

	//transform point cloud
	void class_transformation(float* fDevScnX,					//piont cloud to be transformed
			float* fDevScnY,
			float* fDevScnZ,
			float m00,	float m01,	float m02,	float m03,
			float m10,	float m11,	float m12,	float m13,
			float m20,	float m21,	float m22,	float m23);
};



#endif