#include <math.h>
#include <stdio.h>
#include "../../common/objects.h"
#include "../../common/vector.h"
#include "../../common/debug.h"
#include "cylinder.h"

extern scene_data* main_scene;
extern int DEBUG_LEVEL;

//extern inline double dot_product(vector* a, vector* b);
//From Graphic Gems IV

double intersect_cylinder(point* origin, vector* projection, object* obj, point* intersection)
{
	int		hit;		/* True if ray intersects cyl	*/
	double		d;		/* Shortest distance between	*/
					/*   the ray and the cylinder	*/
	double		t, s;		/* Distances along the ray	*/
	vector		n, D, O;
	double		ln;
	const	double		pinf = HUGE;	// Positive infinity
	double in=0, out=0;
	vector obj_vector;

	//stuff for the caps
	double plane_distancexxx = 0;
	//double cap_dis = 0;
	double plane_vectorDnorm;
	double projDnorm;
	//vector plane_intersection;
	vector origin_to_plane;
	object top;
	object bottom;

	if(DEBUG_LEVEL == 0)
		hit = 1 + 1;
		
	//main_scene->models[1]->amb.r = 0;
	//main_scene->models[1]->amb.g = 0;
	//main_scene->models[1]->amb.b = 200;

	sub_vectors(&obj_vector, origin, &obj->pos);
	cross_product (projection, &obj->norm, &n);

	if  ( (ln = vector_length (&n)) == 0. ) {	// ray parallel to cyl
		return 0;
	    d	 = dot_product (&obj_vector, &obj->norm);
	    D.x	 = obj_vector.x - d*obj->norm.x;
	    D.y	 = obj_vector.y - d*obj->norm.y;
	    D.z	 = obj_vector.z - d*obj->norm.z;
	    d	 = vector_length (&D);
	    in	 = -pinf;
	    out =  pinf;
	    return (d <= obj->radius);		// true if ray is in cyl
	}

	normalize (&n);
	d    = fabs (dot_product (&obj_vector,&n));		// shortest distance
	hit  = (d <= obj->radius);

	if  (hit) {				// if ray hits cylinder
	    cross_product (&obj_vector,&obj->norm,&O);
	    t = - dot_product (&O,&n) / ln;
	    cross_product (&n,&obj->norm,&O);
	    normalize (&O);
	    s = fabs (sqrt(obj->radius*obj->radius - d*d) / dot_product (projection,&O));
	    in = t - s;			// entering distance
	    out = t + s;			// exiting  distance
	}

	
	//infinite cylinder
	if (obj->dist == 0)
	{
		multiply_vector(intersection, projection, in);
		add_vectors(intersection, intersection, origin);
		return in;
	}

	//make sure the ray hit the cylinder
	if(in < EPSILON && out < EPSILON)
		return 0;
		
	//do the caps



	//create bottom plane
	copy_vector(&obj->pos, &bottom.pos);
	copy_vector(&obj->norm, &bottom.norm);
	invert_vector(&bottom.norm);

	//	Intersect the ray with the bottom end-cap plane.
	sub_vectors(&origin_to_plane, &bottom.pos, origin);
	normalize(&origin_to_plane);
	
	////// plane /////
	projDnorm = dot_product(&bottom.norm, projection);
	if(projDnorm == 0)
		return 0;
	sub_vectors(&origin_to_plane, &bottom.pos, origin);
	plane_vectorDnorm = dot_product(&bottom.norm, &origin_to_plane);
	plane_distancexxx = plane_vectorDnorm / projDnorm;	
	////end plane ////

	if(plane_vectorDnorm > 0)  //on the same side as cylinder section
	{
		if(plane_distancexxx  < in && plane_distancexxx > 0)  //throw this stuff away
		{
			return 0;
		}
	}
	else  //on side without cylinder section
	{
		if(plane_distancexxx > in || plane_distancexxx <= 0)
		{
			if(plane_distancexxx < out && plane_distancexxx > 0)
				in = out;
			else
			{
				return 0;
			}
		}
	}
	
	
	// create top plane
	copy_vector(&obj->norm, &top.norm);
	normalize(&top.norm);
	multiply_vector(&top.pos, &top.norm, obj->dist);
	
	////// plane /////
	projDnorm = dot_product(&top.norm, projection);
	if(projDnorm == 0)
		return 0;
	sub_vectors(&origin_to_plane, &top.pos, origin);
	plane_vectorDnorm = dot_product(&top.norm, &origin_to_plane);
	plane_distancexxx = plane_vectorDnorm / projDnorm;	
	////end plane ////

	if(plane_vectorDnorm > 0)  //on the same side as cylinder section
	{
		if(plane_distancexxx  < in && plane_distancexxx > 0)  //throw this stuff away
		{
			return 0;
		}
	}
	else  //on side without cylinder section
	{
		if(plane_distancexxx > in || plane_distancexxx <= 0)
		{
			if(plane_distancexxx < out && plane_distancexxx > 0)
				in = out;
			else
			{
				return 0;
			}
		}
	}
	
	
	
	
	

	if(in<0 && out > 0)
	{
		//multiply_vector(intersection, projection, out);
		//add_vectors(intersection, intersection, origin);
		return out;
	}
	
	if(in>0)
	{
		//multiply_vector(intersection, projection, in);
		//add_vectors(intersection, intersection, origin);
		return in;
	}
	
	return 0;
}

double testo(point* origin, vector* projection, object* obj, point* intersection)
{
	double a;
	double b;
	double c;
	double determinant;
	double dist;
	double t0, t1;
	double cos_proj_axis;
	vector obj_vector;
	vector temp_projection;

	printd(DEBUG, "{intersect_cylinder\n");
	
	copy_vector(projection, &temp_projection);
	sub_vectors(&obj_vector, origin, &obj->pos);
	
	//map ray to cylinder space
	
	temp_projection.z = 0;
	obj_vector.z = 0;
	
	cos_proj_axis =  dot_product(&obj_vector,projection);

	if (cos_proj_axis == 0)
		return 0;  //parallel

	a = dot_product(&temp_projection, &temp_projection);
	b = 2*dot_product(&temp_projection, &obj_vector);
	c = dot_product(&obj_vector, &obj_vector) - obj->radius;

	determinant = pow(b, 2) - 4*a*c;

	//no hits at all
	if(determinant < 0)
		return 0;

	t0 = (-b + sqrt(determinant)) / 2*a;
	t1 = (-b - sqrt(determinant)) / 2*a;
	
	if(t0 < 0 && t1 <0)	// Can't use negative xIntersects
	{
		return 0;
	}
	else if(t0 > 0 && t1 > 0)	
	{
		if(t0 < t1)
		{
			dist = t0;
		}
		else
		{
			dist = t1;
		}
	}
	else if(t0 > 0)  // If only one is positive, figure out which one
	{
		dist = t0;
	}
	else
	{
		dist = t1;
	}
/*
	if(t0 < EPSILON)
	{
		if(t1 < EPSILON)
			dist = 0;
		else
			dist = t1;
	}
	else if(t1 < EPSILON)  //t0 is >= than EPSILON
	{
		dist = t0;
	}
	else if(t1 < t0)
	{
		dist = t1;
	}
	else
		dist = t0;*/

	multiply_vector(intersection, projection, dist);
	add_vectors(intersection, intersection, origin);
	
	printd(DEBUG, "}intersect_cylinder\n");
	
	return dist;
}




/*****************************************************
 Calculates the normal vector at a point, on an object
 *****************************************************/
vector* normal_cylinder(point* intersection, object* obj, vector* n)
{
	double lengthObjOnNorm;
	double lengthOfNorm;
	vector obj_vector;
	
	printd(DEBUG, " _normal_sphere_hyper\n");
	
	sub_vectors(&obj_vector, intersection, &obj->pos); //points for obj center to intersect
	
	//vector projection
	lengthObjOnNorm = dot_product(&obj_vector,&obj->norm); //cos between obj and norm
	lengthOfNorm = vector_length(&obj->norm);
	multiply_vector(n, &obj->norm, 1/lengthOfNorm);
	multiply_vector(n, &obj->norm, lengthObjOnNorm);
	
	sub_vectors(n,intersection,n);

	return n;
}

