#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;
		std::vector< stim::vec<T> > pos;
		std::vector< stim::vec<T> > mags;
		std::vector< T > L;
	
		void
		init()
		{

		}

		void
		init(std::vector<stim::vec<T> > inP, std::vector<stim::vec<T> > inM)
		{
			pos = inP;
			mags = inM;
			L.resize(pos.size()-1);
			for(int i = 0; i < L.size(); i++)
			{
				L[i] += (pos[i] - pos[i+1]).len();
			}
		}
		
		stim::vec<T>
		d(int idx)
		{
			return (pos[idx] - pos[idx+1]).norm();
			
		}

		T
		getl(int j)
		{
			for(int i = 0; i < j-1; ++i)
			{
				L += (pos[i] -pos[i+1]).len();
			}
		}

		int
		findIdx(T l)
		{
			int i = pos.size()/2;
			while(1)
			{
				if(L[i] < l)
				{
					i = i/2;
				}
				else if(L[i] < l && L[i+1] > l)
				{
					break;
				}
				else
				{
					i = i+i/2;
				}
			}
			return i;
		}

	public:
		cylinder()
		{

		}

		///constructor to create a cylinder from a set of points, radii, and the number of sides for the cylinder.
		///The higher the number of sides, the more rectangeles compose the surface of 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).
		stim::vec<T>
		p(T pvalue)
		{
			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])));
		}

		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).
		T
		r(T pvalue)
		{
			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])));
		}

		T
		r(T l, int idx)
		{
			return (mags[idx] + (mags[idx+1]-mags[idx])*((l-L[idx])/(L[idx+1]- L[idx])));
		}


		///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
		stim::vec<T>
		surf(T pvalue, T theta)
		{
			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));
		}

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

}
#endif