#ifndef STIM_CYLINDER_H
#define STIM_CYLINDER_H
#include <iostream>
#include <stim/math/circle.h>
#include <stim/math/vector.h>


namespace stim
{
template<typename T>
class cylinder
{
	private:
		stim::circle<T> s;			//an arbitrary circle
		std::vector< stim::vec<T> > pos;	//positions of the cylinder.
		std::vector< stim::vec<T> > mags;	//radii at each position
		std::vector< T > L;			//length of the cylinder at each position.

		///default init
		void
		init(){

		}

		///inits the cylinder from a list of points (inP) and radii (inM)
		void
		init(std::vector<stim::vec<T> > inP, std::vector<stim::vec<T> > inM){
			pos = inP;
			mags = inM;

			//calculate each L.
			L.resize(pos.size()-1);
			T temp = (T)0;
			for(int i = 0; i < L.size(); i++)
			{
				temp += (pos[i] - pos[i+1]).len();
				L[i] = temp;
			}
		}

		///returns the direction vector at point idx.
		stim::vec<T>
		d(int idx){
			return (pos[idx] - pos[idx+1]).norm();
		}

		///returns the total length of the line at index j.
		T
		getl(int j){
			for(int i = 0; i < j-1; ++i)
			{
				temp += (pos[i] - pos[i+1]).len();
				L[i] = temp;
			}
		}

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

		//initializes the length array given the current set of positions
		void init_length(){
			vec<T> p0, p1;
			p0 = pos[0];						//initialize the first point in the segment to the first point in the cylinder
			T l;								//allocate space for the segment length
			for(unsigned p = 1; p < pos.size(); p++){		//for each point in the cylinder
				p1 = pos[p];					//get the second point in the segment
				l = (p1 - p0).len();			//calculate the length of the segment

				if(p == 1) L[0] = l;			//set the length for the first segment
				else L[p-1] = L[p-2] + l;		//calculate and set the running length for each additional segment
			}

		}

	public:
		///default constructor
		cylinder(){}

		///constructor to create a cylinder from a set of points, radii, and the number of sides for the cylinder.
		///@param inP:  Vector of stim vecs composing the points of the centerline.
		///@param inM:  Vector of stim vecs composing the radii of the centerline.
		cylinder(std::vector<stim::vec<T> > inP, std::vector<stim::vec<T> > inM){
			init(inP, inM);
		}

		///Constructor defines a cylinder with centerline inP and magnitudes of zero
		///@param inP: Vector of stim vecs composing the points of the centerline
		cylinder(std::vector< stim::vec<T> > inP){
			std::vector< stim::vec<T> > inM;						//create an array of arbitrary magnitudes

			stim::vec<T> zero;
			zero.push_back(0);

			inM.resize(inP.size(), zero);								//initialize the magnitude values to zero
			init(inP, inM);
		}


		///Returns the number of points on the cylinder centerline

		unsigned int size(){
			return pos.size();
		}


		///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::vec<T>
		p(T pvalue){
			if(pvalue < 0.0 || pvalue > 1.0)
				return;
			T l = pvalue*L[L.size()-1];
			int idx = findIdx(l);
			return (pos[idx] + (pos[idx+1]-pos[idx])*((l-L[idx])/(L[idx+1]- L[idx])));
		}

		///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::vec<T>
		p(T l, int idx){
			return (pos[idx] + (pos[idx+1]-pos[idx])*((l-L[idx])/(L[idx+1]- L[idx])));
		}

		///Returns a radius at the given p-value (p value ranges from 0 to 1).
		///interpolates the radius along the line.
		///@param pvalue: the location of the in the cylinder, from 0 (beginning to 1).
		T
		r(T pvalue){
			if(pvalue < 0.0 || pvalue > 1.0)
				return;
			T l = pvalue*L[L.size()-1];
			int idx = findIdx(l);
			return (mags[idx] + (mags[idx+1]-mags[idx])*((l-L[idx])/(L[idx+1]- L[idx])));
		}

		///Returns a radius at the given length into the fiber (based on the pvalue).
		///Interpolates the position 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.
		T
		r(T l, int idx){
			return (mags[idx] + (mags[idx+1]-mags[idx])*((l-L[idx])/(L[idx+1]- L[idx])));
		}

		///	Returns the magnitude at the given index
		///	@param i is the index of the desired point
		/// @param m is the index of the magnitude value
		T ri(unsigned i, unsigned m = 0){
			return mags[i][m];
		}

		/// Adds a new magnitude value to all points
		/// @param m is the starting value for the new magnitude
		void add_mag(T m = 0){
			for(unsigned int p = 0; p < pos.size(); p++)
				mags[p].push_back(m);
		}

		/// Sets a magnitude value
		/// @param val is the new value for the magnitude
		/// @param p is the point index for the magnitude to be set
		/// @param m is the index for the magnitude
		void set_mag(T val, unsigned p, unsigned m = 0){
			mags[p][m] = val;
		}



		///returns the position of the point with a given pvalue and theta on the surface
		///in x, y, z coordinates. Theta is in degrees from 0 to 360.
		///@param pvalue: the location of the in the cylinder, from 0 (beginning to 1).
		///@param theta: the angle to the point of a circle.
		stim::vec<T>
		surf(T pvalue, T theta)
		{
			if(pvalue < 0.0 || pvalue > 1.0)
				return;
			T l = pvalue*L[L.size()-1];
			int idx = findIdx(l);
			stim::vec<T> ps = p(l, idx);
			T m = r(l, idx);
			stim::vec<T> dr = d(idx);
			s = stim::circle<T>(ps, m, dr);
			return(s.p(theta));
		}

		///returns a vector of points necessary to create a circle at every position in the fiber.
		///@param sides: the number of sides of each circle.
		std::vector<std::vector<vec<T> > >
		getPoints(int sides)
		{
			if(pos.size() < 2)
			{
				return;
			} else {
				std::vector<std::vector <vec<T> > > points;
				points.resize(pos.size());
				stim::vec<T> d = (pos[0] - pos[1]).norm();
				s = stim::circle<T>(pos[0], mags[0][0], d);
				points[0] = s.getPoints(sides);
				for(int i = 1; i < pos.size(); i++)
				{
					d = (pos[i] - pos[i-1]).norm();
					s.center(pos[i]);
					s.normal(d);
					s.scale(mags[i][0]/mags[i-1][0], mags[i][0]/mags[i-1][0]);
					points[i] = s.getPoints(sides);
				}
				return points;
			}
		}

		/// Allows a point on the centerline to be accessed using bracket notation

		vec<T> operator[](unsigned int i){
			return pos[i];
		}

		/// Returns the total length of the cylinder centerline
		T length(){
			return L.back();
		}

		/// Integrates a magnitude value along the cylinder.
		/// @param m is the magnitude value to be integrated (this is usually the radius)
		T integrate(unsigned m = 0){

			T M = 0;						//initialize the integral to zero
			T m0, m1;						//allocate space for both magnitudes in a single segment

			//vec<T> p0, p1;					//allocate space for both points in a single segment

			m0 = mags[0][m];				//initialize the first point and magnitude to the first point in the cylinder
			//p0 = pos[0];

			T len = L[0];						//allocate space for the segment length

			//for every consecutive point in the cylinder
			for(unsigned p = 1; p < pos.size(); p++){

				//p1 = pos[p];							//get the position and magnitude for the next point
				m1 = mags[p][m];

				if(p > 1) len = (L[p-1] - L[p-2]);		//calculate the segment length using the L array

				//add the average magnitude, weighted by the segment length
				M += (m0 + m1)/2.0 * len;

				m0 = m1;								//move to the next segment by shifting points
			}
			return M;			//return the integral
		}

		/// Averages a magnitude value across the cylinder
		/// @param m is the magnitude value to be averaged (this is usually the radius)
		T average(unsigned m = 0){			

			//return the average magnitude
			return integrate(m) / L.back();
		}

		/// Resamples the cylinder to provide a maximum distance of "spacing" between centerline points. All current
		///		centerline points are guaranteed to exist in the new cylinder
		/// @param spacing is the maximum spacing allowed between sample points
		cylinder<T> resample(T spacing){

			std::vector< vec<T> > result;

			vec<T> p0 = pos[0];								//initialize p0 to the first point on the centerline
			vec<T> p1;
			unsigned N = size();							//number of points in the current centerline

			//for each line segment on the centerline
			for(unsigned int i = 1; i < N; i++){
				p1 = pos[i];								//get the second point in the line segment

				vec<T> v = p1 - p0;							//calculate the vector between these two points
				T d = v.len();								//calculate the distance between these two points (length of the line segment)

				unsigned nsteps = d / spacing+1;		//calculate the number of steps to take along the segment to meet the spacing criteria
				T stepsize = 1.0 / nsteps;			//calculate the parametric step size between new centerline points

				//for each step along the line segment
				for(unsigned s = 0; s < nsteps; s++){
					T alpha = stepsize * s;					//calculate the fraction of the distance along the line segment covered
					result.push_back(p0 + alpha * v);	//push the point at alpha position along the line segment
				}

				p0 = p1;								//shift the points to move to the next line segment
			}

			result.push_back(pos[size() - 1]);			//push the last point in the centerline

			return cylinder<T>(result);

		}

};

}
#endif