#include "nurbs.h"

//________________________________NURBS FUNCTIONS_______________________________


/* convert nurbs ctrl pt vector from cartesian to homogenous coordinates */
 void cart2homo_vect(nurbs_ctrl_pt* cart, nurbs_ctrl_pt* homo, unsigned int len)
{
	unsigned int i;
	for(i=0; i<len; i++)
	{
		homo[i].x = cart[i].x * cart[i].w;
		homo[i].y = cart[i].y * cart[i].w;
		homo[i].z = cart[i].z * cart[i].w;
	}
}

/* convert nurbs ctrl pt vector from homogenous to cartesian coordinates */
 void homo2cart_vect(nurbs_ctrl_pt* homo, nurbs_ctrl_pt* cart, unsigned int len)
{
	unsigned int i;
	for(i=0; i<len; i++)
	{
		homo[i].x = cart[i].x / cart[i].w;
		homo[i].y = cart[i].y / cart[i].w;
		homo[i].z = cart[i].z / cart[i].w;
	}
}

 void scale_pts(nurbs_ctrl_pt* pt, double C, int len)
{
	int i;
	for(i=0;i<len;i++)
	{
		pt[i].x *= C;
		pt[i].y *= C;
		pt[i].z *= C;
	}
}

/******************************************************************/
/* PUBLIC FUNCTIONS 											  */
/******************************************************************/

void knot_setup_uniform(nurbs_curve* curve)
{
	int i;
	// set up knot vector
	for(i=0;i<curve->nk;i++)
	{
		if(i < (curve->deg+1))
		{
			curve->knots[i] = 0;
		}
		else if(i >= curve->ncp)
		{
			curve->knots[i] = 1;
		}
		else
		{
			/* equally spaced knots */
			curve->knots[i] = (double)(i - curve->deg)/(double)(curve->ncp - curve->deg);
		}

		if(i>0 && i<(curve->nk-1))
		{
			curve->knots_1d[i-1] = curve->knots[i];// sub vector for 1st deriv
		}
		if(i>1 && i<(curve->nk-2))
		{
			curve->knots_2d[i-2] = curve->knots[i];// sub vector for 2nd deriv
		}
		if(i>2 && i<(curve->nk-3))
		{
			curve->knots_3d[i-3] = curve->knots[i];// sub vector for 3rd deriv
		}
	}
}


/*************************************************************************************
* FUNCTION:	nurbs_curve_alloc_file()
* Allocate memory and initialize fields of given nurbs_curve struct
* based on definition contained in .nurbs file
*
* ACCEPTS:
* 	path - string containing path to .nurbs file, if NULL default path is used (./..)
* 	curve - pointer to empty nurbs_curve struct
*
* RETURNS:
* 	curve - allocated and initialized curve struct
*************************************************************************************/
void nurbs_curve_alloc_file(char* path, nurbs_curve* curve)
{
	FILE *fd;
	unsigned int i, dim;
	double b,c,d;

	if(path!=NULL)
		fd = fopen(path,"r"); //open .nurbs file
	else
		fd = fopen("./../curve.nurbs","r"); //open .nurbs file with default path (project directory)

	if(fd==NULL)
	{
		printf("!!! ERROR: could not open .nurbs file !!!\n");
		exit(-1);
	}

	fscanf(fd,"N%u\tL%u\tD%u\n",&(curve->ncp),&(curve->deg), &dim);// read number of ctrl points and curve degree

	curve->control_points = malloc(curve->ncp*sizeof(nurbs_ctrl_pt));
	curve->control_points_1d = malloc((curve->ncp-1)*sizeof(nurbs_ctrl_pt));
	curve->control_points_2d = malloc((curve->ncp-2)*sizeof(nurbs_ctrl_pt));
	curve->control_points_3d = malloc((curve->ncp-3)*sizeof(nurbs_ctrl_pt));
	curve->tmp_pts = malloc((curve->deg+1)*sizeof(nurbs_ctrl_pt));
	curve->tmp_pts_1d = malloc((curve->deg)*sizeof(nurbs_ctrl_pt));
	curve->tmp_pts_2d = malloc((curve->deg-1)*sizeof(nurbs_ctrl_pt));
	curve->tmp_pts_3d = malloc((curve->deg-2)*sizeof(nurbs_ctrl_pt));
	curve->ctp_u = malloc(curve->ncp*sizeof(nurbs_ctrl_pt));
	curve->nk = curve->ncp + curve->deg + 1;
	curve->knots = malloc(curve->nk*sizeof(double));
	curve->knots_1d = malloc((curve->nk-2)*sizeof(double));
	curve->knots_2d = malloc((curve->nk-4)*sizeof(double));
	curve->knots_3d = malloc((curve->nk-6)*sizeof(double));
	curve->kL = malloc((curve->nk)*sizeof(double));
	curve->nsubk = NUM_SUBKL*(curve->nk - 2*curve->deg - 1)+1;
	curve->subku = malloc((curve->nsubk)*sizeof(double));
	curve->subkL = malloc((curve->nsubk)*sizeof(double));

	// read control points from file
    if(dim == 2){
        for(i=0;i<(curve->ncp);i++)
        {
            fscanf(fd,"X%lf\tY%lf\tP%lf\n",
                    &(curve->control_points[i].x),
                    &(curve->control_points[i].y),
                    &(curve->control_points[i].w)
                    );
            curve->control_points[i].z = 0.0;
        }
    }
    else if(dim == 3){
        for(i=0;i<(curve->ncp);i++)
        {
            fscanf(fd,"X%lf\tY%lf\tZ%lf\tP%lf\n",
                    &(curve->control_points[i].x),
                    &(curve->control_points[i].y),
                    &(curve->control_points[i].z),
                    &(curve->control_points[i].w)
                    );
        }
    }
	

	//scale_pts(curve->control_points, NURBS_SCALE, curve->ncp);
	cart2homo_vect(curve->control_points, curve->control_points, curve->ncp);

	knot_setup_uniform(curve);

	for(i=0;i<(curve->ncp-1);i++)
	{
		b = ((double)curve->deg)/(curve->knots[curve->deg + i + 1] - curve->knots[i+1]);
		curve->control_points_1d[i].x = b*(curve->control_points[i+1].x - curve->control_points[i].x);
		curve->control_points_1d[i].y = b*(curve->control_points[i+1].y - curve->control_points[i].y);
		curve->control_points_1d[i].z = b*(curve->control_points[i+1].z - curve->control_points[i].z);
		curve->control_points_1d[i].w = b*(curve->control_points[i+1].w - curve->control_points[i].w);
	}

	for(i=0;i<(curve->ncp-2);i++)
	{
		c = ((double)(curve->deg-1))/(curve->knots_1d[curve->deg + i] - curve->knots_1d[i+1]);
		curve->control_points_2d[i].x = c*(curve->control_points_1d[i+1].x - curve->control_points_1d[i].x);
		curve->control_points_2d[i].y = c*(curve->control_points_1d[i+1].y - curve->control_points_1d[i].y);
		curve->control_points_2d[i].z = c*(curve->control_points_1d[i+1].z - curve->control_points_1d[i].z);
		curve->control_points_2d[i].w = c*(curve->control_points_1d[i+1].w - curve->control_points_1d[i].w);
	}

	for(i=0;i<(curve->ncp-3);i++)
	{
		d = ((double)(curve->deg-2))/(curve->knots_2d[curve->deg + i - 1] - curve->knots_2d[i+1]);
		curve->control_points_3d[i].x = d*(curve->control_points_2d[i+1].x - curve->control_points_2d[i].x);
		curve->control_points_3d[i].y = d*(curve->control_points_2d[i+1].y - curve->control_points_2d[i].y);
		curve->control_points_3d[i].z = d*(curve->control_points_2d[i+1].z - curve->control_points_2d[i].z);
		curve->control_points_3d[i].w = d*(curve->control_points_2d[i+1].w - curve->control_points_2d[i].w);
	}

	fclose(fd);
}

/*************************************************************************************
* FUNCTION:	nurbs_curve_free()
* Free memory in nurbs curve struct
*
* ACCEPTS:
* 	curve - pointer to allocated nurbs_curve struct
*
* RETURNS:
* 	nothing
*************************************************************************************/
void nurbs_curve_free(nurbs_curve* curve)
{
	free(curve->control_points);
	free(curve->control_points_1d);
	free(curve->control_points_2d);
	free(curve->control_points_3d);
	free(curve->tmp_pts);
	free(curve->tmp_pts_1d);
	free(curve->tmp_pts_2d);
	free(curve->tmp_pts_3d);
	free(curve->knots);
	free(curve->knots_1d);
	free(curve->knots_2d);
	free(curve->knots_3d);
	free(curve->kL);
	free(curve->subku);
	free(curve->subkL);
}

/*************************************************************************************
* FUNCTION:	nurbs_find_span()
* Finds lower index of the knot span containing the given u parameter
* uses binary search alogrithm
*
* ACCEPTS:
* 	n - number of ctrl. pts. - upper search limit +1
* 	p - degree of curve - bottom search limit
* 	u - parameter at which to evaluate the curve
* 	*U - knot array
*
* RETURNS:
* 	(FUN) lower index of found knot span
*************************************************************************************/
int nurbs_find_span (int n, int p, double u, double *U)
{

	int low, high, mid;
	if (u >= U[n+1]) // safety >= check
	{
		return (n);
	}
	low = p;
	high = n+1;
	mid = (low + high)/2;
	while (u < U[mid] || u >= U[mid+1])
	{
		if ( u < U[mid])
		{ high = mid;
		}
		else
		{ low = mid;
		}
		mid = (low + high)/2;
	}

	return (mid);
}

double nurbs_arc_len(double u,void* data)
{
	cart_point Pt, dPt;
	nurbs_curve* curve = data;
	nurbs_deBoor_eval(u,-1,curve,&Pt,&dPt,NULL,NULL);
	return(sqrt(dPt.x*dPt.x+dPt.y*dPt.y+dPt.z*dPt.z));
}

/*****************************************************************************************
* FUNCTION:	nurbs_deBoor_eval()
* Finds point on the curve corresponding to given parameter u
* uses the fast and numerically stable DeBoor algorithm
* see: http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/de-Boor.html
*
* if dPt and/or ddPt pointers are given (not NULL) this function also evaluates
* 1st and 2nd derivatives
*
* Derivatives are computed by generating derivative B-Splines in homogenous coordinates
* then evaluating them using deBoor alg. and then applying formula 4.8, p.125 "NURBS Book"
*
* ACCEPTS:
* 	u - parameter at which to evaluate the curve
* 	ek - externally given knot span if we dont have to find it here
* 	curve - structure containing all curve data
*
* RETURNS:
* 	Pt - structure containing cordinates of the evaluated point
* 	dPt - 1st derivative at u
* 	ddPt - 2nd derivative at u
******************************************************************************************/
void nurbs_deBoor_eval(
		double u,			// parameter at which to evaluate curve
		int ek,				// externally given knot span (if -1 find it here)
		nurbs_curve* curve, // NURBS curve to be evaluated
		cart_point* Pt,		// evaluated point
		cart_point* dPt,	// evaluated point of 1st derivative
		cart_point* ddPt,	// evaluated point of 2nd derivative
		cart_point* dddPt	// evaluated point of 3rd derivative
		)
{
	int r,i;
	int k = ek;
	int dk,ddk,dddk;
	unsigned int deg = curve->deg;					// degree of curve
	unsigned int degd = deg-1;						// degree of curve
	unsigned int degdd = deg-2;						// degree of curve
	unsigned int degddd = deg-3;					// degree of curve
	//unsigned int nk = curve->nk;					// number of knots
	double* knots = curve->knots;					// knot vector
	double* knots_1d = curve->knots_1d;				// knot vector
	double* knots_2d = curve->knots_2d;				// knot vector
	double* knots_3d = curve->knots_3d;				// knot vector
	nurbs_ctrl_pt* tmp_pts = curve->tmp_pts;		// ptr to workspace for affected ctrl points
	nurbs_ctrl_pt* tmp_pts_1d = curve->tmp_pts_1d;	// ptr to workspace for affected ctrl points
	nurbs_ctrl_pt* tmp_pts_2d = curve->tmp_pts_2d;	// ptr to workspace for affected ctrl points
	nurbs_ctrl_pt* tmp_pts_3d = curve->tmp_pts_3d;	// ptr to workspace for affected ctrl points
	double c;

	/* if we're at the end just give the last ctrl. pts. */
	if(u>=1.0)
	{
		Pt->x = curve->control_points[curve->ncp-1].x;
		Pt->y = curve->control_points[curve->ncp-1].y;
		Pt->z = curve->control_points[curve->ncp-1].z;
		if(dPt != NULL)
		{
			dPt->x = curve->control_points_1d[curve->ncp-2].x;
			dPt->y = curve->control_points_1d[curve->ncp-2].y;
			dPt->z = curve->control_points_1d[curve->ncp-2].z;
		}
		if(ddPt != NULL)
		{
			ddPt->x = curve->control_points_2d[curve->ncp-3].x;
			ddPt->y = curve->control_points_2d[curve->ncp-3].y;
			ddPt->z = curve->control_points_2d[curve->ncp-3].z;
		}
		if(dddPt != NULL)
		{
			dddPt->x = curve->control_points_3d[curve->ncp-4].x;
			dddPt->y = curve->control_points_3d[curve->ncp-4].y;
			dddPt->z = curve->control_points_3d[curve->ncp-4].z;
		}
		return;
	}


	//check if k is given externally, if not find it here
	if(k<0)
	{
		// locate the lower index k of knot span [u_k,u_k+1) in which the current u is located
		k = nurbs_find_span(curve->ncp, deg, u, knots);
	}
	dk  = k-1;
	ddk = k-2;
	dddk = k-3;

	// All points have been already converted to homogenous coordinates during NURBS struct initialization

	// copy the affected deg+1 control points to workspace, these are P(k-deg), P(k-deg+1), ...,P(k)
	memcpy(tmp_pts, &(curve->control_points[k-deg]), (deg+1)*sizeof(nurbs_ctrl_pt));


	//------------------------------------------------------------------------------------------------------
	// compute value of NURBS at u using deBoor algorithm
	//------------------------------------------------------------------------------------------------------
	for(r=0; r<deg; r++)
	{
		for(i=deg; i>r; i--)
		{
			c = (u - knots[k-deg+i])/(knots[k+i-r] - knots[k-deg+i]);
			tmp_pts[i].x = (1 - c)*tmp_pts[i-1].x + c*tmp_pts[i].x;
			tmp_pts[i].y = (1 - c)*tmp_pts[i-1].y + c*tmp_pts[i].y;
			tmp_pts[i].z = (1 - c)*tmp_pts[i-1].z + c*tmp_pts[i].z;
			tmp_pts[i].w = (1 - c)*tmp_pts[i-1].w + c*tmp_pts[i].w;
		}
	}

	// write cartesian coords of homogenous point evaluated at u
	Pt->x = tmp_pts[deg].x / tmp_pts[deg].w;
	Pt->y = tmp_pts[deg].y / tmp_pts[deg].w;
	Pt->z = tmp_pts[deg].z / tmp_pts[deg].w;



	//------------------------------------------------------------------------------------------------------
	// compute NURBS 1st derivative if  required (dPt!=NULL)
	//------------------------------------------------------------------------------------------------------
	if(dPt != NULL)
	{
		// we use deBoor alg. on homogenous b-spline derivative ctrl. pts
		memcpy(tmp_pts_1d, &(curve->control_points_1d[dk-degd]), (degd+1)*sizeof(nurbs_ctrl_pt));
		for(r=0; r<degd; r++)
		{
			for(i=degd; i>r; i--)
			{
				c = (u - knots_1d[dk-degd+i])/(knots_1d[dk+i-r] - knots_1d[dk-degd+i]);
				tmp_pts_1d[i].x = (1 - c)*tmp_pts_1d[i-1].x + c*tmp_pts_1d[i].x;
				tmp_pts_1d[i].y = (1 - c)*tmp_pts_1d[i-1].y + c*tmp_pts_1d[i].y;
				tmp_pts_1d[i].z = (1 - c)*tmp_pts_1d[i-1].z + c*tmp_pts_1d[i].z;
				tmp_pts_1d[i].w = (1 - c)*tmp_pts_1d[i-1].w + c*tmp_pts_1d[i].w;
			}
		}

		// derivatives of rational functions are computed by Leibnitz rule: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
		// that's why we can't just divide points by weights besides the fact that some of them are 0
		// compute NURBS derivative in cartesian coordinates using formula (4.7) from NURBS Book p.125

		dPt->x = (tmp_pts_1d[degd].x - tmp_pts_1d[degd].w*Pt->x)/(tmp_pts[deg].w);
		dPt->y = (tmp_pts_1d[degd].y - tmp_pts_1d[degd].w*Pt->y)/(tmp_pts[deg].w);
		dPt->z = (tmp_pts_1d[degd].z - tmp_pts_1d[degd].w*Pt->z)/(tmp_pts[deg].w);
	}


	//------------------------------------------------------------------------------------------------------
	// compute NURBS 2nd derivative if  required (ddPt!=NULL)
	//------------------------------------------------------------------------------------------------------
	if(ddPt != NULL)
	{
		memcpy(tmp_pts_2d, &(curve->control_points_2d[ddk-degdd]), (degdd+1)*sizeof(nurbs_ctrl_pt));
		for(r=0; r<degdd; r++)
		{
			for(i=degdd; i>r; i--)
			{
				c = (u - knots_2d[ddk-degdd+i])/(knots_2d[ddk+i-r] - knots_2d[ddk-degdd+i]);
				tmp_pts_2d[i].x = (1 - c)*tmp_pts_2d[i-1].x + c*tmp_pts_2d[i].x;
				tmp_pts_2d[i].y = (1 - c)*tmp_pts_2d[i-1].y + c*tmp_pts_2d[i].y;
				tmp_pts_2d[i].z = (1 - c)*tmp_pts_2d[i-1].z + c*tmp_pts_2d[i].z;
				tmp_pts_2d[i].w = (1 - c)*tmp_pts_2d[i-1].w + c*tmp_pts_2d[i].w;
			}
		}

		// second derivatives are even more complicated
		// we use NURBS book general formula (4.8) p. 125

		ddPt->x = (tmp_pts_2d[degdd].x - (2*tmp_pts_1d[degd].w*dPt->x + tmp_pts_2d[degdd].w*Pt->x))/(tmp_pts[deg].w);
		ddPt->y = (tmp_pts_2d[degdd].y - (2*tmp_pts_1d[degd].w*dPt->y + tmp_pts_2d[degdd].w*Pt->y))/(tmp_pts[deg].w);
		ddPt->z = (tmp_pts_2d[degdd].z - (2*tmp_pts_1d[degd].w*dPt->z + tmp_pts_2d[degdd].w*Pt->z))/(tmp_pts[deg].w);
	}
	//------------------------------------------------------------------------------------------------------
	// compute NURBS 3rd derivative if  required (dddPt!=NULL)
	//------------------------------------------------------------------------------------------------------
	if(dddPt != NULL)
	{
		memcpy(tmp_pts_3d, &(curve->control_points_3d[dddk-degddd]), (degddd+1)*sizeof(nurbs_ctrl_pt));
		for(r=0; r<degddd; r++)
		{
			for(i=degddd; i>r; i--)
			{
				c = (u - knots_3d[dddk-degddd+i])/(knots_3d[dddk+i-r] - knots_3d[dddk-degddd+i]);
				tmp_pts_3d[i].x = (1 - c)*tmp_pts_3d[i-1].x + c*tmp_pts_3d[i].x;
				tmp_pts_3d[i].y = (1 - c)*tmp_pts_3d[i-1].y + c*tmp_pts_3d[i].y;
				tmp_pts_3d[i].z = (1 - c)*tmp_pts_3d[i-1].z + c*tmp_pts_3d[i].z;
				tmp_pts_3d[i].w = (1 - c)*tmp_pts_3d[i-1].w + c*tmp_pts_3d[i].w;
			}
		}

		// thrid derivatives are even more complicated
		// we use NURBS book general formula (4.8) p. 125

		dddPt->x = (tmp_pts_3d[degddd].x - (3*tmp_pts_1d[degd].w*ddPt->x + 3*tmp_pts_2d[degdd].w*dPt->x + tmp_pts_3d[degddd].w*Pt->x))/(tmp_pts[deg].w);
		dddPt->y = (tmp_pts_3d[degddd].y - (3*tmp_pts_1d[degd].w*ddPt->y + 3*tmp_pts_2d[degdd].w*dPt->y + tmp_pts_3d[degddd].w*Pt->y))/(tmp_pts[deg].w);
		dddPt->z = (tmp_pts_3d[degddd].z - (3*tmp_pts_1d[degd].w*ddPt->z + 3*tmp_pts_2d[degdd].w*dPt->z + tmp_pts_3d[degddd].w*Pt->z))/(tmp_pts[deg].w);
	}
}

double adaptiveSimpsonsAux(double (*f)(double,void*), void* data, double a, double b, double epsilon,
                         double S, double fa, double fb, double fc, int bottom)
{
  double c = (a + b)/2, h = b - a;
  double d = (a + c)/2, e = (c + b)/2;
  double fd = f(d,data), fe = f(e,data);
  double Sleft = (h/12)*(fa + 4*fd + fc);
  double Sright = (h/12)*(fc + 4*fe + fb);
  double S2 = Sleft + Sright;
  if (bottom <= 0 || fabs(S2 - S) <= 15*epsilon)
    return S2 + (S2 - S)/15;
  return adaptiveSimpsonsAux(f, data, a, c, epsilon/2, Sleft,  fa, fc, fd, bottom-1) +
         adaptiveSimpsonsAux(f, data, c, b, epsilon/2, Sright, fc, fb, fe, bottom-1);
}

//
// Adaptive Simpson's Rule
//
double adaptiveSimpsons(double (*f)(double,void*), void* data,   // ptr to function
                           double a, double b,  // interval [a,b]
                           double epsilon,  // error tolerance
                           int maxRecursionDepth)// recursion cap
{
  double c = (a + b)/2, h = b - a;
  double fa = f(a,data), fb = f(b,data), fc = f(c,data);
  double S = (h/6)*(fa + 4*fc + fb);
  return adaptiveSimpsonsAux(f, data, a, b, epsilon, S, fa, fb, fc, maxRecursionDepth);
}
