/*
Copyright <2017> <David Mayerich>

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#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{

protected:
	unsigned int N;					//number of points in the fiber
	double **c;						//centerline (array of double pointers)

	/// Initialize an empty fiber
	void init(){
		N=0;
		c=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
		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
		}
	}

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

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

	//initialize a centerline with n points
	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];
		}
	}

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

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


	/// 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

	}

	/// 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)
		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