#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::circle<T> > e;
		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;
			stim::vec<float> v1;
			stim::vec<float> v2;
			e.resize(inP.size());
			if(inP.size() < 2)
				return;

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

			stim::vec<T> dr = (inP[1] - inP[0]).norm();
			s = stim::circle<T>(inP[0], inM[0][0], dr, stim::vec<T>(1,0,0));
			e[0] = s;
			for(int i = 1; i < inP.size()-1; i++)
			{
				s.center(inP[i]);
				v1 = (inP[i] - inP[i-1]).norm();
				v2 = (inP[i+1] - inP[i]).norm();
				dr = (v1+v2).norm();
				s.normal(dr);
				s.scale(inM[i][0]/inM[i-1][0]);
				e[i] = s;
			}
			
			int j = inP.size()-1;
			s.center(inP[j]);
			dr = (inP[j] - inP[j-1]).norm();
			s.normal(dr);
			s.scale(inM[j][0]/inM[j-1][0]);
			e[j] = s;
		}
		
		///returns the direction vector at point idx.
		stim::vec<T>
		d(int idx)
		{
			if(idx == 0)
			{
				return (e[idx+1].P - e[idx].P).norm();
			}
			else if(idx == e.size()-1)
			{
				return (e[idx].P - e[idx-1].P).norm();
			}
			else
			{
//				return (e[idx+1].P - e[idx].P).norm();
				stim::vec<float> v1 = (e[idx].P-e[idx-1].P).norm();
				stim::vec<float> v2 = (e[idx+1].P-e[idx].P).norm();
				return (v1+v2).norm();			
			} 
	//		return e[idx].N;	

		}

		stim::vec<T>
		d(T l, int idx)
		{
			if(idx == 0 || idx == e.size()-1)
			{
				return e[idx].N;
			}
			else
			{
				T rat = (l-L[idx])/(L[idx+1]-L[idx]);
				return(	e[idx].N + (e[idx+1].N - e[idx].N)*rat);
			} 
		}


		///finds the index of the point closest to the length l on the lower bound.
		///binary search.
		int
		findIdx(T l)
		{
			unsigned int i = L.size()/2;
			unsigned int max = L.size()-1;
			unsigned int min = 0;
			while(i > 0 && i < L.size()-1)
			{
//				std::cerr << "Trying " << i << std::endl;
//				std::cerr << "l is " << l << ", L[" << i << "]" << L[i] << std::endl;
				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;
		}

	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);
		}


		///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 stim::vec<float>(-1,-1,-1);
			}
			T l = pvalue*L[L.size()-1];
			int idx = findIdx(l);
				T rat = (l-L[idx])/(L[idx+1]-L[idx]);
			return(	e[idx].P + (e[idx+1].P-e[idx].P)*rat);
		}

		///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)
		{
				T rat = (l-L[idx])/(L[idx+1]-L[idx]);
			return(	e[idx].P + (e[idx+1].P-e[idx].P)*rat);
//			return(
//			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 (e[idx].U.len() + (e[idx+1].U.len() - e[idx].U.len())*((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)
		{
				T rat = (l-L[idx])/(L[idx+1]-L[idx]);
			return(	e[idx].U.len() + (e[idx+1].U.len() - e[idx].U.len())*rat);
//			return (mags[idx] + (mags[idx+1]-mags[idx])*((l-L[idx])/(L[idx+1]- L[idx])))[0];
		}


		///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 stim::vec<float>(-1,-1,-1);
			} else {
//			std::cout << "AAAAAAAAAAAAAAAAAAAAAAA             " << pvalue*L[L.size()-1] << " " << L[L.size()-1] << std::endl;
			T l = pvalue*L[L.size()-1];
//			std::cerr << "The l value is: " << l << std::endl;
			int idx = findIdx(l);
//			std::cerr << "Index: " << idx << std::endl;
			stim::vec<T> ps = p(l, idx); 
			T m = r(l, idx);
			s = e[idx];
			s.center(ps);
			s.normal(d(l, idx));
			s.scale(m/e[idx].U.len());
			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)
		{
			std::vector<std::vector <vec<T> > > points;
			points.resize(e.size());
			for(int i = 0; i < e.size(); i++)
			{
				points[i] = e[i].getPoints(sides);
			}
			return points;
		}

		void
		print(int idx)
		{
			std::cout << d(idx) << std::endl;
		}

		///returns the total length of the line at index j.
		T
		getl(int j)
		{
			return (L[j]);
		}
		
};

}
#endif