#ifndef STIM_GL_SPIDER_H
#define STIM_GL_SPIDER_H

#include <GL/glew.h>
#include <GL/glut.h>
#include <cuda.h>
#include <cuda_gl_interop.h>
#include <cudaGL.h>
#include <math.h>
#include "stim/gl/gl_texture.h"
#include "stim/visualization/camera.h"
#include "stim/gl/error.h"
#include "stim/math/vector.h"
#include "stim/math/rect.h"
#include "stim/math/matrix.h"
#include "stim/cuda/spider_cost.cuh"
#include <stim/cuda/cudatools/glbind.h>
#include <stim/cuda/arraymath.cuh>
#include <stim/cuda/cudatools.h>
#include <stim/cuda/ivote.cuh>
#include <stim/visualization/glObj.h>
#include <vector>
#include <stim/cuda/branch_detection.cuh>
#include "../../../volume-spider/fiber.h"
#include "../../../volume-spider/glnetwork.h"
//#include <stim/cuda/testKernel.cuh>

//#include <stim/cuda/testKernel.cuh>

#include <iostream>
#include <fstream>
#ifdef TESTING
	#include <iostream>
	#include <cstdio>
	#include <ctime>
#endif


namespace stim
{

template<typename T>
class gl_spider : public virtual gl_texture<T>
{
	//doen't use gl_texture really, just needs the GLuint id.
	//doesn't even need the texture iD really.
	private:

		//
		stim::vec<float> p;  	//vector designating the position of the spider.
		stim::vec<float> d;	//vector designating the orientation of the spider
						//always a unit vector.
		stim::vec<float> m;	//magnitude of the spider vector.
						//mag[0] = length.
						//mag[1] = width.
		std::vector<stim::vec<float> > dV;	//A list of all the direction vectors.
		std::vector<stim::vec<float> > pV;	//A list of all the position vectors.
		std::vector<stim::vec<float> > mV;	//A list of all the size vectors.

		stim::matrix<float, 4> cT;		//current Transformation matrix
							//From tissue space to texture space.
		GLuint texID;
		stim::vec<float> S;			//Size of a voxel in the volume.
		stim::vec<float> R;			//Dimensions of the volume.


		//GL and Cuda variables
		GLuint dList;				//displaylist ID
		GLuint fboID;				//framebuffer ID
		GLuint texbufferID;			//texbuffer ID, only necessary for 
							//cuda aspect of the calculation.
		GLuint pfboID;
		GLuint ptexbufferID;

		GLuint mfboID;
		GLuint mtexbufferID;

		GLuint bfboId;
		GLuint btexbufferID;


		int numSamples;				//The number of templates in the buffer.
		float stepsize = 4.0;			//Step size.
		int current_cost;
		
		
		//Tracing variables.
		std::stack< stim::vec<float> > seeds;	//Variables for tracing
		std::stack< stim::vec<float> > seedsvecs;
		std::stack< float > seedsmags;
		std::vector< stim::vec<float> > cL;	//Line currently being traced.

		stim::glObj<float> sk;
		stim::glnetwork<float> nt;

		stim::vec<float> rev;			//reverse vector;
		stim::camera camSel;
		stim::vec<float> ps;
		stim::vec<float> ups;
		stim::vec<float> ds;
		std::vector<stim::vec<float> > last3;
		


		/// Method for finding the best scale for the spider.
		/// changes the x, y, z size of the spider to minimize the cost
		/// function. 
		void
		findOptimalDirection()
		{
			setMatrix();
			glCallList(dList);
			int best = getCost();
			stim::vec<float> next(
 			dV[best][0]*S[0]*R[0],
			dV[best][1]*S[1]*R[1],
			dV[best][2]*S[2]*R[2],
			0);
			next = (cT*next).norm();
			setPosition(	p[0]+next[0]*m[0]/stepsize,
					p[1]+next[1]*m[0]/stepsize,
					p[2]+next[2]*m[0]/stepsize);
			setDirection(next[0], next[1], next[2]);
		}

		/// Method for finding the best d for the spider.
		/// Not sure if necessary since the next p for the spider
		/// will be at d * m.
		void
		findOptimalPosition()
		{
			setMatrix();
			glCallList(dList+1);
			int best = getCost();
			stim::vec<float> next(
 			pV[best][0],
			pV[best][1],
			pV[best][2],
			1);
			next = cT*next;	
			setPosition(
					next[0]*S[0]*R[0],
					next[1]*S[1]*R[1],
					next[2]*S[2]*R[2]
				   );
		}
	
		/// Method for finding the best scale for the spider.
		/// changes the x, y, z size of the spider to minimize the cost
		/// function. */
		void
		findOptimalScale()
		{
			setMatrix();
			glCallList(dList+2);
			int best = getCost();
			setMagnitude(m[0]*mV[best][0]);
		}

		void
		branchDetection()
		{
			setMatrix();
			glCallList(dList+3);
			std::vector< stim::vec<float> > result = find_branch(
					btexbufferID, GL_TEXTURE_2D, 16, 216);
			stim::vec<float> size(S[0]*R[0], S[1]*R[1], S[2]*R[2]);
			if(!result.empty())
			{
				for(int i = 1; i < result.size(); i++)
				{
					stim::vec<float> cylp(
						0.5 * cos(2*M_PI*(result[i][1])),
						0.5 * sin(2*M_PI*(result[i][1])),
						result[i][0]-0.5,
						1.0);
					cylp = cT*cylp;
					
					stim::vec<float> vec(
						cylp[0]*S[0]*R[0],
						cylp[1]*S[1]*R[1],
						cylp[2]*S[2]*R[2]);
					stim::vec<float> seeddir(-p[0] + cylp[0]*S[0]*R[0],
						   -p[1] + cylp[1]*S[1]*R[1],
						   -p[2] + cylp[2]*S[2]*R[2]);
						seeddir = seeddir.norm();
					float seedm = m[0]/2.0;
// Uncomment for global run 
/*					stim::vec<float> lSeed = getLastSeed();
					if(sqrt(pow((lSeed[0] - vec[0]),2)
					 + pow((lSeed[1] - vec[1]),2) + 
					 pow((lSeed[2] - vec[2]),2)) > m[0]/4.0
					 &&  */
					if(
					 !(vec[0] > size[0] || vec[1] > size[1]
					 || vec[2] > size[2] || vec[0] < 0
					 || vec[1] < 0 || vec[2] < 0))
					{
						setSeed(vec);
						setSeedVec(seeddir);
						setSeedMag(seedm);
					}
				}
			}    
			
		}


		
		void
		Optimize()
		{
			/*find the optimum d and scale */ 
		}

		
		
		
//--------------------------------------------------------------------------//
//---------------------TEMPLATE CREATION METHODS----------------------------//
//--------------------------------------------------------------------------//

		///@param solidAngle, the size of the arc to sample.
		///Method for populating the vector arrays with sampled vectors.
		///uses the default d vector <0,0,1>
		void
		genDirectionVectors(float solidAngle = 5/M_PI*4)
		{
			//ofstream file;
			//file.open("dvectors.txt");
			//Set up the vectors necessary for Rectangle creation.
			vec<float> Y(1.0,0.0,0.0);
			vec<float> pos(0.0,0.0,0.0);
			vec<float> mag(1.0, 1.0, 1.0);
			vec<float> dir(0.0, 0.0, 1.0);

			//Set up the variable necessary for vector creation.
			vec<float> d_s = d.cart2sph().norm();
			vec<float> temp(0,0,0);
			int dim = (sqrt(numSamples)-1)/2;
			float p0  	= -M_PI;
			float dt  	= solidAngle/(2.0 * ((float)dim + 1.0));
			float dp  	= p0/(2.0*((float)dim + 1.0));
			
			glNewList(dList, GL_COMPILE);
			//Loop over the space
			int idx = 0;
			for(int i = -dim; i <= dim; i++){
				for(int j = -dim; j <= dim; j++){
					
					//Create linear index
					idx = (j+dim)+(i+dim)*((dim*2)+1);	
					temp[0] = d_s[0]; 			//rotate vector
					temp[1] = d_s[1]+dp*(float) i;
					temp[2] = d_s[2]+dt*(float) j;
					
					temp = (temp.sph2cart()).norm();	//back to cart
					dV.push_back(temp);
				 	if(cos(Y.dot(temp))< 0.087){                                                                             Y[0] = 0.0; Y[1] = 1.0;}
					else{Y[0] = 1.0; Y[1] = 0.0;}

                                       	hor = stim::rect<float>(mag,
						 pos, temp,
                       				((Y.cross(temp)).cross(temp)).norm());
       				 	ver = stim::rect<float>(mag,
						 pos, temp,
                       				hor.n());
					UpdateBuffer(0.0, 0.0+idx*8.0);
					CHECK_OPENGL_ERROR
				}
			}
			glEndList();
		}

		///@param solidAngle, the size of the arc to sample.
		///Method for populating the buffer with the sampled texture.
		///uses the default vector <0,0,0>
		void
		genPositionVectors(float delta = 0.4)
		{
			//Set up the vectors necessary for Rectangle creation.
			vec<float> Y(1.0,0.0,0.0);
			vec<float> pos(0.0,0.0,0.0);
			vec<float> mag(1.0, 1.0, 1.0);
			vec<float> dir(0.0, 0.0, 1.0);

			//Set up the variable necessary for vector creation.
			vec<float> temp(0,0,0);
			int dim = (sqrt(numSamples)-1)/2;
			stim::rect<float> samplingPlane =
				 stim::rect<float>(p, d);
			samplingPlane.scale(mag[0]*delta, mag[0]*delta);
			float step = 1.0/(dim);

			//Loop over the samples, keeping the original p sample
			//in the center of the resulting texture.
			int idx;
			glNewList(dList+1, GL_COMPILE);
			for(int i = -dim; i <= dim; i++){
				for(int j = -dim; j <= dim; j++){
					//Create linear index
					idx = (j+dim)+(i+dim)*((dim*2)+1);	

					temp = samplingPlane.p(
							0.5+step*i,
								 0.5+step*j
										);
					pV.push_back(temp);
                			hor = stim::rect<float>(mag,
						 temp, dir,
                      				((Y.cross(d)).cross(d))
						.norm());
                			ver = stim::rect<float>(mag,
						 temp, dir,
                        			hor.n());
					UpdateBuffer(0.0, 0.0+idx*8.0);
				CHECK_OPENGL_ERROR
				}
			}
			glEndList();
		}

		///@param solidAngle, the size of the arc to sample.
		///Method for populating the buffer with the sampled texture.
		///uses the default m <1,1,0>
		void
		genMagnitudeVectors(float delta = 0.70)
//		genMagnitudeVectors(float delta = 0.50)
		{
			
			//Set up the vectors necessary for Rectangle creation.
			vec<float> Y(1.0,0.0,0.0);
			vec<float> pos(0.0,0.0,0.0);
			vec<float> mag(1.0, 1.0, 1.0);
			vec<float> dir(0.0, 0.0, 1.0);

			//Set up the variable necessary for vector creation.
			int dim = (sqrt(numSamples)-1)/2;
			float min 	= 1.0-delta;
			float max 	= 1.0+delta;
			float step	= (max-min)/(numSamples-1);
			float factor;
			vec<float> temp(0.0,0.0,0.0);

			glNewList(dList+2, GL_COMPILE);
			for(int i = 0; i < numSamples; i++){
				//Create linear index
				factor = (min+step*i)*mag[0];
				temp = factor;
				mV.push_back(temp);	
				hor = stim::rect<float>(temp,
					 pos, dir, 
       	       				((Y.cross(d)).cross(d))
					.norm());
               			ver = stim::rect<float>(temp,
					 pos, dir,
                       			hor.n());
				UpdateBuffer(0.0, 0.0+i*8.0);
			CHECK_OPENGL_ERROR
			}
			glEndList();
		}
		///@param v_x x-coordinate in buffer-space,
		///@param v_y y-coordinate in buffer-space.
		///Samples the texturespace and places a sample in the provided coordinates
		///of bufferspace.
		void
		UpdateBuffer(float v_x, float v_y)
		{	
			float len = 8.0;
			stim::vec<float>p1; 
        	        stim::vec<float>p2; 
	                stim::vec<float>p3; 
                	stim::vec<float>p4;	
			p1 = hor.p(1,1);
			p2 = hor.p(1,0);
			p3 = hor.p(0,0);
			p4 = hor.p(0,1);
			glBegin(GL_QUADS);
				glTexCoord3f(
					p1[0],
					p1[1],
					p1[2]
					);
				glVertex2f(v_x,v_y);
				glTexCoord3f(
					p2[0],
					p2[1],
					p2[2]
					);
				glVertex2f(v_x+len, v_y);
				glTexCoord3f(
					p3[0],
					p3[1],
					p3[2]
					);
				glVertex2f(v_x+len, v_y+len);
				glTexCoord3f(
					p4[0],
					p4[1],
					p4[2]
					);
				glVertex2f(v_x, v_y+len);
			 glEnd();

			 p1 = ver.p(1,1);
			 p2 = ver.p(1,0);
			 p3 = ver.p(0,0);
			 p4 = ver.p(0,1);
		 	 glBegin(GL_QUADS);
				glTexCoord3f(
					p1[0],
					p1[1],
					p1[2]
					);
				glVertex2f(v_x+len, v_y);
				glTexCoord3f(
					p2[0],
					p2[1],
					p2[2]
					);
				glVertex2f(v_x+2.0*len, v_y);
				glTexCoord3f(
					p3[0],
					p3[1],
					p3[2]
					);
				glVertex2f(v_x+2.0*len, v_y+len);
				glTexCoord3f(
					p4[0],
					p4[1],
					p4[2]
					);
				glVertex2f(v_x+len, v_y+len);
			glEnd(); 
		}
		


//--------------------------------------------------------------------------//
//--------------------------------GL METHODS--------------------------------//
//--------------------------------------------------------------------------//

		///@param width sets the width of the buffer.
		///@param height sets the height of the buffer.
		///Function for setting up the 2D buffer that stores the samples.
		void
		GenerateFBO(unsigned int width, unsigned int height, GLuint &textureID, GLuint &framebufferID)
		{
			glGenFramebuffers(1, &framebufferID);
			glBindFramebuffer(GL_FRAMEBUFFER, framebufferID);
			int numChannels = 1;
			unsigned char* texels = new unsigned char[width * height * numChannels];
			glGenTextures(1, &textureID);
			glBindTexture(GL_TEXTURE_2D, textureID);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
			glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE,
				 width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, texels);   
			delete[] texels;
			glBindFramebuffer(GL_FRAMEBUFFER, 0); 
			glBindTexture(GL_TEXTURE_2D, 0);
				CHECK_OPENGL_ERROR
		}

		///@param width sets the width of the buffer.
		///@param height sets the height of the buffer.
		///Function for setting up the 2D buffer that stores the samples.
		void
		GenerateFBO(unsigned int width, unsigned int height)
		{
			glGenFramebuffers(1, &fboID);
			glBindFramebuffer(GL_FRAMEBUFFER, fboID);
			int numChannels = 1;
			unsigned char* texels = new unsigned char[width * height * numChannels];
			glGenTextures(1, &texbufferID);
			glBindTexture(GL_TEXTURE_2D, texbufferID);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
			glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE,
				 width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, texels);   
			delete[] texels;
			glBindFramebuffer(GL_FRAMEBUFFER, 0); 
			glBindTexture(GL_TEXTURE_2D, 0);
				CHECK_OPENGL_ERROR
		}


		///Method for using the gl manipulation to alighn templates from
		///Template space (-0.5 0.5) to Texture space (0.0, 1.0),
		///Based on the p of the spider in real space (arbitrary).
		void setMatrix()
		{
			float curTrans[16];
			stim::vec<float> rot = getRotation(d);
			glMatrixMode(GL_TEXTURE);
			glLoadIdentity();
			glScalef(1.0/S[0]/R[0], 1.0/S[1]/R[1], 1.0/S[2]/R[2]);


			glTranslatef(p[0],
				     p[1],
				     p[2]);

			glRotatef(rot[0], rot[1], rot[2], rot[3]);

			glScalef(m[0],
				 m[0],
				 m[0]);

			glGetFloatv(GL_TEXTURE_MATRIX, curTrans);
			cT.set(curTrans);
		//	printTransform();
			
			CHECK_OPENGL_ERROR
			glMatrixMode(GL_MODELVIEW);
		}

		///Method for controling the buffer and texture binding in order to properly
		///do the render to texture.
		void
		Bind()
		{
			float len = 8.0;
			glBindFramebuffer(GL_FRAMEBUFFER, fboID);//set up GL buffer		
			glFramebufferTexture2D(
				GL_FRAMEBUFFER,
				GL_COLOR_ATTACHMENT0,
				GL_TEXTURE_2D,
				texbufferID,
				0);
			glBindFramebuffer(GL_FRAMEBUFFER, fboID);
			GLenum DrawBuffers[1] = {GL_COLOR_ATTACHMENT0};
			glDrawBuffers(1, DrawBuffers);
			glBindTexture(GL_TEXTURE_2D, texbufferID);
			glClearColor(1,1,1,1);
			glClear(GL_COLOR_BUFFER_BIT);
			glMatrixMode(GL_PROJECTION);
			glLoadIdentity();
			glMatrixMode(GL_MODELVIEW);
			glLoadIdentity();
			glViewport(0,0,2.0*len, numSamples*len);
			gluOrtho2D(0.0,2.0*len,0.0,numSamples*len);
			glEnable(GL_TEXTURE_3D);
			glBindTexture(GL_TEXTURE_3D, texID);

			CHECK_OPENGL_ERROR
		}
		
		///Method for controling the buffer and texture binding in order to properly
		///do the render to texture.
		///@param GLuint tbID
		void
		Bind(GLuint &textureID, GLuint &framebufferID, int nSamples)
		{
			float len = 8.0;
			glBindFramebuffer(GL_FRAMEBUFFER, framebufferID);//set up GL buffer
			CHECK_OPENGL_ERROR

			glFramebufferTexture2D(
				GL_FRAMEBUFFER,
				GL_COLOR_ATTACHMENT0,
				GL_TEXTURE_2D,
				textureID,
				0);
			CHECK_OPENGL_ERROR

			glBindFramebuffer(GL_FRAMEBUFFER, framebufferID);
			CHECK_OPENGL_ERROR

			GLenum DrawBuffers[1] = {GL_COLOR_ATTACHMENT0};
			glDrawBuffers(1, DrawBuffers);
			CHECK_OPENGL_ERROR

			glBindTexture(GL_TEXTURE_2D, textureID);
			CHECK_OPENGL_ERROR

			glClearColor(1,1,1,1);
			glClear(GL_COLOR_BUFFER_BIT);
			glMatrixMode(GL_PROJECTION);
			glLoadIdentity();
			glMatrixMode(GL_MODELVIEW);
			glLoadIdentity();
			glViewport(0,0,2.0*len, nSamples*len);
			gluOrtho2D(0.0,2.0*len,0.0,nSamples*len);
			glEnable(GL_TEXTURE_3D);
			glBindTexture(GL_TEXTURE_3D, texID);

			CHECK_OPENGL_ERROR
		}
		
		///Method for Unbinding all of the texture resources
		void
		Unbind()
		{
			//Finalize GL_buffer
			glBindTexture(GL_TEXTURE_3D, 0);                      
			CHECK_OPENGL_ERROR
			glBindTexture(GL_TEXTURE_2D, 0);                      
			CHECK_OPENGL_ERROR
			glBindFramebuffer(GL_FRAMEBUFFER, 0);
			CHECK_OPENGL_ERROR
			glDisable(GL_TEXTURE_3D);
			CHECK_OPENGL_ERROR
		}
		


//--------------------------------------------------------------------------//
//--------------------------------CUDA METHODS------------------------------//
//--------------------------------------------------------------------------//
		

		///Entry-point into the cuda code for calculating the cost
		///	of a given samples array (in texture form) 
		int
		getCost()
		{
			#ifdef TESTING
				start = std::clock();
			#endif
			stim::vec<int> cost = 
				stim::cuda::get_cost(texbufferID, GL_TEXTURE_2D, numSamples);
			cudaDeviceSynchronize();
			#ifdef TESTING
				duration_cuda = duration_cuda +
					 (std::clock() - start) / (double) CLOCKS_PER_SEC;
				num_cuda = num_cuda + 1.0;
			#endif
			current_cost = cost[1];
			return cost[0];
		}

	public:
		stim::rect<float> hor;
		stim::rect<float> ver;	

		//Testing and Timing variables.
		#ifdef TESTING
			std::clock_t start;
			double duration_sampling = 0.0;
			double duration_cuda = 0.0;
			double num_sampling = 0.0;
			double num_cuda = 0.0;
		#endif

//--------------------------------------------------------------------------//
//-----------------------------CONSTRUCTORS---------------------------------//
//--------------------------------------------------------------------------//


		///@param samples, the number of samples this spider is going to use.
		///best results if samples is can create a perfect root.
		///Default Constructor
		gl_spider
		(int samples = 1089)
		{
			p = vec<float>(0.0, 0.0, 0.0);
			d = vec<float>(0.0, 0.0, 1.0);
			m = vec<float>(1.0, 1.0);
			S = vec<float>(1.0, 1.0, 1.0);
			R = vec<float>(1.0, 1.0, 1.0);
			//setPosition(0.0,0.0,0.0);
			//setDirection(0.0,0.0,1.0);
			//setMagnitude(1.0);
			numSamples = samples;
		}

		///temporary constructor for convenience, will be removed in further updates.	
		gl_spider
		(float pos_x, float pos_y, float pos_z, float dir_x, float dir_y, float dir_z,
			float mag_x, int numSamples = 1089)
		{
			p = vec<float>(pos_x, pos_y, pos_z);
			d = vec<float>(dir_x, dir_y, dir_z);
			m = vec<float>(mag_x, mag_x, mag_x);
			S = vec<float>(1.0,1.0,1.0);
			R = vec<float>(1.0,1.0,1.0);
			//setPosition(pos_x, pos_y, pos_z);
			//setDirection(dir_x, dir_y, dir_z);
			//setMagnitude(mag_x);
		
		}
	
		~gl_spider
		(void)
		{
			Unbind();
			glDeleteTextures(1, &texbufferID);
			glDeleteBuffers(1, &fboID);
		/*	glDeleteTextures(1, &ptexbufferID);
			glDeleteBuffers(1, &pfboId);
			glDeleteTextures(1, &mtexbufferID);
			glDeleteBuffers(1, &mfboId);     */
			glDeleteTextures(1, &btexbufferID);
			glDeleteBuffers(1, &bfboId);
		}

		///@param GLuint id texture that is going to be sampled.
		///Attached the spider to the texture with the given GLuint ID.
		///Samples in the default d acting as the init method.
		///Also acts an init.	
		void
		attachSpider(GLuint id)
		{
			texID = id;
			GenerateFBO(16, numSamples*8);
			GenerateFBO(16, 216, btexbufferID, bfboId); 
			setDims(0.6, 0.6, 1.0);
			setSize(512.0, 512.0, 426.0);
			setMatrix();
			dList = glGenLists(3);
			glListBase(dList);
			Bind();
			genDirectionVectors(5*M_PI/4);
			genPositionVectors();
			genMagnitudeVectors();
			Unbind();
			///temporarily changed to 216
			Bind(btexbufferID, bfboId, 27);
			DrawCylinder();
			Unbind();
		}
		
//--------------------------------------------------------------------------//
//-----------------------------ACCESS METHODS-------------------------------//
//--------------------------------------------------------------------------//
		///Returns the p vector.
		vec<float>
		getPosition()
		{
			return p;
		}
	
		///Returns the d vector.
		vec<float>
		getDirection()
		{
			return d;
		}

		///Returns the m vector.
		vec<float>
		getMagnitude()
		{
			return m;
		}
	
		///@param vector pos, the new p.
		///Sets the p vector to input vector pos.
		void
		setPosition(vec<float> pos)
		{
			p = pos;
		}

		///@param x x-coordinate.
		///@param y y-coordinate.
		///@param z z-coordinate.
		///Sets the p vector to the input float coordinates x,y,z.
		void
		setPosition(float x, float y, float z)
		{
			p[0] = x;
			p[1] = y;
			p[2] = z;
		}
		
		///@param vector dir, the new d.
		///Sets the d vector to input vector dir.
		void
		setDirection(vec<float> dir)
		{
			d = dir;
		}
		
		///@param x x-coordinate.
		///@param y y-coordinate.
		///@param z z-coordinate.
		///Sets the d vector to the input float coordinates x,y,z.
		void
		setDirection(float x, float y, float z)
		{
			d[0] = x;
			d[1] = y;
			d[2] = z;
		}
			
		///@param vector dir, the new d.
		///Sets the m vector to the input vector mag.	
		void
		setMagnitude(vec<float> mag)
		{
			m[0] = mag[0];
			m[1] = mag[0];
		}
		
		///@param mag size of the sampled region.
		///Sets the m vector to the input mag for both templates.
		void
		setMagnitude(float mag)
		{
			m[0] = mag;
			m[1] = mag;
		}
		

		void
		setDims(float x, float y, float z)
		{
			S[0] = x;
			S[1] = y;
			S[2] = z;
		}

		void
		setSize(float x, float y, float z)
		{
			R[0] = x;
			R[1] = y;
			R[2] = z;
		}
		
		///@param dir, the vector to which we are rotating
		///given a vector to align to, finds the required
		///axis and angle for glRotatef
		stim::vec<float>
		getRotation(stim::vec<float> dir)
		{
			stim::vec<float> out(0.0,0.0,0.0,0.0);
			stim::vec<float> from(0.0,0.0,1.0);
			out[0] = acos(dir.dot(from))*180/M_PI;
			if(out[0] < 1.0){
				out[0] = 0.0;
				out[1] = 0.0;
				out[2] = 0.0;
				out[3] = 1.0;
			} else {
				stim::vec<float> temp(0.0, 0.0, 0.0);;
				temp = (from.cross(dir)).norm();
				out[1] = temp[0];
				out[2] = temp[1];
				out[3] = temp[2];
			}
			return out;
		}

		///@param pos, the position of the seed to be added.
		///Adds a seed to the seed list.
		///Assumes that the coordinates passes are in tissue space.
		void
		setSeed(stim::vec<float> pos)
		{
			seeds.push(pos);
		}

		void
		setSeedVec(stim::vec<float> dir)
		{
			seedsvecs.push(dir);
		}

		void
		setSeedMag(float mag)
		{
			seedsmags.push(mag);
		}


		///@param x, y, z: variables for the x, y and z coordinate of the seed
		///Adds a seed to the seed list.
		///Assumes that the coordinates passes are in tissue space.
		void
		setSeed(float x, float y, float z)
		{
			seeds.push(stim::vec<float>(x, y, z));
		}

		void
		setSeedVec(float x, float y, float z)
		{
			seedsvecs.push(stim::vec<float>(x, y, z));
		}

		stim::vec<float> 
		getLastSeed()
		{
			stim::vec<float> tp = seeds.top();
			return tp;
		}

		stim::vec<float> 
		getLastSeedVec()
		{
			stim::vec<float> tp = seedsvecs.top();
			return tp;
		}

		float
		getLastSeedMag()
		{
			float tp = seedsmags.top();
			return tp;
		}

		void
		popSeed()
		{
			seeds.pop();
			seedsvecs.pop();
		//	seedsmags.pop();
		}

		std::stack<stim::vec<float> >
		getSeeds()
		{
			return seeds;
		}

		bool
		Empty()
		{
			return (seeds.empty() && seedsvecs.empty());
		}
		///@param string file: variables for the x, y and z coordinate of the seed
		///Adds a seed to the seed list.
		///Assumes that the coordinates passes are in tissue space.
		void
		setSeeds(std::string file)
		{
			std::ifstream myfile(file.c_str());
			string line;
			if(myfile.is_open())
           		{
                   		while (getline(myfile, line))
                   		{
                           		float x, y, z, u, v, w;
                           		myfile >> x >> y >> z >> u >> v >> w;
                           		seeds.push(stim::vec<float>(
                                  		((float) x), 
                                  		((float) y), 
                                  		((float) z)));
					seedsvecs.push(stim::vec<float>(
						((float) u),
						((float) v),
						((float) w)));
                   		}
                   	myfile.close();
	          	} else {
        	                  std::cerr<<"failed" << std::endl;
			}
		}
		
		void
		saveNetwork(std::string name)
		{
			sk.save(name);
		}

		stim::glObj<float>
		getNetwork()
		{
			return sk;
		}
		
		///Function to get back the framebuffer Object attached to the spider.
		///For external access.
		GLuint
		getFB()
		{
			return bfboId;
		}

//--------------------------------------------------------------------------//
//-----------------------------TEMPORARY METHODS----------------------------//
//--------------------------------------------------------------------------//

		///temporary Method necessary for visualization and testing.
		void
		Update()
		{
			vec<float> Y(1.0,0.0,0.0);
			if(cos(Y.dot(d))< 0.087){
				Y[0] = 0.0; Y[1] = 1.0;}
			hor = stim::rect<float>(m, p, d.norm(),
				((Y.cross(d)).cross(d)).norm());
			ver = stim::rect<float>(m, p, d.norm(),
				 hor.n());
		}


		int
		Step()
		{
			Bind();
			CHECK_OPENGL_ERROR
			#ifdef TESTING
				start = std::clock();
			#endif
			findOptimalDirection();
			//test(texbufferID, GL_TEXTURE_2D); 
			findOptimalPosition();
			findOptimalScale();
			Unbind();
			CHECK_OPENGL_ERROR

			#ifdef TESTING
				duration_sampling = duration_sampling +
					 (std::clock() - start) / (double) CLOCKS_PER_SEC;
				num_sampling = num_sampling + 1.0; 
			#endif
			return current_cost;
		}

		int
		StepP()
		{
			Bind();
			CHECK_OPENGL_ERROR
			#ifdef TESTING
				start = std::clock();
			#endif
			findOptimalDirection();
			//test(texbufferID, GL_TEXTURE_2D); 
			findOptimalPosition();
			findOptimalScale();
			Unbind();
			CHECK_OPENGL_ERROR
			Bind(btexbufferID, bfboId, 27);
			CHECK_OPENGL_ERROR
			branchDetection();
			CHECK_OPENGL_ERROR
			Unbind();
			CHECK_OPENGL_ERROR

			#ifdef TESTING
				duration_sampling = duration_sampling +
					 (std::clock() - start) / (double) CLOCKS_PER_SEC;
				num_sampling = num_sampling + 1.0; 
			#endif
			return current_cost;
		}


		void
		printTransform()
		{
			std::cout << cT << std::endl;
		}

		/* Method for initializing the cuda devices, necessary only
			there are multiple cuda devices */
		void
		initCuda()
		{	
			stim::cudaSetDevice();
			//GLint max;
			//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &max);
			//std::cout << max << std::endl;
		}

//--------------------------------------------------------------------------//
//-----------------------------EXPERIMENTAL METHODS-------------------------//
//--------------------------------------------------------------------------//

		void
		DrawCylinder()
		{	 
			 glNewList(dList+3, GL_COMPILE);
			 float z0 = -0.5; float z1 = 0.5; float r0 = 0.5;
			 float x,y;
			 float xold = 0.5; float yold = 0.0;
			 float step = 360.0/numSamples*32;
			 glEnable(GL_TEXTURE_3D);
			 glBindTexture(GL_TEXTURE_3D, texID);
			 glBegin(GL_QUAD_STRIP);
			 int j = 0;
			 	for(float i = step; i <= 360.0; i += step)
			 	{
					 x=r0*cos(i*2.0*M_PI/360.0);
					 y=r0*sin(i*2.0*M_PI/360.0);
					 glTexCoord3f(x,y,z0); 
					 glVertex2f(0.0, j*6.4+6.4);
					 glTexCoord3f(x,y,z1); 
					 glVertex2f(16.0, j*6.4+6.4);
					 glTexCoord3f(xold,yold,z1); 
					 glVertex2f(16.0, j*6.4); 
					 glTexCoord3f(xold,yold,z0); 
					 glVertex2f(0.0, j*6.4);
					 xold=x;
					 yold=y;
					 j++;
				}
		      	 glEnd();  
			 glEndList();
		}
		

		///@param min_cost the cost value used for tracing
		///traces out each seedpoint in the seeds queue to completion in both directions.
		void
		trace(int min_cost)
		{	
			Bind();
			rev = stim::vec<float>(0.0,0.0,1.0);
			bool sEmpty = true;
			float lastmag = 16.0;;
			while(!seeds.empty())
			{
				//clear the currently traced line and start a new one.
				cL.clear();
				sk.Begin(stim::OBJ_LINE);
				stim::vec<float> curSeed = seeds.top();
//				std::cout << "The current seeds is " << curSeed << std::endl;
				stim::vec<float> curSeedVec = seedsvecs.top();
				seeds.pop();
				seedsvecs.pop();
//				std::cout << "The current seed Vector is " << curSeedVec << std::endl;
				setPosition(curSeed);
				setDirection(curSeedVec);
				cL.push_back(curSeed);
				sk.createFromSelf(GL_SELECT);
				traceLine(min_cost);

				sk.rev();
		//		std::cout << "reversed" << std::endl;
				std::reverse(cL.begin(), cL.end());
				setPosition(curSeed);
				setDirection(-rev);
				setMagnitude(16.0);
				sk.createFromSelf(GL_SELECT);
				traceLine(min_cost);
				sk.End();
			}
			Unbind();
		}

		///@param min_cost the cost value used for tracing
		///traces the seedpoint passed to completion in one directions.
		void
		traceLine(int min_cost)
		{
			stim::vec<float> pos;
			stim::vec<float> mag;
			int h;
			bool started = false;
			bool running = true;
			stim::vec<float> size(S[0]*R[0], S[1]*R[1], S[2]*R[2]);
			while(running)
			{
				int cost = Step();
				if (cost > min_cost){
					running = false;
					break;
				} else {
					//Have we found an edge?
					pos = getPosition();
					if(pos[0] > size[0] || pos[1] > size[1]
					 || pos[2] > size[2] || pos[0] < 0
					 || pos[1] < 0 || pos[2] < 0)
					{
//					       std::cout << "Found Edge" << std::endl;
						running = false;
						break;
					}
					//If this is the first step in the trace,
					// save the direction
					//(to be used later to trace the fiber in the opposite direction)
					if(started == false){
						rev = -getDirection();
						started = true;
					}
//					std::cout << i << p << std::endl;
					m = getMagnitude();
					//Has the template size gotten unreasonable?
					if(m[0] > 75 || m[0] < 1){
//						std::cout << "Magnitude Limit" << std::endl;
						running = false;
						break;
					}
					else
					{
						h = selectObject(pos, getDirection(), m[0]);
						//Have we hit something previously traced?
						if(h != -1){
							std::cout << "I hit a line" << h << std::endl;
							running = false;
							break;
						}
						else {          
							cL.push_back(stim::vec<float>(p[0], p[1],p[2]));
							sk.TexCoord(m[0]);
							sk.Vertex(p[0], p[1], p[2]);
							Bind(btexbufferID, bfboId, 27);
							CHECK_OPENGL_ERROR
							branchDetection();
							CHECK_OPENGL_ERROR
							Unbind();
							CHECK_OPENGL_ERROR
						}
				 	}
                 		}
         		}
		}	


		int
		selectObject(stim::vec<float> loc, stim::vec<float> dir, float mag) 
		{
		//Define the varibles and turn on Selection Mode

			float s = 3.0;
			GLuint selectBuf[2048];
			GLint hits;
			glSelectBuffer(2048, selectBuf);
			glDisable(GL_CULL_FACE);
			(void) glRenderMode(GL_SELECT);
		//Init Names stack

			glInitNames();
			glPushName(1);

			CHECK_OPENGL_ERROR
		//What would that vessel see in front of it.
				camSel.setPosition(loc);
				camSel.setFocalDistance(mag/s);
				camSel.LookAt((loc[0]+dir[0]*mag/s),
					 (loc[1]+dir[1]*mag/s),
					 (loc[2]+dir[2]*mag/s));
				ps  = camSel.getPosition();
				ups = camSel.getUp();
				ds  = camSel.getLookAt();
				glMatrixMode(GL_PROJECTION);
				glPushMatrix();
				glLoadIdentity();
				glOrtho(-mag/s/2.0, mag/s/2.0, -mag/s/2.0, mag/s/2.0, 0.0, mag/s/2.0);
				glMatrixMode(GL_MODELVIEW);
				glPushMatrix();
				glLoadIdentity();
				
				CHECK_OPENGL_ERROR
				gluLookAt(ps[0], ps[1], ps[2],
					 ds[0], ds[1], ds[2],
					 ups[0], ups[1], ups[2]);

//				sk.Render();
				nt.Render();								

				CHECK_OPENGL_ERROR


//				glLoadName((int) sk.numL());
				glLoadName(nt.sizeE());

//				sk.RenderLine(cL);
				nt.RenderLine(cL);	

//				glPopName();
				glFlush();

				glMatrixMode(GL_PROJECTION);
				glPopMatrix();
				glMatrixMode(GL_MODELVIEW);
				CHECK_OPENGL_ERROR
				glPopMatrix();

		//	glEnable(GL_CULL_FACE);
			hits = glRenderMode(GL_RENDER);
			int found_hits = processHits(hits, selectBuf);
			return found_hits;
		}

		//Given a size of the array (hits) and the memory holding it (buffer)
		//returns whether a hit tool place or not.
		int
		processHits(GLint hits, GLuint buffer[])
		{
			GLuint names, *ptr;
			//printf("hits = %u\n", hits);
			ptr = (GLuint *) buffer;
		//	for (int i = 0; i < hits; i++) { /*  for each hit  */
			names = *ptr;
		//		printf (" number of names for hit = %u\n", names);
			ptr++;
			ptr++; //Skip the minimum depth value.  
			ptr++; //Skip the maximum depth value.
		//		printf ("   the name is ");                                                        
		//		for (int j = 0; j < names; j++) {     /*  for each name */
		//			printf ("%u ", *ptr); ptr++;
		//		}
		//		printf ("\n");
	//		}

			
			if(hits == 0)
			{
				return -1;
			}
			else
			{
				printf ("%u ", *ptr);
				return *ptr;
			}
		}

		void
		clearCurrent()
		{
			cL.clear();
		}	
	
		void
		addToNetwork(pair<stim::fiber<float>, int> in)
		{
                        std::vector<stim::vec<float> > ce = in.first.centerline();                
                        std::vector<stim::vec<float> > cm = in.first.centerlinemag();
			if(ce.size() >2)
			{
				if(in.second == -1)
				{
					nt.addEdge(ce, cm, -1, -1);
				}
				else if(in.second != -1)
				{
					nt.addEdge(ce, cm, -1, in.second);
				}
			}		
		}

		stim::glnetwork<T>
		getGLNetwork()
		{
			return nt;
		}

		std::pair<stim::fiber<float>, int >
		traceLine(stim::vec<float> pos, stim::vec<float> mag, int min_cost)
		{
			Bind();
			sk.Begin(stim::OBJ_LINE);


//			sk.createFromSelf(GL_SELECT);
			nt.createFromSelf(GL_SELECT);

			std::vector<stim::vec<float> > cM;
			cL.push_back(pos);
			cM.push_back(mag);

//			setPosition(pos);
//			setMagnitude(mag);
			int h;
			bool started = false;
			bool running = true;
			stim::vec<float> size(S[0]*R[0], S[1]*R[1], S[2]*R[2]);
			while(running)
			{
				int cost = Step();
				if (cost > min_cost){
					running = false;
					sk.End();
					pair<stim::fiber<float>, int> a(stim::fiber<float> (cL, cM), -1);
					addToNetwork(a);
					return a;
					break;
				} else {
					//Have we found the edge of the map?
					pos = getPosition();
					if(pos[0] > size[0] || pos[1] > size[1]
					 || pos[2] > size[2] || pos[0] < 0
					 || pos[1] < 0 || pos[2] < 0)
					{
//					       std::cout << "Found Edge" << std::endl;
						running = false;
						sk.End();
						pair<stim::fiber<float>, int> a(stim::fiber<float> (cL, cM), -1);
						addToNetwork(a);
						return a;
						break;
					}
					//If this is the first step in the trace,
					// save the direction
					//(to be used later to trace the fiber in the opposite direction)
					if(started == false){
						rev = -getDirection();
						started = true;
					}
//					std::cout << i << p << std::endl;
					//Has the template size gotten unreasonable?
					mag = getMagnitude();
					if(mag[0] > 75 || mag[0] < 1){
//						std::cout << "Magnitude Limit" << std::endl;
						running = false;
						sk.End();
						pair<stim::fiber<float>, int> a(stim::fiber<float> (cL, cM), -1);
						addToNetwork(a);
						return a;
						break;
					}
					else
					{
						h = selectObject(p, getDirection(), m[0]);
						//Have we hit something previously traced?
						if(h != -1){
							std::cout << "I hit a line" << h << std::endl;
							running = false;
							sk.End();
							pair<stim::fiber<float>, int> a(stim::fiber<float> (cL, cM), -1);
							addToNetwork(a);
							return a;
							break;
						}
						else {  
							cL.push_back(stim::vec<float>(p[0], p[1],p[2]));
							cM.push_back(m[0]);
							sk.TexCoord(m[0]);
							sk.Vertex(p[0], p[1], p[2]);
							Bind(btexbufferID, bfboId, 27);
							CHECK_OPENGL_ERROR
							branchDetection();
							CHECK_OPENGL_ERROR
							Unbind();
							CHECK_OPENGL_ERROR
							
						}
				 	}
                 		}
         		}
		}

			

};
}
#endif