#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/cost.h"
#include <stim/cuda/cudatools/glbind.h>
#include <stim/visualization/obj.h>
#include <vector>

#include <iostream>
#include <fstream>



/* Technically since gl_spider inherits from gl_texture, we could
	call the init with a path to an image stack, and upload
	the images while creating the spider (calling init) */
namespace stim
{

template<typename T>
class gl_spider
{
	//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;
		std::vector<stim::vec<float> > pV;
		std::vector<stim::vec<float> > mV;
		//currentTransform
		stim::matrix<float, 4> cT;
		GLuint texID;
		stim::vec<float> S;
		stim::vec<float> R;
		cudaGraphicsResource_t resource;

		GLuint dList;
		GLuint fboID;
		GLuint texbufferID;
		int numSamples;
		float stepsize = 3.0;
		int current_cost;

		/// 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();
			//next = (cT*next);
			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()
		{
			Bind();
			setMatrix();
			glCallList(dList+3);
			
		//	int best = getCost();
			
		}


		
		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 = 3*M_PI/2)
		{
			//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)
		{
			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);
		}


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


//--------------------------------------------------------------------------//
//--------------------------------CUDA METHODS------------------------------//
//--------------------------------------------------------------------------//
		
		/// Method for registering the texture with Cuda for shared
		///	access.
		void
		createResource()
		{
			HANDLE_ERROR(
				cudaGraphicsGLRegisterImage(
					 &resource,
				 	texbufferID,
				 	GL_TEXTURE_2D,
				 	//CU_GRAPHICS_REGISTER_FLAGS_NONE)
					cudaGraphicsMapFlagsReadOnly)
			);
		} 
		
		///Method for freeing the texture from Cuda for gl access.
		void
		destroyResource()
		{
			HANDLE_ERROR(
				cudaGraphicsUnregisterResource(resource)
			);		
		}

		///Entry-point into the cuda code for calculating the cost
		///	of a given samples array (in texture form) 
		int
		getCost()
		{
			createResource();
			stim::vec<int> cost = 	get_cost(resource, numSamples);
			destroyResource();
//			if (cost[1] >= 80)
//				exit(0);
			current_cost = cost[1];
			return cost[0];
		}

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

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

		///@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);
			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();
			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;
	//		m[2] = 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;
		}
		
		///Function to get back the framebuffer Object attached to the spider.
		///For external access.
		GLuint
		getFB()
		{
			return fboID;
		}

		///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 Unbinding all of the texture resources
		void
		Unbind()
		{
			//Finalize GL_buffer
			glBindTexture(GL_TEXTURE_3D, 0);                      
			glDisable(GL_TEXTURE_3D);
			glBindFramebuffer(GL_FRAMEBUFFER,0);
			glBindTexture(GL_TEXTURE_2D, 0);
		}
//--------------------------------------------------------------------------//
//-----------------------------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();
			findOptimalDirection();
			findOptimalPosition();
			findOptimalScale();
			branchDetection();
		//	Unbind();
			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()
		{	 
			 Bind();
			 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.5;
			 float step = 360.0/numSamples;
		 	 int j = 0;
			 glEnable(GL_TEXTURE_3D);
			 glBindTexture(GL_TEXTURE_3D, texID);
			 glBegin(GL_QUAD_STRIP);
			 	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*0.1+0.1);
					 glTexCoord3f(x,y,z1); 
					 glVertex2f(16.0, j*0.1+0.1);
					 glTexCoord3f(xold,yold,z1); 
					 glVertex2f(16.0, j*0.1); 
					 glTexCoord3f(xold,yold,z0); 
					 glVertex2f(0.0, j*0.1);
					 xold=x;
					 yold=y;
					 j++;
				}
		      	 glEnd();  
			 glEndList();
			 Unbind();
		}
};
}
#endif