#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*********************/
/* 2d geometry types */
/*********************/

typedef struct Point2Struct { /* 2d point */
  double x, y;
  } Point2;
typedef Point2 Vector2;

typedef struct IntPoint2Struct {  /* 2d integer point */
  int x, y;
  } IntPoint2;

typedef struct Matrix3Struct {  /* 3-by-3 matrix */
  double element[3][3];
  } Matrix3;

typedef struct Box2dStruct {    /* 2d box */
  Point2 min, max;
  } Box2;
  

/*********************/
/* 3d geometry types */
/*********************/

typedef struct Point3Struct { /* 3d point */
  double x, y, z;
  } Point3;
typedef Point3 Vector3;

typedef struct IntPoint3Struct {  /* 3d integer point */
  int x, y, z;
  } IntPoint3;


typedef struct Matrix4Struct {  /* 4-by-4 matrix */
  double element[4][4];
} Matrix4;

typedef struct Box3dStruct {    /* 3d box */
  Point3 min, max;
  } Box3;



/***********************/
/* one-argument macros */
/***********************/

/* absolute value of a */
#define ABS(a)    (((a)<0) ? -(a) : (a))

/* round a to nearest int */
#define ROUND(a)  floor((a)+0.5)

/* take sign of a, either -1, 0, or 1 */
#define ZSGN(a)   (((a)<0) ? -1 : (a)>0 ? 1 : 0)  

/* take binary sign of a, either -1, or 1 if >= 0 */
#define SGN(a)    (((a)<0) ? -1 : 1)

/* shout if something that should be true isn't */
#define ASSERT(x) \
if (!(x)) fprintf(stderr," Assert failed: x\n");

/* square a */
#define SQR(a)    ((a)*(a)) 


/***********************/
/* two-argument macros */
/***********************/

/* find minimum of a and b */
#define MIN(a,b)  (((a)<(b))?(a):(b)) 

/* find maximum of a and b */
#define MAX(a,b)  (((a)>(b))?(a):(b)) 

/* swap a and b (see Gem by Wyvill) */
#define SWAP(a,b) { a^=b; b^=a; a^=b; }

/* linear interpolation from l (when a=0) to h (when a=1)*/
/* (equal to (a*h)+((1-a)*l) */
#define LERP(a,l,h) ((l)+(((h)-(l))*(a)))

/* clamp the input to the specified range */
#define CLAMP(v,l,h)  ((v)<(l) ? (l) : (v) > (h) ? (h) : v)


/****************************/
/* memory allocation macros */
/****************************/

/* create a new instance of a structure (see Gem by Hultquist) */
#define NEWSTRUCT(x)  (struct x *)(malloc((unsigned)sizeof(struct x)))

/* create a new instance of a type */
#define NEWTYPE(x)  (x *)(malloc((unsigned)sizeof(x)))


/********************/
/* useful constants */
/********************/

#define PI    3.141592f /* the venerable pi */
#define PITIMES2  6.283185f /* 2 * pi */
#define PIOVER2   1.570796f /* pi / 2 */
#define E   2.718282f /* the venerable e */
#define SQRT2   1.414214f /* sqrt(2) */
#define SQRT3   1.732051f /* sqrt(3) */
#define GOLDEN    1.618034f /* the golden ratio */
#define DTOR    0.017453f /* convert degrees to radians */
#define RTOD    57.29578f /* convert radians to degrees */


/************/
/* booleans */
/************/

#define TRUE    1
#define FALSE   0
#define ON    1
#define OFF     0
typedef int boolean;      /* boolean data type */
typedef boolean flag;     /* flag data type */


/* GGVecLib.c */
/* 
2d and 3d Vector C Library 
by Andrew Glassner
from "Graphics Gems", Academic Press, 1990
*/
/******************/
/*   2d Library   */
/******************/

/* returns squared length of input vector */    
double V2SquaredLength(a) 
Vector2 *a;
{       return((a->x * a->x)+(a->y * a->y));
        }
        
/* returns length of input vector */
double V2Length(a) 
Vector2 *a;
{
        return(sqrt(V2SquaredLength(a)));
        }
        
/* negates the input vector and returns it */
Vector2 *V2Negate(v) 
Vector2 *v;
{
        v->x = -v->x;  v->y = -v->y;
        return(v);
        }

/* normalizes the input vector and returns it */
Vector2 *V2Normalize(v) 
Vector2 *v;
{
double len = V2Length(v);
        if (len != 0.0) { v->x /= len;  v->y /= len; }
        return(v);
        }


/* scales the input vector to the new length and returns it */
Vector2 *V2Scale(v, newlen) 
Vector2 *v;
double newlen;
{
double len = V2Length(v);
        if (len != 0.0) { v->x *= newlen/len;   v->y *= newlen/len; }
        return(v);
        }

/* return vector sum c = a+b */
Vector2 *V2Add(a, b, c)
Vector2 *a, *b, *c;
{
        c->x = a->x+b->x;  c->y = a->y+b->y;
        return(c);
        }
        
/* return vector difference c = a-b */
Vector2 *V2Sub(a, b, c)
Vector2 *a, *b, *c;
{
        c->x = a->x-b->x;  c->y = a->y-b->y;
        return(c);
        }

/* return the dot product of vectors a and b */
double V2Dot(a, b) 
Vector2 *a, *b; 
{
        return((a->x*b->x)+(a->y*b->y));
        }

/* linearly interpolate between vectors by an amount alpha */
/* and return the resulting vector. */
/* When alpha=0, result=lo.  When alpha=1, result=hi. */
Vector2 *V2Lerp(lo, hi, alpha, result) 
Vector2 *lo, *hi, *result; 
double alpha;
{
        result->x = LERP(alpha, lo->x, hi->x);
        result->y = LERP(alpha, lo->y, hi->y);
        return(result);
        }


/* make a linear combination of two vectors and return the result. */
/* result = (a * ascl) + (b * bscl) */
Vector2 *V2Combine (a, b, result, ascl, bscl) 
Vector2 *a, *b, *result;
double ascl, bscl;
{
        result->x = (ascl * a->x) + (bscl * b->x);
        result->y = (ascl * a->y) + (bscl * b->y);
        return(result);
        }

/* multiply two vectors together component-wise */
Vector2 *V2Mul (a, b, result) 
Vector2 *a, *b, *result;
{
        result->x = a->x * b->x;
        result->y = a->y * b->y;
        return(result);
        }

/* return the distance between two points */
double V2DistanceBetween2Points(a, b)
Point2 *a, *b;
{
double dx = a->x - b->x;
double dy = a->y - b->y;
        return(sqrt((dx*dx)+(dy*dy)));
        }

/* return the vector perpendicular to the input vector a */
Vector2 *V2MakePerpendicular(a, ap)
Vector2 *a, *ap;
{
        ap->x = -a->y;
        ap->y = a->x;
        return(ap);
        }

/* create, initialize, and return a new vector */
Vector2 *V2New(x, y)
double x, y;
{
Vector2 *v = NEWTYPE(Vector2);
        v->x = x;  v->y = y; 
        return(v);
        }
        

/* create, initialize, and return a duplicate vector */
Vector2 *V2Duplicate(a)
Vector2 *a;
{
Vector2 *v = NEWTYPE(Vector2);
        v->x = a->x;  v->y = a->y; 
        return(v);
        }
        
/* multiply a point by a projective matrix and return the transformed point */
Point2 *V2MulPointByProjMatrix(pin, m, pout)
Point2 *pin, *pout;
Matrix3 *m;
{
double w;
        pout->x = (pin->x * m->element[0][0]) + 
             (pin->y * m->element[1][0]) + m->element[2][0];
        pout->y = (pin->x * m->element[0][1]) + 
             (pin->y * m->element[1][1]) + m->element[2][1];
        w    = (pin->x * m->element[0][2]) + 
             (pin->y * m->element[1][2]) + m->element[2][2];
        if (w != 0.0) { pout->x /= w;  pout->y /= w; }
        return(pout);
        }

/* multiply together matrices c = ab */
/* note that c must not point to either of the input matrices */
Matrix3 *V2MatMul(a, b, c)
Matrix3 *a, *b, *c;
{
int i, j, k;
        for (i=0; i<3; i++) {
                for (j=0; j<3; j++) {
                        c->element[i][j] = 0;
                for (k=0; k<3; k++) c->element[i][j] += 
                                a->element[i][k] * b->element[k][j];
                        }
                }
        return(c);
        }

/* transpose matrix a, return b */
Matrix3 *TransposeMatrix3(a, b)
Matrix3 *a, *b;
{
int i, j;
        for (i=0; i<3; i++) {
                for (j=0; j<3; j++)
                        b->element[i][j] = a->element[j][i];
                }
        return(b);
        }




/******************/
/*   3d Library   */
/******************/
        
/* returns squared length of input vector */    
double V3SquaredLength(a) 
Vector3 *a;
{
        return((a->x * a->x)+(a->y * a->y)+(a->z * a->z));
        }

/* returns length of input vector */
double V3Length(a) 
Vector3 *a;
{
        return(sqrt(V3SquaredLength(a)));
        }

/* negates the input vector and returns it */
Vector3 *V3Negate(Vector3* v) 
{
  v->x = -v->x;  v->y = -v->y;  v->z = -v->z;
  return v;
}

/* normalizes the input vector and returns it */
Vector3 *V3Normalize(v) 
Vector3 *v;
{
double len = V3Length(v);
        if (len != 0.0) { v->x /= len;  v->y /= len; v->z /= len; }
        return(v);
        }

/* scales the input vector to the new length and returns it */
Vector3 *V3Scale(v, newlen) 
Vector3 *v;
double newlen;
{
double len = V3Length(v);
        if (len != 0.0) {
        v->x *= newlen/len;   v->y *= newlen/len;  v->z *= newlen/len;
        }
        return(v);
        }


/* return vector sum c = a+b */
Vector3 *V3Add(a, b, c)
Vector3 *a, *b, *c;
{
        c->x = a->x+b->x;  c->y = a->y+b->y;  c->z = a->z+b->z;
        return(c);
        }
        
/* return vector difference c = a-b */
Vector3 *V3Sub(a, b, c)
Vector3 *a, *b, *c;
{
        c->x = a->x-b->x;  c->y = a->y-b->y;  c->z = a->z-b->z;
        return(c);
        }

/* return the dot product of vectors a and b */
double V3Dot(a, b) 
Vector3 *a, *b; 
{
        return((a->x*b->x)+(a->y*b->y)+(a->z*b->z));
        }

/* linearly interpolate between vectors by an amount alpha */
/* and return the resulting vector. */
/* When alpha=0, result=lo.  When alpha=1, result=hi. */
Vector3 *V3Lerp(lo, hi, alpha, result) 
Vector3 *lo, *hi, *result; 
double alpha;
{
        result->x = LERP(alpha, lo->x, hi->x);
        result->y = LERP(alpha, lo->y, hi->y);
        result->z = LERP(alpha, lo->z, hi->z);
        return(result);
        }

/* make a linear combination of two vectors and return the result. */
/* result = (a * ascl) + (b * bscl) */
Vector3 *V3Combine (a, b, result, ascl, bscl) 
Vector3 *a, *b, *result;
double ascl, bscl;
{
        result->x = (ascl * a->x) + (bscl * b->x);
        result->y = (ascl * a->y) + (bscl * b->y);
        result->z = (ascl * a->z) + (bscl * b->z);
        return(result);
        }


/* multiply two vectors together component-wise and return the result */
Vector3 *V3Mul (a, b, result) 
Vector3 *a, *b, *result;
{
        result->x = a->x * b->x;
        result->y = a->y * b->y;
        result->z = a->z * b->z;
        return(result);
        }

/* return the distance between two points */
double V3DistanceBetween2Points(a, b)
Point3 *a, *b;
{
double dx = a->x - b->x;
double dy = a->y - b->y;
double dz = a->z - b->z;
        return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
        }

/* return the cross product c = a cross b */
Vector3 *V3Cross(a, b, c)
Vector3 *a, *b, *c;
{
        c->x = (a->y*b->z) - (a->z*b->y);
        c->y = (a->z*b->x) - (a->x*b->z);
        c->z = (a->x*b->y) - (a->y*b->x);
        return(c);
        }

/* create, initialize, and return a new vector */
Vector3 *V3New(x, y, z)
double x, y, z;
{
Vector3 *v = NEWTYPE(Vector3);
        v->x = x;  v->y = y;  v->z = z;
        return(v);
        }

/* create, initialize, and return a duplicate vector */
Vector3 *V3Duplicate(a)
Vector3 *a;
{
Vector3 *v = NEWTYPE(Vector3);
        v->x = a->x;  v->y = a->y;  v->z = a->z;
        return(v);
        }

        
/* multiply a point by a matrix and return the transformed point */
Point3 *V3MulPointByMatrix(pin, m, pout)
Point3 *pin, *pout;
Matrix3 *m;
{
        pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
                 (pin->z * m->element[2][0]);
        pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
                 (pin->z * m->element[2][1]);
        pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
                 (pin->z * m->element[2][2]);
        return(pout);
        }

/* multiply a point by a projective matrix and return the transformed point */
Point3 *V3MulPointByProjMatrix(pin, m, pout)
Point3 *pin, *pout;
Matrix4 *m;
{
double w;
        pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
                 (pin->z * m->element[2][0]) + m->element[3][0];
        pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
                 (pin->z * m->element[2][1]) + m->element[3][1];
        pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
                 (pin->z * m->element[2][2]) + m->element[3][2];
        w =    (pin->x * m->element[0][3]) + (pin->y * m->element[1][3]) + 
                 (pin->z * m->element[2][3]) + m->element[3][3];
        if (w != 0.0) { pout->x /= w;  pout->y /= w;  pout->z /= w; }
        return(pout);
        }

/* multiply together matrices c = ab */
/* note that c must not point to either of the input matrices */
Matrix4 *V3MatMul(a, b, c)
Matrix4 *a, *b, *c;
{
int i, j, k;
        for (i=0; i<4; i++) {
                for (j=0; j<4; j++) {
                        c->element[i][j] = 0;
                        for (k=0; k<4; k++) c->element[i][j] += 
                                a->element[i][k] * b->element[k][j];
                        }
                }
        return(c);
        }

/* binary greatest common divisor by Silver and Terzian.  See Knuth */
/* both inputs must be >= 0 */
int gcd(int u, int v)
{
int t, f;
        if ((u<0) || (v<0)) return(1); /* error if u<0 or v<0 */
        f = 1;
        while ((0 == (u%2)) && (0 == (v%2))) {
                u>>=1;  v>>=1,  f*=2;
                }
        if (u&01) { t = -v;  goto B4; } else { t = u; }
        B3: if (t > 0) { t >>= 1; } else { t = -((-t) >> 1); }
        B4: if (0 == (t%2)) goto B3;

        if (t > 0) u = t; else v = -t;
        if (0 != (t = u - v)) goto B3;
        return(u*f);
        }      

/***********************/
/*   Useful Routines   */
/***********************/

/* return roots of ax^2+bx+c */
/* stable algebra derived from Numerical Recipes by Press et al.*/
int quadraticRoots(a, b, c, roots)
double a, b, c, *roots;
{
double d, q;
int count = 0;
        d = (b*b)-(4*a*c);
        if (d < 0.0) { *roots = *(roots+1) = 0.0;  return(0); }
        q =  -0.5 * (b + (SGN(b)*sqrt(d)));
        if (a != 0.0)  { *roots++ = q/a; count++; }
        if (q != 0.0) { *roots++ = c/q; count++; }
        return(count);
        }


/* generic 1d regula-falsi step.  f is function to evaluate */
/* interval known to contain root is given in left, right */
/* returns new estimate */
double RegulaFalsi(f, left, right)
double (*f)(), left, right;
{
double d = (*f)(right) - (*f)(left);
        if (d != 0.0) return (right - (*f)(right)*(right-left)/d);
        return((left+right)/2.0);
        }

/* generic 1d Newton-Raphson step. f is function, df is derivative */
/* x is current best guess for root location. Returns new estimate */
double NewtonRaphson(f, df, x)
double (*f)(), (*df)(), x;
{
double d = (*df)(x);
        if (d != 0.0) return (x-((*f)(x)/d));
        return(x-1.0);
        }


/* hybrid 1d Newton-Raphson/Regula Falsi root finder. */
/* input function f and its derivative df, an interval */
/* left, right known to contain the root, and an error tolerance */
/* Based on Blinn */
double findroot(left, right, tolerance, f, df)
double left, right, tolerance;
double (*f)(), (*df)();
{
double newx = left;
        while (ABS((*f)(newx)) > tolerance) {
                newx = NewtonRaphson(f, df, newx);
                if (newx < left || newx > right) 
                        newx = RegulaFalsi(f, left, right);
                if ((*f)(newx) * (*f)(left) <= 0.0) right = newx;  
                        else left = newx;
                }
        return(newx);
        } 








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

/*	Piecewise cubic fitting code	*/
typedef Point2 *BezierCurve;

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

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


void DrawBezierCurve(int n, BezierCurve curve)
{
  /* You'll have to write this yourself. */
}

/*
 *  main:
 *  Example of how to use the curve-fitting code.  Given an array
 *   of points and a tolerance (squared error between points and 
 *  fitted curve), the algorithm will generate a piecewise
 *  cubic Bezier representation that approximates the points.
 *  When a cubic is generated, the routine "DrawBezierCurve"
 *  is called, which outputs the Bezier curve just created
 *  (arguments are the degree and the control points, respectively).
 *  Users will have to implement this function themselves   
 *   ascii output, etc. 
 *
 */
// int main()
// {
//     static Point2 d[7] = { /*  Digitized points */
//  { 0.0, 0.0 },
//  { 0.0, 0.5 },
//  { 1.1, 1.4 },
//  { 2.1, 1.6 },
//  { 3.2, 1.1 },
//  { 4.0, 0.2 },
//  { 4.0, 0.0 },
//     };
//     double error = 4.0;    /*  Squared error */
//     FitCurve(d, 7, error);   /*  Fit the Bezier curves */
// }
// #endif            /* TESTMODE */


/*
 *  FitCurve :
 *  	Fit a Bezier curve to a set of digitized points 
 */
void FitCurve(d, nPts, error)
    Point2	*d;			/*  Array of digitized points	*/
    int		nPts;		/*  Number of digitized points	*/
    double	error;		/*  User-defined error squared	*/
{
    Vector2	tHat1, tHat2;	/*  Unit tangent vectors at endpoints */

    tHat1 = ComputeLeftTangent(d, 0);
    tHat2 = ComputeRightTangent(d, nPts - 1);
    FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
}



/*
 *  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 */
    int		i;		

    iterationError = error * 4.0;	/* fixed issue 23 */
    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 *)malloc(4 * sizeof(Point2));
		bezCurve[0] = d[first];
		bezCurve[3] = d[last];
		V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
		V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
		DrawBezierCurve(3, bezCurve);
		free((void *)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) {
		DrawBezierCurve(3, bezCurve);
		free((void *)u);
		free((void *)bezCurve);
		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);
	    	free((void *)bezCurve);
	    	bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
	    	maxError = ComputeMaxError(d, first, last,
				       bezCurve, uPrime, &splitPoint);
	    	if (maxError < error) {
			DrawBezierCurve(3, bezCurve);
			free((void *)u);
			free((void *)bezCurve);
			free((void *)uPrime);
			return;
	    }
	    free((void *)u);
	    u = uPrime;
	}
    }

    /* Fitting failed -- split at max error point and fit recursively */
    free((void *)u);
    free((void *)bezCurve);
    tHatCenter = ComputeCenterTangent(d, splitPoint);
    FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
    V2Negate(&tHatCenter);
    FitCubic(d, splitPoint, last, tHatCenter, 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	*/
	double  segLength;
	double  epsilon;

    bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
    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] = 0.0;
    C[0][1] = 0.0;
    C[1][0] = 0.0;
    C[1][1] = 0.0;
    X[0]    = 0.0;
    X[1]    = 0.0;

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

		tmp = V2SubII(d[first + i],
	        V2AddII(
	          V2ScaleIII(d[first], B0(uPrime[i])),
		    	V2AddII(
		      		V2ScaleIII(d[first], B1(uPrime[i])),
		        			V2AddII(
	                  		V2ScaleIII(d[last], B2(uPrime[i])),
	                    		V2ScaleIII(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[1][0] * X[0];
    det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];

    /* Finally, derive alpha values	*/
    alpha_l = (det_C0_C1 == 0) ? 0.0 : det_X_C1 / det_C0_C1;
    alpha_r = (det_C0_C1 == 0) ? 0.0 : det_C0_X / det_C0_C1;

    /* If alpha negative, use the Wu/Barsky heuristic (see text) */
    /* (if alpha is 0, you get coincident control points that lead to
     * divide by zero in any subsequent NewtonRaphsonRootFind() call. */
    segLength = V2DistanceBetween2Points(&d[last], &d[first]);
    epsilon = 1.0e-6 * segLength;
    if (alpha_l < epsilon || alpha_r < epsilon)
    {
		/* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */
		double dist = segLength / 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 *)malloc(nPts * sizeof(double));
    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 = BezierII(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 = BezierII(2, Q1, u);
    Q2_u = BezierII(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);
    if (denominator == 0.0f) return u;

    /* u = u - f(u)/f'(u) */
    uPrime = u - (numerator/denominator);
    return (uPrime);
}

	
		       
/*
 *  Bezier :
 *  	Evaluate a Bezier curve at a particular parameter value
 * 
 */
static Point2 BezierII(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 *)malloc((unsigned)((degree+1) 
				* sizeof (Point2)));
    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];
    free((void *)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(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]);
    tHatCenter.x = (V1.x + V2.x)/2.0;
    tHatCenter.y = (V1.y + V2.y)/2.0;
    tHatCenter = *V2Normalize(&tHatCenter);
    return tHatCenter;
}


/*
 *  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 *)malloc((unsigned)(last-first+1) * sizeof(double));

    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;
    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;
    for (i = first + 1; i < last; i++) {
		P = BezierII(3, bezCurve, u[i-first]);
		v = V2SubII(P, d[i]);
		dist = V2SquaredLength(&v);
		if (dist >= maxDist) {
	    	maxDist = dist;
	    	*splitPoint = i;
		}
    }
    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 V2ScaleIII(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);
}
