/**
 * $Id:$
 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 *
 * The contents of this file may be used under the terms of either the GNU
 * General Public License Version 2 or later (the "GPL", see
 * http://www.gnu.org/licenses/gpl.html ), or the Blender License 1.0 or
 * later (the "BL", see http://www.blender.org/BL/ ) which has to be
 * bought from the Blender Foundation to become active, in which case the
 * above mentioned GPL option does not apply.
 *
 * The Original Code is Copyright (C) 1997 by Ton Roosendaal, Frank van Beek and Joeri Kassenaar.
 * All rights reserved.
 *
 * The Original Code is: all of this file.
 *
 * Contributor(s): none yet.
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */

/*

An Algorithm for Automatically Fitting Digitized Curves
by Philip J. Schneider
from "Graphics Gems", Academic Press, 1990

ADAPTED: 5-93,  NeoGeo

*/

/* mogelijke verbeteringen:
 * 
 * - punten opnieuw parametriseren (constante afstand)
 * - interactief knippen
 * - handles beter berekenen.
 * 
 * - standaard een aantal keer itereren geeft betere resultaten.
 */

/*  fit_cubic.c	*/									
/*	Piecewise cubic fitting code	*/

#include <local/Trace.h>
#undef ABS
#undef SGN
#undef SWAP
#undef PI
#undef TRUE
#undef FALSE

#include "/usr/people/insane/public/gems1/GraphicsGems.h"
#include <stdio.h>
#include <math.h>

/* #include <sys/sysmips.h>	*/ /* voor fp-exeption trap */

typedef Point2 *BezierCurve;

/* Forward declarations */
void				FitCurve();
static	void		FitCubic();
static	double		*Reparameterize();
static	double		NewtonRaphsonRootFind();
static	Point2		Bezier();
static	double 		B0(), B1(), B2(), B3();
static	Vector2		ComputeLeftTangent();
static	Vector2		ComputeRightTangent();
static	Vector2		ComputeCenterTangent();
void				ComputeCenterTangents();
static	double		ComputeMaxError();
static	double		*ChordLengthParameterize();
static	BezierCurve	GenerateBezier();
static	Vector2		V2AddII();
static	Vector2		V2ScaleII();
static	Vector2		V2SubII();

#define MAXPOINTS	1000		/* The most points you can have */

#define SPLT_ERR	0	/* grootste error */
#define SPLT_MID	1	/* in het midden */
#define SPLT_EXT	2	/* op extremen */
#define SPLT_BND	3	/* op maximale buiging */

#define DFLT_ERROR 8.0

int splitmode = SPLT_EXT;
int optim = TRUE;
long segcount=0;
float *BezierData=0;


void AddBezierSeg(curve)
BezierCurve curve;
{
	float *fp;
	long i;
	
	fp= BezierData+ 3*segcount*2;
	
	for(i = 0; i < 4; i++, fp+=2) {
	
		fp[0] = curve[i].x;
		fp[1] = curve[i].y;
	}
	
	if(3*segcount<MAXPOINTS) segcount++;
	
}

/*
 *  main:
 *	(Dit is veranderd)
 
	aanroepen als:
	FitCurve(d, i, error, type, out);
		d: doubles
		i: aantal
		error: oppervlakte
		type: 0= Bezier,  1= BezTriples
		out: pointerpointer naar output
		
	er wordt een datablok gemaakt van curvesegmenten.
	deze worden ahv type omgezet en als pointer in out geschreven

*/


void FitCurve(d, nPts, error, type, out)
Point2	*d;			/*  Array of digitized points	*/
int		nPts;		/*  Number of digitized points	*/
double	error;		/*  User-defined error squared	*/
long type;
long **out;
{
    Vector2	tHat1, tHat2;	/*  Unit tangent vectors at endpoints */
	struct Bezier *bez;
	float *data, *fp;
	long a;
	
	
	/* globaal array */
	BezierData= callocN(MAXPOINTS*2*4+16, "FitCurve");

    tHat1 = ComputeLeftTangent(d, 0);
    tHat2 = ComputeRightTangent(d, nPts - 1);
    FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
	
	if(segcount) {
		if(type==0) {
			bez= callocN(sizeof(struct Bezier)+3*3*4*(segcount+10), "Fitcurve2");
			bez->cp= segcount+1;
			data= (float *)(bez+1);
			fp= BezierData;
			
			/* eerste handle */
			data[0]= 2*fp[0]-fp[2];
			data[1]= 2*fp[1]-fp[3];
			data+= 3;
			for(a=1; a<3*bez->cp; a++) {
				data[0]= fp[0];
				data[1]= fp[1];
				data+= 3;
				fp+= 2;
			}
			fp-= 2;
			data[0]= 2*fp[0]-fp[-2];
			data[1]= 2*fp[1]-fp[-1];
			
			*out= (long *)bez;
		}
	}
	else *out= 0;
	
	freeN(BezierData);
}



/*
 *  FitCubic :
 *  	Fit a Bezier curve to a (sub)set of digitized points
 */
 
static void FitCubic(d, first, last, tHat1, tHat2, error)
    Point2	*d;					/*  Array of digitized points */
    int		first, last;		/* Indices of first and last pts in region */
    Vector2	tHat1, tHat2;		/* Unit tangent vectors at endpoints */
    double	error;				/*  User-defined error squared	   */
{
    BezierCurve	bezCurve;		/*Control points of fitted Bezier curve*/
    double	*u;					/*  Parameter values for point  */
    double	*uPrime;			/*  Improved parameter values */
    double	maxError;			/*  Maximum fitting error	 */
    int		splitPoint;			/*  Point to split point set at	 */
    int		nPts;				/*  Number of points in subset  */
    double	iterationError;		/*Error below which you try iterating  */
    int		maxIterations = 4;	/*  Max times to try iterating  */
    Vector2	tHatCenter;   		/* Unit tangent vector at splitPoint */
	Vector2 tHatLeft,tHatRight;	/* om vector handles mogelijk te maken */
    int		i;

    iterationError = error * error;
    nPts = last - first + 1;

    /*  Use heuristic if region only has two points in it */
    if (nPts == 2) {
	    double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;

		bezCurve = (Point2 *)mallocN(4 * sizeof(Point2), "FitCubic");
		bezCurve[0] = d[first];
		bezCurve[3] = d[last];
		V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
		V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
		AddBezierSeg(bezCurve);
		freeN(bezCurve);
		return;
    }

    /*  Parameterize points, and attempt to fit curve */
    u = ChordLengthParameterize(d, first, last);
    bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);

    /*  Find max deviation of points to fitted curve */
    maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);

	if (maxError < error) {
		AddBezierSeg(bezCurve);
		freeN(bezCurve);
		freeN(u);
		return;
    }
	
    /*  If error not too large, try some reparameterization  */
    /*  and iteration */
    if (maxError < iterationError) {
		for (i = 0; i < maxIterations; i++) {
	    	uPrime = Reparameterize(d, first, last, u, bezCurve);
			freeN(bezCurve);
	    	bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
	    	maxError = ComputeMaxError(d, first, last, bezCurve, uPrime, &splitPoint);
	    	if (maxError < error) {
				AddBezierSeg(bezCurve);
				freeN(bezCurve);
				freeN(u);
				freeN(uPrime);
				return;
			}
			freeN((char *)u);
			u = uPrime;
		}
	}
	freeN(bezCurve);
	freeN(u);
	
    /* Fitting failed -- split at max error point and fit recursively */
	
	if (splitmode != SPLT_ERR) splitPoint = (first + last) / 2;
	if (splitmode == SPLT_EXT) {
		/* zoek naar extreem (x of y) in spline */
		/* degene het dichst bij het midden wint */
		/* misschien dat ook naar 45 gekeken kan worden */
		
		char * splitarr;	/* jes or no split */
		long i, c;
		long dist, mdist, best;
		float dx, dy;
		
		splitarr = (char *) mallocN(nPts, "FitCubic2");
		
		for (i = first; i < last; i ++){
			c = 0;
			dx = d[i].x - d[i + 1].x;
			dy = d[i].y - d[i + 1].y;
			if (dx < 0.0) c |= 1;
			else if (dx > 0.0) c |= 2;
			if (dy < 0.0) c |= 4;
			else if (dy > 0.0) c |= 8;
			/*if (fabsf(dx) < fabsf(dy)) c |= 16;*/
			splitarr[i - first] = c;
		}
		
		/* zoek naar omslagpunten */
		for (i = 0; i < nPts - 1; i++) splitarr[i] ^= splitarr[i + 1];
		
		best = splitPoint; mdist = 0xffffff;
		/* eventueel nog optimaliseren naar meerdere omslagen na elkaar ?? */
		for (i = 1; i < nPts - 3; i++) {
			if (splitarr[i]) {
				dist = (i + first + 1) - splitPoint;
				if (dist < 0) dist = -dist;
				if (dist < mdist) {
					best = i + first + 1;
					mdist = dist;
				}
			}
		}
		splitPoint = best;
		freeN(splitarr);
	}
	
/*
    tHatCenter = ComputeCenterTangent(d, splitPoint);
    FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
    V2Negate(&tHatCenter);
    FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
*/
    ComputeCenterTangents(d, splitPoint, &tHatLeft, &tHatRight);
    FitCubic(d, first, splitPoint, tHat1, tHatLeft, error);
    FitCubic(d, splitPoint, last, tHatRight, tHat2, error);
}


/*
 *  GenerateBezier :
 *  Use least-squares method to find Bezier control points for region.
 *
 */
static BezierCurve  GenerateBezier(d, first, last, uPrime, tHat1, tHat2)
    Point2	*d;				/*  Array of digitized points	*/
    int		first, last;	/*  Indices defining region	*/
    double	*uPrime;		/*  Parameter values for region */
    Vector2	tHat1, tHat2;	/*  Unit tangents at endpoints	*/
{
    int 	i;
    Vector2 A[MAXPOINTS][2];		/* Precomputed rhs for eqn	*/
    int 	nPts;					/* Number of pts in sub-curve */
    double 	C[2][2];				/* Matrix C		*/
    double 	X[2];					/* Matrix X			*/
    double 	det_C0_C1,				/* Determinants of matrices	*/
    	   	det_C0_X,
	   		det_X_C1;
    double 	alpha_l,				/* Alpha values, left and right	*/
    	   	alpha_r;
    Vector2 	tmp;				/* Utility variable		*/
    BezierCurve	bezCurve;			/* RETURN bezier curve ctl pts	*/

    bezCurve = (Point2 *)mallocN(4 * sizeof(Point2), "Generate Bezier");
    nPts = last - first + 1;

 
    /* Compute the A's	*/
    for (i = 0; i < nPts; i++) {
		Vector2		v1, v2;

		v1 = tHat1; v2 = tHat2;
		V2Scale(&v1, B1(uPrime[i]));
		V2Scale(&v2, B2(uPrime[i]));
		A[i][0] = v1;
		A[i][1] = v2;
    }

    /* Create the C and X matrices	*/
    C[0][0] = C[0][1] = C[1][0] = C[1][1] = 0.0;
    X[0]    = X[1]    = 0.0;

    for (i = 0; i < nPts; i++) {
        C[0][0] += V2Dot(&A[i][0], &A[i][0]);
		C[1][0] = C[0][1] += V2Dot(&A[i][0], &A[i][1]);
		C[1][1] += V2Dot(&A[i][1], &A[i][1]);

		tmp = V2SubII(d[first + i],
	        V2AddII(
	          V2ScaleII(d[first], B0(uPrime[i])),
		    	V2AddII(
		      		V2ScaleII(d[first], B1(uPrime[i])),
		        			V2AddII(
	                  		V2ScaleII(d[last], B2(uPrime[i])),
	                    		V2ScaleII(d[last], B3(uPrime[i]))))));
	

		X[0] += V2Dot(&A[i][0], &tmp);
		X[1] += V2Dot(&A[i][1], &tmp);
    }
	
    /* Compute the determinants of C and X	*/
    det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
    det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
    det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];

    /* Finally, derive alpha values	*/
    if (det_C0_C1 == 0.0) {
		det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
    }
	
	/*if (det_C0_C1) {*/
		alpha_l = det_X_C1 / det_C0_C1;
		alpha_r = det_C0_X / det_C0_C1;
	/*} else alpha_l = -1.0;*/

    /*  If alpha negative, use the Wu/Barsky heuristic (see text) */
    if (alpha_l < 0.0 || alpha_r < 0.0) {
		double	dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;

		bezCurve[0] = d[first];
		bezCurve[3] = d[last];
		V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
		V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
		return (bezCurve);
    }

    /*  First and last control points of the Bezier curve are */
    /*  positioned exactly at the first and last data points */
    /*  Control points 1 and 2 are positioned an alpha distance out */
    /*  on the tangent vectors, left and right, respectively */
    bezCurve[0] = d[first];
    bezCurve[3] = d[last];
    V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
    V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
    return (bezCurve);
}


/*
 *  Reparameterize:
 *	Given set of points and their parameterization, try to find
 *   a better parameterization.
 *
 */
static double *Reparameterize(d, first, last, u, bezCurve)
    Point2	*d;				/*  Array of digitized points	*/
    int		first, last;	/*  Indices defining region	*/
    double	*u;				/*  Current parameter values	*/
    BezierCurve	bezCurve;	/*  Current fitted curve	*/
{
    int 	nPts = last-first+1;	
    int 	i;
    double	*uPrime;		/*  New parameter values	*/

    uPrime = (double *)mallocN(nPts * sizeof(double), "Reparametr");
    for (i = first; i <= last; i++) {
		uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-first]);
    }
    return (uPrime);
}



/*
 *  NewtonRaphsonRootFind :
 *	Use Newton-Raphson iteration to find better root.
 */
static double NewtonRaphsonRootFind(Q, P, u)
    BezierCurve	Q;			/*  Current fitted curve	*/
    Point2 		P;		/*  Digitized point		*/
    double 		u;		/*  Parameter value for "P"	*/
{
    double 		numerator, denominator;
    Point2 		Q1[3], Q2[2];	/*  Q' and Q''			*/
    Point2		Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q''	*/
    double 		uPrime;		/*  Improved u			*/
    int 		i;
    
    /* Compute Q(u)	*/
    Q_u = Bezier(3, Q, u);
    
    /* Generate control vertices for Q'	*/
    for (i = 0; i <= 2; i++) {
		Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
		Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
    }
    
    /* Generate control vertices for Q'' */
    for (i = 0; i <= 1; i++) {
		Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
		Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
    }
    
    /* Compute Q'(u) and Q''(u)	*/
    Q1_u = Bezier(2, Q1, u);
    Q2_u = Bezier(1, Q2, u);
    
    /* Compute f(u)/f'(u) */
    numerator = (Q_u.x - P.x) * (Q1_u.x) + (Q_u.y - P.y) * (Q1_u.y);
    denominator = (Q1_u.x) * (Q1_u.x) + (Q1_u.y) * (Q1_u.y) +
		      	  (Q_u.x - P.x) * (Q2_u.x) + (Q_u.y - P.y) * (Q2_u.y);
    
    /* u = u - f(u)/f'(u) */
    uPrime = u - (numerator/denominator);
    return (uPrime);
}

	
		       
/*
 *  Bezier :
 *  	Evaluate a Bezier curve at a particular parameter value
 * 
 */
static Point2 Bezier(degree, V, t)
    int		degree;		/* The degree of the bezier curve	*/
    Point2 	*V;		/* Array of control points		*/
    double 	t;		/* Parametric value to find point for	*/
{
    int 	i, j;		
    Point2 	Q;	        /* Point on curve at parameter t	*/
    Point2 	*Vtemp;		/* Local copy of control points		*/

    /* Copy array	*/
    Vtemp = (Point2 *)mallocN( (unsigned)((degree+1)* sizeof (Point2)), "Bezier(");
    for (i = 0; i <= degree; i++) {
		Vtemp[i] = V[i];
    }

    /* Triangle computation	*/
    for (i = 1; i <= degree; i++) {	
		for (j = 0; j <= degree-i; j++) {
	    	Vtemp[j].x = (1.0 - t) * Vtemp[j].x + t * Vtemp[j+1].x;
	    	Vtemp[j].y = (1.0 - t) * Vtemp[j].y + t * Vtemp[j+1].y;
		}
    }

    Q = Vtemp[0];
    freeN((char *)Vtemp);
    return Q;
}


/*
 *  B0, B1, B2, B3 :
 *	Bezier multipliers
 */
static double B0(u)
    double	u;
{
    double tmp = 1.0 - u;
    return (tmp * tmp * tmp);
}


static double B1(u)
    double	u;
{
    double tmp = 1.0 - u;
    return (3 * u * (tmp * tmp));
}

static double B2(u)
    double	u;
{
    double tmp = 1.0 - u;
    return (3 * u * u * tmp);
}

static double B3(u)
    double	u;
{
    return (u * u * u);
}



/*
 * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
 *Approximate unit tangents at endpoints and "center" of digitized curve
 */
 
static Vector2 ComputeLeftTangent(d, end)
    Point2	*d;			/*  Digitized points*/
    int		end;		/*  Index to "left" end of region */
{
    Vector2	tHat1;
    tHat1 = V2SubII(d[end+1], d[end]);
    tHat1 = *V2Normalize(&tHat1);
    return tHat1;
}

static Vector2 ComputeRightTangent(d, end)
    Point2	*d;			/*  Digitized points		*/
    int		end;		/*  Index to "right" end of region */
{
    Vector2	tHat2;
    tHat2 = V2SubII(d[end-1], d[end]);
    tHat2 = *V2Normalize(&tHat2);
    return tHat2;
}


static Vector2 ComputeCenterTangent_oud(d, center)
    Point2	*d;			/*  Digitized points			*/
    int		center;		/*  Index to point inside region	*/
{
    Vector2	V1, V2, tHatCenter;

    V1 = V2SubII(d[center-1], d[center]);
    V2 = V2SubII(d[center], d[center+1]);
	V2Normalize(&V1); V2Normalize(&V2);
	
    tHatCenter.x = V1.x + V2.x;
    tHatCenter.y = V1.y + V2.y;
    tHatCenter = *V2Normalize(&tHatCenter);
    return tHatCenter;
}


static Vector2 ComputeCenterTangent(d, center)
    Point2	*d;			/*  Digitized points			*/
    int		center;		/*  Index to point inside region	*/
{
    Vector2	tHatCenter;

    tHatCenter = V2SubII(d[center-1], d[center + 1]);
 	V2Normalize(&tHatCenter);
	
    return tHatCenter;
}


void ComputeCenterTangents(d, center, V1, V2)
    Point2	*d;			/*  Digitized points			*/
    int		center;		/*  Index to point inside region	*/
	Vector2 *V1, *V2;
{
    Vector2	tHatCenter;

    V2Sub(&d[center-1], &d[center], V1);
    V2Sub(&d[center+1], &d[center], V2);
	V2Normalize(V1); V2Normalize(V2);
	if (V2Dot(V1, V2) > -0.7) return;
	
	*V1 = *V2 = ComputeCenterTangent(d, center);
	V2Negate(V2);
}


/*
 *  ChordLengthParameterize :
 *	Assign parameter values to digitized points 
 *	using relative distances between points.
 */
static double *ChordLengthParameterize(d, first, last)
    Point2	*d;			/* Array of digitized points */
    int		first, last;		/*  Indices defining region	*/
{
    int		i;	
    double	*u;			/*  Parameterization		*/

    u = (double *)mallocN((unsigned)(last-first+1) * sizeof(double), "ChordLength");

    u[0] = 0.0;
    for (i = first+1; i <= last; i++) {
		u[i-first] = u[i-first-1] +
	  			V2DistanceBetween2Points(&d[i], &d[i-1]);
    }

    for (i = first + 1; i <= last; i++) {
		u[i-first] = u[i-first] / u[last-first];
    }

    return(u);
}




/*
 *  ComputeMaxError :
 *	Find the maximum squared distance of digitized points
 *	to fitted curve.
*/
static double ComputeMaxError(d, first, last, bezCurve, u, splitPoint)
    Point2	*d;				/*  Array of digitized points	*/
    int		first, last;	/*  Indices defining region	*/
    BezierCurve	bezCurve;	/*  Fitted Bezier curve		*/
    double	*u;				/*  Parameterization of points	*/
    int		*splitPoint;	/*  Point of maximum error	*/
{
    int		i, j;
    double	maxDist;				/*  Maximum error		*/
    double	dist;					/*  Current error		*/
    Point2	P;						/*  Point on curve		*/
    Vector2	v;						/*  Vector from point to curve	*/

    *splitPoint = (last - first + 1)/2;
    maxDist = 0.0;
	
	/* sysmips(MIPS_FPSIGINTR, 0); */

    for (i = first + 1; i < last; i++) {				
		P = Bezier(3, bezCurve, u[i-first]);
		v = V2SubII(P, d[i]);
		dist = V2SquaredLength(&v);
		

		if (dist >= maxDist) {
	    	maxDist = dist;
	    	*splitPoint = i;
		}
    }
	
/* 	sysmips(MIPS_FPSIGINTR, 1); */

    return (maxDist);
}

static Vector2 V2AddII(a, b)
    Vector2 a, b;
{
    Vector2	c;
    c.x = a.x + b.x;  c.y = a.y + b.y;
    return (c);
}
static Vector2 V2ScaleII(v, s)
    Vector2	v;
    double	s;
{
    Vector2 result;
    result.x = v.x * s; result.y = v.y * s;
    return (result);
}

static Vector2 V2SubII(a, b)
    Vector2	a, b;
{
    Vector2	c;
    c.x = a.x - b.x; c.y = a.y - b.y;
    return (c);
}