#ifndef STIM_CENTERLINE_H
#define STIM_CENTERLINE_H

#include <vector>
#include <stim/math/vec3.h>
//#include <ANN/ANN.h>

namespace stim{

/**	This class stores information about a single fiber represented as a set of geometric points
 *	between two branch or end points. This class is used as a fundamental component of the stim::network
 *	class to describe an interconnected (often biological) network.
 */
template<typename T>
class centerline{

protected:
	unsigned int N;					//number of points in the fiber
	double **c;						//centerline (array of double pointers)
//	ANNkd_tree* kdt;				//kd-tree stores all points in the fiber for fast searching

	/// Initialize an empty fiber
	void init()
	{
		N=0;
		c=NULL;
//		kdt = NULL;
	}

	/// Initialize a fiber with N centerline points (all located at [0, 0, 0] with radius 0)
	void init(unsigned int n)
	{

		N = n;												//set the number of points
//		kdt = NULL;
		c = (double**) malloc(sizeof(double*) * N);			//allocate the array pointer

		for(unsigned int i = 0; i < N; i++)					//allocate space for each point
			c[i] = (double*) malloc(sizeof(double) * 3);
	}

	/// Copies an existing fiber to the current fiber

	/// @param cpy stores the new copy of the fiber
	void copy( const stim::centerline<T>& cpy, bool kd = 0){

		///allocate space for the new fiber
		init(cpy.N);

		///copy the points
		for(unsigned int i = 0; i < N; i++){
			for(unsigned int d = 0; d < 3; d++)		//for each dimension
				c[i][d] = cpy.c[i][d];				//copy the coordinate
		}
//		if(kd)
//			gen_kdtree();							//generate the kd tree for the new fiber
	}

	/// generate a KD tree for points on fiber
//	void gen_kdtree()
//	{
//		int n_data = N; //create an array of data points
//		ANNpointArray pts = (ANNpointArray)c;			//cast the centerline list to an ANNpointArray
//		kdt = new ANNkd_tree(pts, n_data, 3);			//build a KD tree
//	}

	/// find distance between two points
	double dist(double* p0, double* p1){

		double sum = 0; // initialize variables
		float v;
		for(unsigned int d = 0; d < 3; d++)
		{
			v = p1[d] - p0[d];
			sum +=v * v;
		}
		return sqrt(sum);
	}

	/// This function retreives the index for the fiber point closest to q

	/// @param q is a reference point used to find the closest point on the fiber center line
//	unsigned int ann( stim::vec<double> q ){
//		ANNidxArray idx = new ANNidx[1];			//variable used to hold the nearest point
//		ANNdistArray sq_dist = new ANNdist[1];		//variable used to hold the squared distance to the nearest point
//		kdt->annkSearch(q.data(), 1, idx, sq_dist);	//search the KD tree for the nearest neighbor
//		return *idx;
//	}

	/// Returns a stim::vec representing the point at index i

	/// @param i is an index of the desired centerline point
	stim::vec<T> get_vec(unsigned i){
		stim::vec3<T> r;
		r.resize(3);
		r[0] = c[i][0];
		r[1] = c[i][1];
		r[2] = c[i][2];

		return r;
	}


public:

	centerline(){
		init();
	}

	/// Copy constructor
	centerline(const stim::centerline<T> &obj){
		copy(obj);
	}

	//temp constructor for graph visualization
	centerline(int n)
	{
		init(n);
	}

	/// Constructor takes a list of stim::vec points, the radius at each point is set to zero
	centerline(std::vector< stim::vec<T> > p, bool kd = 0){
		init(p.size());		//initialize the fiber

		//for each point, set the centerline position and radius
		for(unsigned int i = 0; i < N; i++){

			//set the centerline position
			for(unsigned int d = 0; d < 3; d++)
				c[i][d] = (double) p[i][d];

			//set the radius
		}
		//generate a kd tree
//		if(kd)
//			gen_kdtree();
	}

	/// constructor takes a list of points
	centerline(std::vector< stim::vec3< T > > pos, bool kd = 0){
		init(pos.size());		//initialize the fiber

		//for each point, set the centerline position and radius
		for(unsigned int i = 0; i < N; i++){
			//set the centerline position
			for(unsigned int d = 0; d < 3; d++)
				c[i][d] = (double) pos[i][d];
			//set the radius
		}

		//generate a kd tree
		//if(kd)
		//	gen_kdtree();
	}

	/// Assignment operation
	centerline& operator=(const centerline &rhs){
		if(this == &rhs) return *this;			//test for and handle self-assignment
		copy(rhs);
		return *this;
	}


	/// Return the point on the fiber closest to q
	/// @param q is the query point used to locate the nearest point on the fiber centerline
//	stim::vec<T> nearest(stim::vec<T> q){
//
//		stim::vec<double> temp( (double) q[0], (double) q[1], (double) q[2]);
//
//		unsigned int idx = ann(temp);		//determine the index of the nearest neighbor
//
//		return stim::vec<T>((T) c[idx][0], (T) c[idx][1], (T) c[idx][2]);	//return the nearest centerline point
//	}

	/// Return the point index on the fiber closest to q
	/// @param q is the query point used to locate the nearest point on the fiber centerline
//	unsigned int nearest_idx(stim::vec<T> q){
//
//		stim::vec<double> temp((double) q[0], (double) q[1], (double) q[2]);
//
//		unsigned int idx = ann(temp);		//determine the index of the nearest neighbor
//
//		return idx;	//return the nearest centerline point index
//	}

	/// Returns the fiber centerline as an array of stim::vec points
	std::vector< stim::vec<T> > get_centerline(){

		//create an array of stim vectors
		std::vector< stim::vec3<T> > pts(N);

		//cast each point to a stim::vec, keeping only the position information
		for(unsigned int i = 0; i < N; i++)
			pts[i] = stim::vec3<T>((T) c[i][0], (T) c[i][1], (T) c[i][2]);

		//return the centerline array
		return pts;
	}

	/// Split the fiber at the specified index. If the index is an end point, only one fiber is returned
	std::vector< stim::centerline<T> > split(unsigned int idx){

		std::vector< stim::centerline<T> > fl;		//create an array to store up to two fibers

		//if the index is an end point, only the existing fiber is returned
		if(idx == 0 || idx == N-1){
			fl.resize(1);							//set the size of the fiber to 1
			fl[0] = *this;							//copy the current fiber
		}

		//if the index is not an end point
		else{

			unsigned int N1 = idx + 1;					//calculate the size of both fibers
			unsigned int N2 = N - idx;

			fl.resize(2);								//set the array size to 2

			fl[0].init(N1);								//set the size of each fiber
			fl[1].init(N2);

			//copy both halves of the fiber
			unsigned int i, d;

			//first half
			for(i = 0; i < N1; i++){					//for each centerline point
				for(d = 0; d < 3; d++)
					fl[0].c[i][d] = c[i][d];			//copy each coordinate
			}

			//second half
			for(i = 0; i < N2; i++){
				for(d = 0; d < 3; d++)
					fl[1].c[i][d] = c[idx + i][d];

			}
		}

		return fl;										//return the array

	}

	/// Calculates the set of fibers resulting from a connection between the current fiber and a fiber f

	/// @param f is the fiber that will be connected to the current fiber
/*	std::vector< stim::centerline<T> > connect( stim::centerline<T> &f, double dist){

		double min_dist;
		unsigned int idx0, idx1;

		//go through each point in the query fiber, looking for the indices for the closest points
		for(unsigned int i = 0; i < f.n_pts(); i++){
			//Run through all points and find the index with the closest point, then partition the fiber and return two fibers.

		}



	}
*/
	/// Outputs the fiber as a string
	std::string str(){
		std::stringstream ss;

		//create an iterator for the point list
		//typename std::list< point<T> >::iterator i;
		for(unsigned int i = 0; i < N; i++){
			ss<<"  [  ";
			for(unsigned int d = 0; d < 3; d++){
				ss<<c[i][d]<<"  ";
			}
		}

		return ss.str();
	}
	/// Returns the number of centerline points in the fiber
	unsigned int size(){
		return N;
	}


	/// Bracket operator returns the element at index i

	/// @param i is the index of the element to be returned as a stim::vec
	stim::vec<T> operator[](unsigned i){
		return get_vec(i);
	}

	/// Back method returns the last point in the fiber
	stim::vec<T> back(){
		return get_vec(N-1);
	}
		////resample a fiber in the network
	stim::centerline<T> resample(T spacing)
	{
		std::cout<<"fiber::resample()"<<std::endl;

		std::vector<T> v(3);    //v-direction vector of the segment
		stim::vec<T> p(3);      //- intermediate point to be added
		stim::vec<T> p1(3);   // p1 - starting point of an segment on the fiber,
		stim::vec<T> p2(3);   // p2 - ending point,
		double sum=0;  //distance summation
		std::vector<stim::vec<T> > fiberPositions = centerline();
		std::vector<stim::vec<T> > newPointList; // initialize list of new resampled points on the fiber
		// for each point on the centerline (skip if it is the last point on centerline)
		//unsigned int N = fiberPositions.size(); // number of points on the fiber
		for(unsigned int f=0; f< N-1; f++)
		{
			
			p1 = fiberPositions[f]; p2 = fiberPositions[f + 1]; v = p2 - p1;
			for(unsigned int d = 0; d < 3; d++){
				sum +=v[d] * v[d];}              //length of segment-distance between starting and ending point

			T lengthSegment = sqrt(sum);  //find Length of the segment as distance between the starting and ending points of the segment

			if(lengthSegment >= spacing) // if length of the segment is greater than standard deviation resample
				{
					// repeat resampling until accumulated stepsize is equsl to length of the segment
					for(T step=0.0; step<lengthSegment; step+=spacing)
					{
						// calculate the resampled point by travelling step size in the direction of normalized gradient vector
						for(unsigned int i=0; i<3;i++)
							{
								p[i] = p1[i] + v[i]*(step/lengthSegment);
							} //for each dimension
						// add this resampled points to the new fiber list
						newPointList.push_back(p);
					}
				}
			else       // length of the segment is now less than standard deviation, push the ending point of the segment and proceed to the next point in the fiber
				newPointList.push_back(fiberPositions[f+1]);
			}
		newPointList.push_back(fiberPositions[N-1]);   //add the last point on the fiber to the new fiber list
		centerline newFiber(newPointList);
		return newFiber;
	}

};



}	//end namespace stim



#endif