#ifndef STIM_CENTERLINE_H
#define STIM_CENTERLINE_H

#include <vector>
#include <stim/math/vec3.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 : public std::vector< stim::vec3<T> >{

protected:

	std::vector<T> L;										//stores the integrated length along the fiber (used for parameterization)

	///Return the normalized direction vector at point i (average of the incoming and outgoing directions)
	vec3<T> d(size_t i) {
		if (size() <= 1) return vec3<T>(0, 0, 0);						//if there is insufficient information to calculate the direction, return a null vector
		if (size() == 2) return (at(1) - at(0)).norm();					//if there are only two points, the direction vector at both is the direction of the line segment
		if (i == 0) return (at(1) - at(0)).norm();						//the first direction vector is oriented towards the first line segment
		if (i == size() - 1) return at(size() - 1) - at(size() - 2);	//the last direction vector is oriented towards the last line segment

		//all other direction vectors are the average direction of the two joined line segments
		return ((at(i) - at(i - 1)).norm() + (at(i + 1) - at(i)).norm()).norm();
	}
	//initializes the integrated length vector to make parameterization easier, starting with index idx (all previous indices are assumed to be correct)
	void update_L(size_t start = 0) {
		L.resize(size());									//allocate space for the L array
		if (start == 0) {
			L[0] = 0;											//initialize the length value for the first point to zero (0)
			start++;
		}

		stim::vec3<T> d;
		for (size_t i = start; i < size(); i++) {		//for each line segment in the centerline
			d = at(i) - at(i - 1);
			L[i] = L[i - 1] + d.len();				//calculate the running length total
		}
	}

	void init() {
		if (size() == 0) return;								//return if there aren't any points
		update_L();
	}

	/// 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){
		return std::vector< stim::vec3<T> >::at(i);
	}

	///finds the index of the point closest to the length l on the lower bound.
	///binary search.
	size_t findIdx(T l) {
		size_t i = L.size() / 2;
		size_t max = L.size() - 1;
		size_t min = 0;
		while (i > 0 && i < L.size() - 1){
			if (l < L[i]) {
				max = i;
				i = min + (max - min) / 2;
			}
			else if (L[i] <= l && L[i + 1] >= l) {
				break;
			}
			else {
				min = i;
				i = min + (max - min) / 2;
			}
		}
		return i;
	}

	///Returns a position vector at the given length into the fiber (based on the pvalue).
	///Interpolates the radius along the line.
	///@param l: the location of the in the cylinder.
	///@param idx: integer location of the point closest to l but prior to it.
	stim::vec3<T> p(T l, int idx) {
		T rat = (l - L[idx]) / (L[idx + 1] - L[idx]);
		stim::vec3<T> v1 = at(idx);
		stim::vec3<T> v2 = at(idx + 1);
		return(v1 + (v2 - v1)*rat);
	}


public:

	using std::vector< stim::vec3<T> >::at;
	using std::vector< stim::vec3<T> >::size;

	centerline() : std::vector< stim::vec3<T> >() {
		init();
	}
	centerline(size_t n) : std::vector< stim::vec3<T> >(n){
		init();
	}

	//overload the push_back function to update the length vector
	void push_back(stim::vec3<T> p) {
		std::vector< stim::vec3<T> >::push_back(p);
		update_L(size() - 1);
	}

	///Returns a position vector at the given p-value (p value ranges from 0 to 1).
	///interpolates the position along the line.
	///@param pvalue: the location of the in the cylinder, from 0 (beginning to 1).
	stim::vec3<T> p(T pvalue) {
		if (pvalue <= 0.0) return at(0);			//return the first element
		if (pvalue >= 1.0) return back();			//return the last element

		T l = pvalue*L[L.size() - 1];
		int idx = findIdx(l);
		return p(l, idx);
	}

	///Return the length of the entire centerline
	T length() {
		return L.back();
	}

	/// 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
		size_t N = size();

		//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] = stim::centerline<T>(N1);			//set the size of each fiber
			fl[1] = stim::centerline<T>(N2);

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

			//first half
			for(i = 0; i < N1; i++)					//for each centerline point
				fl[0][i] = std::vector< stim::vec3<T> >::at(i);
			fl[0].init();							//initialize the length vector

			//second half
			for(i = 0; i < N2; i++)
				fl[1][i] = std::vector< stim::vec3<T> >::at(idx+i);
			fl[1].init();							//initialize the length vector
		}

		return fl;										//return the array

	}

	/// Outputs the fiber as a string
	std::string str(){
		std::stringstream ss;
		size_t N = std::vector< stim::vec3<T> >::size();
		ss << "---------[" << N << "]---------" << std::endl;
		for (size_t i = 0; i < N; i++)
			ss << std::vector< stim::vec3<T> >::at(i) << std::endl;
		ss << "--------------------" << std::endl;

		return ss.str();
	}

	/// Back method returns the last point in the fiber
	stim::vec3<T> back(){
		return std::vector< stim::vec3<T> >::back();
	}

		////resample a fiber in the network
	stim::centerline<T> resample(T spacing)
	{
		std::cout<<"fiber::resample()"<<std::endl;

		stim::vec3<T> v;    //v-direction vector of the segment
		stim::vec3<T> p;      //- intermediate point to be added
		stim::vec3<T> p1;   // p1 - starting point of an segment on the fiber,
		stim::vec3<T> p2;   // p2 - ending point,
		//double sum=0;  //distance summation

		size_t N = size();

		centerline<T> new_c; // initialize list of new resampled points on the fiber
		// for each point on the centerline (skip if it is the last point on centerline)
		for(unsigned int f=0; f< N-1; f++)
		{			
			p1 = at(f); 
			p2 = at(f+1);
			v = p2 - p1;
			
			T lengthSegment = v.len();			//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
					p = p1 + v * (step / lengthSegment);
					
					// add this resampled points to the new fiber list
					new_c.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
				new_c.push_back(at(f+1));
		}
		new_c.push_back(at(N-1));   //add the last point on the fiber to the new fiber list
		//centerline newFiber(newPointList);
		return new_c;
	}

};



}	//end namespace stim



#endif