Merge branch 'feature/upstream' into develop

39 files changed, 2267 insertions, 236 deletions

diff --git a/numlib/Jamfile b/numlib/Jamfile
index 1053612..2d5edaf 100644..100755
--- a/numlib/Jamfile
+++ b/numlib/Jamfile
@@ -20,13 +20,13 @@ Headers = numlib.h libui.h ;
 HDRS = ../h ;
 
 # Numeric library
-Library libnum.lib : numsup.c dnsq.c powell.c dhsx.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c ;
+Library libnum.lib : numsup.c dnsq.c powell.c dhsx.c varmet.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c quadprog.c ;
 
 # Link all utilities with libnum
 LINKLIBS = libnum ;
 
 # All test programs are made from a single source file
-MainsFromSources dnsqtest.c tpowell.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c ;
+MainsFromSources dnsqtest.c tpowell.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c qptest.c ;
 
 # Compile .c as .m
 if $(OS) = MACOSX {
diff --git a/numlib/LUtest.c b/numlib/LUtest.c
index feb2277..feb2277 100644..100755
--- a/numlib/LUtest.c
+++ b/numlib/LUtest.c
diff --git a/numlib/License.txt b/numlib/License.txt
index a871fcf..a871fcf 100644..100755
--- a/numlib/License.txt
+++ b/numlib/License.txt
diff --git a/numlib/Readme.txt b/numlib/Readme.txt
index 8cc0173..8cc0173 100644..100755
--- a/numlib/Readme.txt
+++ b/numlib/Readme.txt
diff --git a/numlib/aatree.c b/numlib/aatree.c
index 2f38b1d..2f38b1d 100644..100755
--- a/numlib/aatree.c
+++ b/numlib/aatree.c
diff --git a/numlib/aatree.h b/numlib/aatree.h
index 4212b09..325bf1e 100644..100755
--- a/numlib/aatree.h
+++ b/numlib/aatree.h
@@ -4,6 +4,8 @@
 /*
   Andersson binary balanced tree library
 
+  Log n performance on insert/erase
+
   > Created (Julienne Walker): September 10, 2005
 
   This code is in the public domain. Anyone may
@@ -33,6 +35,7 @@ typedef struct aat_atree aat_atree_t;
 typedef struct aat_atrav aat_atrav_t;
 
 /* User-defined item handling */
+/* Return -1, 0, +1 */
 typedef int   (*cmp_f) ( const void *p1, const void *p2 );
 
 /* Andersson tree functions */
diff --git a/numlib/afiles b/numlib/afiles
index 29d6699..b2138b2 100644..100755
--- a/numlib/afiles
+++ b/numlib/afiles
@@ -11,6 +11,8 @@ numsup.h
 powell.h
 powell.c
 tpowell.c
+varmet.h
+varmet.c
 dhsx.h
 dhsx.c
 tdhsx.c
@@ -30,5 +32,8 @@ sobol.h
 soboltest.c
 aatree.h
 aatree.c
+quadprog.h
+quadprog.c
+qptest.c
 ui.h
 ui.c
diff --git a/numlib/dhsx.c b/numlib/dhsx.c
index 180d15c..e9da0eb 100644..100755
--- a/numlib/dhsx.c
+++ b/numlib/dhsx.c
@@ -20,7 +20,9 @@
 
 #undef DEBUG
 
-static void simplexinit(int di, double *cp, double *r,double **p);
+int dhsx_debug = 1;
+
+static void simplexinit(int di, double *cp, double **p, double *r, double sv, int ii);
 static double trypoint(int di,double *cp, double **p, double *y, int hix, double hpfac,
                 double (*funk)(void *fdata, double *tp), void *fdata, double *tryp);
 
@@ -28,68 +30,98 @@ static double trypoint(int di,double *cp, double **p, double *y, int hix, double
 
 #define ALPHA 0.7			/* Extrapolate hight point through oposite face factor */
 #define GAMMA 1.4			/* Aditional extrapolation if ALPHA is good */ 
-#define BETA 0.4			/* One dimensional contraction factor */
+#define BETA 0.4			/* One dimensional contraction factor (smaller is more) */
+#define DELTA 0.5			/* Multi dimensional contraction factor (smaller is more) */
 #define NONEXP 2			/* non expanding passes */
 
 #else			/* Standard tuning values */
 
-#define ALPHA 1.0			/* Extrapolate hight point through oposite face factor */
-#define GAMMA 2.0			/* Aditional extrapolation if ALPHA is good */ 
-#define BETA 0.5			/* One dimensional contraction factor */
-#define NONEXP 2			/* non expanding passes */
+#define ALPHA 1.0			/* [1.0] Extrapolate hight point through oposite face factor */
+#define GAMMA 2.0			/* [2.0] Aditional extrapolation if ALPHA is good */ 
+#define BETA 0.4			/* [0.5] One dimensional contraction factor (smaller is more) */
+#define DELTA 0.4			/* [0.5] Multi dimensional contraction factor (smaller is more) */
+#define NONEXP 3			/* [3] non expanding passes */
 
 #endif
 
-
 /* Down hill simplex function */
 /* return 0 on sucess, 1 on failure due to excessive itterations */
 /* Result will be in cp */
-/* Arrays start at 0 */
 int dhsx(
 double *rv,				/* If not NULL, return the residual error */
 int di,					/* Dimentionality */
-double *cp,				/* Initial starting point */
+double *cp,				/* Initial starting point, return minimum */
 double *s,				/* Size of initial search area */
 double ftol,			/* Finishing tollerance of error change */
-double atol,			/* Absolute return value tollerance */
+double athr,			/* Absolute return value threshold. (Set high to not use) */
 int maxit,				/* Maximum iterations allowed */
 double (*funk)(void *fdata, double *tp),		/* Error function to evaluate */
 void *fdata				/* Data needed by function */
 ) {
+	int ii = 0;			/* Initial simplex orientation */
 	int i, j;
-	int lox, hix, nhix;	/* Lowest point index, highest point, next highest point */
-	int nsp = di+1;		/* Number of simplex verticy points */
 	int nit;			/* Number of iterations */
+	int nsp = di+1;		/* Number of simplex verticy points */
 	double tryy, ysave;
 	double tol;
-	double **p;			/* Simplex array */
-	double *y;			/* values of func at verticies */
+	double **p;			/* Current simplex array */
+	double *y;			/* Values of func at verticies */
+	double **p2;		/* Trial simplex array */
+	double *y2;			/* Trial values of func at verticies */
+	int lox, hix, nhix;	/* Lowest point index, highest point, next highest point */
 	double *tryp;		/* Temporary used by trypoint() */
 
 	/* Allocate array arrays */
-	y = dvector(0, di);				/* Value of function at verticies */
-	tryp = dvector(0, di-1);		
-	p = dmatrix(0, di+1, 0, di);	/* Vertex array of dimentions */
+	tryp = dvector(0, di-1);		/* Trial value */		
+	p = dmatrix(0, nsp-1, 0, di-1);	/* Vertex array of dimentions */
+	y = dvector(0, nsp-1);			/* Value of function at verticies */
+	p2 = dmatrix(0, nsp-1, 0, di-1);	/* Trial vertex array of dimentions */
+	y2 = dvector(0, nsp-1);			/* Trial value of function at verticies */
 	
 	/* Init the search simplex */
-	simplexinit(di, cp, s, p);
+	simplexinit(di, cp, p, s, 1.0, ii);
+
+	/* Compute initial y (function) values at simplex verticies */
+	for (i = 0; i < nsp; i++)			/* For all verticies */
+		y[i] = (*funk)(fdata, p[i]);	/* Compute error function */
 
-	/* Compute current center point position */
-	for (j = 0; j < di; j++) {	/* For all dimensions */
+	/* Locate verticy with best value */
+	lox = 0;
+	for (i = 0; i < nsp; i++) {
+		if (y[i] < y[lox]) 
+			lox = i;
+	}
+	tryy = (*funk)(fdata, cp);	/* Value at initial point */
+
+	/* If our initial point is better than any of the simplex verticies */
+	if (y[lox] > tryy) {
+		/* Move all the verticies to match moving lox to cp */
+		for (i = 0; i < nsp; i++) {
+			if (i == lox)
+				continue;
+			for (j = 0; j < di; j++)
+				p[i][j] += cp[j] - p[lox][j]; 
+			y[i] = (*funk)(fdata, p[i]);	/* Compute error function */
+		}
+		/* Make lox be the input point */
+		for (j = 0; j < di; j++)
+			p[lox][j] = cp[j];
+		y[lox] = tryy;
+	} 
+
+	/* Compute current center point location as sum of verticies. */
+	/* (We use this to compute moves) */
+	for (j = 0; j < di; j++) {					/* For all dimensions */
 		double sum;
 		for (i = 0, sum = 0.0; i < nsp; i++)	/* For all verticies */
 			sum += p[i][j];
 		cp[j] = sum;
 	}
 
-	/* Compute initial y (function) values at verticies */
-	for (i = 0; i < nsp; i++)		/* For all verticies */
-		y[i] = (*funk)(fdata,p[i]);	/* Compute error function */
-
 	/* Untill we find a solution or give up */
-	for (nit = 0; nit < maxit; nit++) {
-		/* Find highest, next highest and lowest vertex */
+	for (nit = 0; ; nit++) {
 
+		/* Find highest, next highest and lowest vertex */
 		lox = nhix = hix = 0;
 		for (i = 0; i < nsp; i++) {
 			if (y[i] < y[lox]) 
@@ -104,82 +136,189 @@ void *fdata				/* Data needed by function */
 
 		tol = y[hix] - y[lox];
 
-#ifdef DEBUG	/* 2D */
-		printf("Current vs = %f,%f  %f,%f  %f,%f\n",
-		        p[0].c[0],p[0].c[1],p[1].c[0],p[1].c[1],p[2].c[0],p[2].c[1]);
-		printf("Current errs = %e    %e    %e\n",y[0],y[1],y[2]);
-		printf("Current sxs = %d    %d    %d\n",sy[0]->no,sy[1]->no,sy[2]->no);
-		printf("Current y[hix] = %e\n",y[hix]);
+#ifdef DEBUG
+		if (dhsx_debug) {
+			printf("Current vs =\n");
+			for (i = 0; i < nsp; i++)
+				printf(" %d: %s\n",i,debPdv(di, p[i]));
+			printf("Current errs = %s\n",debPdv(nsp,y));
+			printf("Current y[lox] = %e, y[hix] = %e\n",y[lox], y[hix]);
+		}
+#endif /* DEBUG */
+
+		/* If we look like we are about to finish, */
+		/* see if we should re-start with a new simplex. */
+		if (tol < ftol && y[lox] < athr 	/* Found an adequate solution */
+		 && nit < maxit) {
+			double scale = 0.0;
+			int lox2;
+
+#ifdef DEBUG
+			if (dhsx_debug) printf(" nit %d, tol %e\n",nit, tol);
+#endif /* DEBUG */
+
+			/* compute center location */
+			tryy = 1.0/nsp;
+			for (j = 0; j < di; j++)		/* For all dimensions */
+				cp[j] *= tryy;				/* Set cp to center point of simplex */
+
+			/* Compute scaled distance of vertexes from center */
+			for (i = 0; i < nsp; i++) {
+				double dist = 0.0;
+				for (j = 0; j < di; j++) {
+					double tt = (cp[j] - p[i][j])/s[j];
+					dist += tt * tt;
+				}
+				scale += sqrt(dist);
+			}
+			scale /= (double)nsp;		/* Average scale compared to starting simplex */
+#ifdef DEBUG
+			if (dhsx_debug) printf(" ave scale = %f\n",scale);
+#endif /* DEBUG */
+
+			/* Enlarge search space, but not more than initial */
+			scale *= 10.0;
+			if (scale > 1.0)
+				scale = 1.0;
+
+			/* Compute trial simplex with different orientation */
+			if (++ii >= (di+1))
+				ii = 0;
+
+			/* Init the search simplex */
+			simplexinit(di, cp, p2, s, scale, ii);
+
+			/* Compute y (function) values at simplex verticies */
+			for (i = 0; i < nsp; i++)			/* For all verticies */
+				y2[i] = (*funk)(fdata, p2[i]);	/* Compute error function */
+
+			/* Locate verticy with best value */
+			lox2 = 0;
+			for (i = 0; i < nsp; i++) {
+				if (y2[i] < y2[lox2]) 
+					lox2 = i;
+			}
+#ifdef DEBUG
+			if (dhsx_debug) printf(" y2lox %f ylox %f\n",y2[lox2], y[lox]);
+#endif /* DEBUG */
+
+			/* If any of its vertexes are better than current best, switch */
+			/* to it and continue (i.e. re-start) */
+			if (y2[lox2] < y[lox]) {
+
+#ifdef DEBUG
+				if (dhsx_debug) printf(" restarting\n");
 #endif /* DEBUG */
 
-		if (tol < ftol && y[lox] < atol) {	/* Found an adequate solution */
-							/* (allow 10 x range for disambiguator) */
-			/* set cp[] to best point, and return error value of that point */
-			tryy = 1.0/(di+1);
+				for (i = 0; i < nsp; i++) {
+					for (j = 0; j < di; j++)
+						p[i][j] = p2[i][j];
+					y[i] = y2[i];
+				}
+
+				/* Compute current center point location as sum of verticies. */
+				/* (We use this to compute moves) */
+				for (j = 0; j < di; j++) {					/* For all dimensions */
+					double sum;
+					for (i = 0, sum = 0.0; i < nsp; i++)	/* For all verticies */
+						sum += p[i][j];
+					cp[j] = sum;
+				}
+
+				/* Find highest, next highest and lowest vertex */
+				lox = nhix = hix = 0;
+				for (i = 0; i < nsp; i++) {
+					if (y[i] < y[lox]) 
+						lox = i;
+					if (y[i] > y[hix]) {
+						nhix = hix;
+						hix = i;
+					} else if (y[i] > y[nhix]) {
+						nhix = i;
+					}
+				}
+
+				tol = y[hix] - y[lox];
+			}
+		}
+
+		if ((tol < ftol && y[lox] < athr) 	/* Found an adequate solution */
+		 || ((nit+1) >= maxit)) {				/* Or we are about to fail */
+
+			/* convert cp[] to center point location, */
+			/* and use best out of it and any simplex verticy. */
+			tryy = 1.0/nsp;
 			for (j = 0; j < di; j++)		/* For all dimensions */
 				cp[j] *= tryy;				/* Set cp to center point of simplex */
 #ifdef DEBUG
-			printf("C point = %f,%f\n",cp[0],cp[1]);
+			if (dhsx_debug) printf("C point = %s\n",debPdv(di,cp));
 #endif
-			tryy = (*funk)(fdata,cp);		/* Compute error function */
+			tryy = (*funk)(fdata, cp);		/* Compute error function */
 
 			if (tryy > y[lox]) {			/* Center point is not the best */
+#ifdef DEBUG
+				if (dhsx_debug) printf("C point val %f is not best, using sx %d val %f instead\n",tryy,lox,y[lox]);
+#endif
 				tryy = y[lox];
 				for (j = 0; j < di; j++)
 					cp[j] = p[lox][j];
 			}
-			free_dmatrix(p, 0, di+1, 0, di);
+			free_dvector(y2, 0, nsp-1);
+			free_dmatrix(p2, 0, nsp-1, 0, di-1);
+			free_dvector(y, 0, nsp-1);
+			free_dmatrix(p, 0, nsp-1, 0, di-1);
 			free_dvector(tryp, 0, di-1);
-			free_dvector(y, 0, di);
-printf("~1 itterations = %d\n",nit);
+#ifdef DEBUG
+			if (dhsx_debug) printf("Total itterations = %d\n",nit);
+#endif
 			if (rv != NULL)
 				*rv = tryy;
+			if ((nit+1) >= maxit)
+				return 1;	/* Failed */
 			return 0;
 		}
 
-		if (nit > NONEXP) {	/* Only try expanding after a couple of iterations */
+		/* Only try expanding after a couple of iterations */
+		if (nit > NONEXP) {
 			/* Try moving the high point through the oposite face by ALPHA */
 #ifdef DEBUG
-			printf("dhsx: try moving high point %d through oposite face",hix);
+			if (dhsx_debug) printf("dhsx: try moving high point %d through oposite face",hix);
 #endif
 			tryy = trypoint(di, cp, p, y, hix, -ALPHA, funk, fdata, tryp);
 		}
 
+		/* If gave good result, continue on in that direction */
 		if (nit > NONEXP && tryy <= y[lox]) {
 #ifdef DEBUG
-			verbose(4,"dhsx: moving high through oposite face worked");
+			if (dhsx_debug) printf("dhsx: moving high through oposite face worked");
 #endif
-			/* Gave good result, so continue on in that direction */
-			tryy=trypoint(di,cp,p,y,hix,GAMMA,funk,fdata,tryp);
+			tryy = trypoint(di, cp, p, y, hix, GAMMA, funk, fdata, tryp);
 
 
+		/* else if ALPHA move made things worse, do a one dimensional */
+		/* contraction by a factor BETA */
 		} else if (nit <= NONEXP || tryy >= y[nhix]) {
 
-			/* else if ALPHA move made things worse, do a one dimensional */
-			/* contraction by a factor BETA */
 #ifdef DEBUG
-			verbose(4,"dhsx: else try moving contracting point %d, y[ini] = %f",hix,y[hix]);
+			if (dhsx_debug) printf("dhsx: else try moving contracting point %d, y[ini] = %f",hix,y[hix]);
 #endif
 			ysave = y[hix];
 			tryy = trypoint(di, cp, p, y, hix, BETA, funk, fdata, tryp);
 			
 			if (tryy >= ysave) {
 #ifdef DEBUG
-				verbose(4,"dhsx: contracting didn't work, try contracting other points to low");
+				if (dhsx_debug) printf("dhsx: contracting didn't work, try contracting other points to low");
 #endif
 				/* That still didn't help us, so move all the */
-				/* other points towards the high point */
+				/* other points towards the low point */
 				for (i = 0; i < nsp; i++) {	/* For all verts except low */
 					if (i != lox) {
-						for (j = 0; j < di; j++) {	/* For all dimensions */
-							cp[j] = 0.5 * (p[i][j] + p[lox][j]);
-							p[i][j] = cp[j];
-						}
-						/* Compute function value for new point */
-						y[i] = (*funk)(fdata,p[i]);		/* Compute error function */
+						for (j = 0; j < di; j++) 	/* For all dimensions */
+							p[i][j] = DELTA * p[i][j] + (1.0 - DELTA) * p[lox][j];
+						y[i] = (*funk)(fdata, p[i]);	/* Compute function value for new point */
 					}
 				}
-				/* Re-compute current center point value */
+				/* Re-compute current center point location */
 				for (j = 0; j < di; j++) {
 					double sum;
 					for (i = 0,sum = 0.0;i<nsp;i++)
@@ -188,22 +327,17 @@ printf("~1 itterations = %d\n",nit);
 				}
 			} else {
 #ifdef DEBUG
-				verbose(4,"dhsx: contracting point %d worked, tryy = %e, ysave = %e",hix,tryy,ysave);
+				if (dhsx_debug) printf("dhsx: contracting point %d worked, tryy = %e, ysave = %e",hix,tryy,ysave);
 #endif
 			}
 		}
 	}
-	free_dmatrix(p, 0, di+1, 0, di);
-	free_dvector(tryp, 0, di-1);
-	free_dvector(y, 0, di);
-	if (rv != NULL)
-		*rv = tryy;
-	return 1;	/* Failed */
 }
 
 /* Try moving the high point through the opposite face */
 /* by a factor of fac, and replaces the high point if */
-/* that proves to be better.                          */
+/* that proves to be better.  Return the failed or new */
+/* function value. */
 static double trypoint(
 int di,					/* Dimentionality */
 double *cp,				/* nsp * center coord/Returned coordinate */
@@ -229,7 +363,7 @@ double *tryp			/* temporary array of size di-1 */
 	/* If new high point pos. is better */
 	if (tryy < y[hix]) {
 #ifdef DEBUG
-		printf("Try gave improved %e from sx %d",tryy,lsxp->last_fwd->no);
+		if (dhsx_debug) printf("Try gave improved %e from sx %d",tryy,hix);
 #endif
 		y[hix] = tryy;		/* Replace func val of hi with trial */
 
@@ -239,8 +373,7 @@ double *tryp			/* temporary array of size di-1 */
 		}
 	} else {
 #ifdef DEBUG
-		verbose(4,"Try gave worse %e from sx %d",tryy,
-		               lsxp->last_fwd == NULL ? -999 : lsxp->last_fwd->no);
+		if (dhsx_debug) printf("Try gave worse %e from sx %d",tryy,hix);
 #endif
 	}
 	return tryy;		/* Function value of trial point */
@@ -251,17 +384,19 @@ double *tryp			/* temporary array of size di-1 */
 static void
 simplexinit(
 int di,			/* Dimentionality */
-double *cp,		/* Initial solution values */
+double *cp,		/* Initial solution location */
+double **p,		/* Simplex to initialize */
 double *s,		/* initial radius for each dimention */
-double **p		/* Simplex to initialize */
+double sv,		/* Radius scaling value */
+int ii			/* Coordinate to start with */
 ) {
 	double bb;
 	double hh = 0.5;			/* Constant */
 	double rr = sqrt(3.0)/2.0;	/* Constant */
-	int i,j;
-	for (i = 0; i <= di; i++) {	/* For each vertex */
+	int i, j;
+	for (i = 0; i < (di+1); i++) {	/* For each vertex */
 		/* The bounding points form a equalateral simplex */
-		/* whose vertexes are on a spehere about the data */
+		/* whose vertexes are on a sphere about the data */
 		/* point center. The coordinate sequence is: */
 		/*  B = sphere radius */
 		/*	H = 0.5         */
@@ -287,16 +422,25 @@ double **p		/* Simplex to initialize */
 		/*      etc.     */
 		bb = 1.0;		/* Initial unscaled radius */
 		for (j = 0; j < di; j++) {	/* For each coordinate in vertex */
-			if (j > i)
-				p[i][j] = cp[j] + s[j] * 0.0;		/* If beyond last */
-			else if (j == i)		/* If last non-zero */
-				p[i][j] = cp[j] + s[j] * bb;
-			else if (i == di && j == (di-1)) /* If last of last */
-				p[i][j] = cp[j] + s[j] * -1.0 * bb;
+			if (j > ii)
+				p[i][j] = cp[j] + sv * s[j] * 0.0;		/* If beyond last */
+			else if (j == ii)		/* If last non-zero */
+				p[i][j] = cp[j] + sv * s[j] * bb;
+			else if (ii == di && j == (di-1)) /* If last of last */
+				p[i][j] = cp[j] + sv * s[j] * -1.0 * bb;
 			else					/* If before last */
-				p[i][j] = cp[j] + s[j] * -hh * bb;
+				p[i][j] = cp[j] + sv * s[j] * -hh * bb;
 			bb *= rr;
 		}
+		/* Increment coordinate number with wrap around */
+		if (++ii >= (di+1))
+			ii = 0;
+	}
+#ifdef DEBUG
+	if (dhsx_debug) {
+		for (i = 0; i < (di+1); i++)
+			printf(" p[%d] = %s\n",i,debPdv(di,p[i]));
 	}
+#endif
 }
 
diff --git a/numlib/dhsx.h b/numlib/dhsx.h
index 828b8b2..78cdb10 100644..100755
--- a/numlib/dhsx.h
+++ b/numlib/dhsx.h
@@ -11,29 +11,23 @@
 #endif
 
 /* Down hill simplex function */
-/* return err on sucess, -1.0 on failure */
-/* Result will be in cp */
-/* Arrays start at 0 */
-
-/* Standard interface for optimizer function */
 /* return 0 on sucess, 1 on failure due to excessive itterations */
 /* Result will be in cp */
-/* Arrays start at 0 */
 int dhsx(
-double *rv,				/* If not NULL, return the residual error */
-int di,					/* Dimentionality */
-double cp[],			/* Initial starting point */
-double s[],				/* Size of initial search area */
-double ftol,			/* Tollerance of error change to stop on */
-double atol,			/* Absolute return value tollerance */
-int maxit,				/* Maximum iterations allowed */
-double (*funk)(void *fdata, double tp[]),		/* Error function to evaluate */
-void *fdata				/* Data needed by function */
+ double *rv,				/* If not NULL, return the residual error */
+ int di,					/* Dimentionality */
+ double *cp,				/* Initial starting point, return minimum */
+ double *s,				/* Size of initial search area */
+ double ftol,			/* Finishing tollerance of error change */
+ double athr,			/* Absolute return value threshold. (Set high to not use) */
+ int maxit,				/* Maximum iterations allowed */
+ double (*funk)(void *fdata, double *tp),		/* Error function to evaluate */
+ void *fdata				/* Data needed by function */
 );
 
 double dhsx_funk(		/* Return function value */
-	void *fdata,		/* Opaque data pointer */
-	double tp[]);		/* Multivriate input value */
+ void *fdata,		/* Opaque data pointer */
+ double tp[]);		/* Multivriate input value */
 
 #ifdef __cplusplus
 	}
diff --git a/numlib/dnsq.c b/numlib/dnsq.c
index 75f00a8..75f00a8 100644..100755
--- a/numlib/dnsq.c
+++ b/numlib/dnsq.c
diff --git a/numlib/dnsq.h b/numlib/dnsq.h
index e06e562..e06e562 100644..100755
--- a/numlib/dnsq.h
+++ b/numlib/dnsq.h
diff --git a/numlib/dnsqtest.c b/numlib/dnsqtest.c
index 3f02819..3f02819 100644..100755
--- a/numlib/dnsqtest.c
+++ b/numlib/dnsqtest.c
diff --git a/numlib/ludecomp.c b/numlib/ludecomp.c
index 72e014a..b756bfe 100644..100755
--- a/numlib/ludecomp.c
+++ b/numlib/ludecomp.c
@@ -162,6 +162,8 @@ int      n	/* Dimensionality */
 }
 
 /* Decompose the square matrix A[][] into lower and upper triangles */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use sym_matrix_trans() to fix this. */
 /* Return 1 if the matrix is singular. */
 int
 lu_decomp(
@@ -339,6 +341,8 @@ int     *pivx		/* Pivoting row permutations record */
 
 
 /* Invert a matrix A using lu decomposition */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */
 /* Return 1 if the matrix is singular, 0 if OK */
 int
 lu_invert(
@@ -385,7 +389,10 @@ int      n	/* Dimensionality */
 	return 0;
 }
 
-#ifdef NEVER		/* It's not clear that this is correct */
+/* Invert a matrix A using lu decomposition, and polish it. */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */
+/* Return 1 if the matrix is singular, 0 if OK */
 int
 lu_polished_invert(
 double **a,	/* A[][] input matrix, returns inversion of A */
@@ -413,8 +420,8 @@ int      n	/* Dimensionality */
 		return i;
 	}
 
-	for (k = 0; k < 10; k++) {
-		matrix_mult(t1, n, n, aa, n, n, a, n, n);
+	for (k = 0; k < 20; k++) {
+		matrix_trans_mult(t1, n, n, aa, n, n, a, n, n);
 		for (i = 0; i < n; i++) {
 			for (j = 0; j < n; j++) {
 				t2[i][j] = a[i][j];
@@ -432,9 +439,10 @@ int      n	/* Dimensionality */
 	free_dmatrix(t2, 0, n-1, 0, n-1);
 	return 0;
 }
-#endif
 
 /* Pseudo-Invert matrix A using lu decomposition */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use matrix_trans() to fix this, or use matrix_trans_mult(). */
 /* Return 1 if the matrix is singular, 0 if OK */
 int
 lu_psinvert(
diff --git a/numlib/ludecomp.h b/numlib/ludecomp.h
index cdb4382..1075a5b 100644..100755
--- a/numlib/ludecomp.h
+++ b/numlib/ludecomp.h
@@ -69,6 +69,8 @@ int     *pivx		/* Pivoting row permutations record */
 );
 
 /* Invert a matrix A using lu decomposition */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */ 
 /* Return 1 if the matrix is singular, 0 if OK */
 int
 lu_invert(
@@ -76,7 +78,19 @@ double **a,	/* A[][] input matrix, returns inversion of A */
 int      n	/* Dimensionality */
 );
 
+/* Invert a matrix A using lu decomposition, and polish it. */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */
+/* Return 1 if the matrix is singular, 0 if OK */
+int
+lu_polished_invert(
+double **a,	/* A[][] input matrix, returns inversion of A */
+int      n	/* Dimensionality */
+);
+
 /* Pseudo-Invert matrix A using lu decomposition */
+/* NOTE that it returns transposed inverse by normal convention. */
+/* Use matrix_trans() to fix this, or use matrix_trans_mult(). */ 
 /* Return 1 if the matrix is singular, 0 if OK */
 int
 lu_psinvert(
diff --git a/numlib/numlib.h b/numlib/numlib.h
index aa34b2d..430ce7a 100644..100755
--- a/numlib/numlib.h
+++ b/numlib/numlib.h
@@ -8,11 +8,13 @@
 #include "dnsq.h"		/* Non-linear equation solver */
 #include "powell.h"		/* Powell multi dimentional minimiser */
 #include "dhsx.h"		/* Downhill simplex multi dimentional minimiser */
+#include "varmet.h"		/* Variable Metric multi dimentional minimiser */
 #include "ludecomp.h"	/* LU decomposition matrix solver */
 #include "svd.h"		/* Singular Value decomposition matrix solver */
 #include "zbrent.h"		/* 1 dimentional brent root search */
 #include "rand.h"		/* Random number generators */
 #include "sobol.h"		/* Sub-random vector generators */
 #include "aatree.h"		/* Anderson balanced binary tree */
+#include "quadprog.h"	/* Quadradic Programming solution */
 
 #endif /* NUMLIB_H */
diff --git a/numlib/numsup.c b/numlib/numsup.c
index 39011ae..eab81ed 100644..100755
--- a/numlib/numsup.c
+++ b/numlib/numsup.c
@@ -483,17 +483,34 @@ a1log *del_a1log(a1log *log) {
 	return NULL;
 }
 
+/* Set the debug level. */
+void a1log_debug(a1log *log, int level) {
+	if (log != NULL) {
+		log->debug = level;
+	}
+}
+
+/* Set the vebosity level. */
+void a1log_verb(a1log *log, int level) {
+	if (log != NULL) {
+		log->verb = level;
+	}
+}
+
 /* Set the tag. Note that the tage string is NOT copied, just referenced */
 void a1log_tag(a1log *log, char *tag) {
-	log->tag = tag;
+	if (log != NULL) {
+		log->tag = tag;
+	}
 }
 
 /* Log a verbose message if level >= verb */
 void a1logv(a1log *log, int level, char *fmt, ...) {
+
 	if (log != NULL) {
 		if (log->verb >= level) {
 			va_list args;
-	
+
 			A1LOG_LOCK(log, 0);
 			va_start(args, fmt);
 			log->logv(log->cntx, log, fmt, args);
@@ -511,7 +528,7 @@ void a1logd(a1log *log, int level, char *fmt, ...) {
 	
 			A1LOG_LOCK(log, 1);
 			va_start(args, fmt);
-			log->loge(log->cntx, log, fmt, args);
+			log->logd(log->cntx, log, fmt, args);
 			va_end(args);
 			A1LOG_UNLOCK(log);
 		}
@@ -601,6 +618,8 @@ void a1logue(a1log *log) {
 void adump_bytes(a1log *log, char *pfx, unsigned char *buf, int base, int len) {
 	int i, j, ii;
 	char oline[200] = { '\000' }, *bp = oline;
+	if (pfx == NULL)
+		pfx = "";
 	for (i = j = 0; i < len; i++) {
 		if ((i % 16) == 0)
 			bp += sprintf(bp,"%s%04x:",pfx,base+i);
@@ -827,7 +846,7 @@ void osx_userinitiated_start() {
 	}
 
 	/* Create a reason string */
-	str = objc_msgSend(objc_getClass("NSString"), sel_getUid("alloc"));
+	str = objc_msgSend((id)objc_getClass("NSString"), sel_getUid("alloc"));
 	str = objc_msgSend(str, sel_getUid("initWithUTF8String:"), "ArgyllCMS");
 			
 	/* Start activity that tells App Nap to mind its own business. */
@@ -842,8 +861,9 @@ void osx_userinitiated_end() {
 		if (osx_userinitiated_cnt == 0 && osx_userinitiated_activity != nil) {
 			a1logd(g_log, 7, "OS X - User Initiated Activity end");
 			objc_msgSend(
-			             objc_msgSend(objc_getClass("NSProcessInfo"), sel_getUid("processInfo")), 
-			             sel_getUid("endActivity:"), osx_userinitiated_activity);
+			             objc_msgSend((id)objc_getClass("NSProcessInfo"),
+			             sel_getUid("processInfo")), sel_getUid("endActivity:"),
+			             osx_userinitiated_activity);
 			osx_userinitiated_activity = nil;
 		}
 	}
@@ -885,7 +905,7 @@ void osx_latencycritical_start() {
 	}
 
 	/* Create a reason string */
-	str = objc_msgSend(objc_getClass("NSString"), sel_getUid("alloc"));
+	str = objc_msgSend((id)objc_getClass("NSString"), sel_getUid("alloc"));
 	str = objc_msgSend(str, sel_getUid("initWithUTF8String:"), "Measuring Color");
 			
 	/* Start activity that tells App Nap to mind its own business. */
@@ -900,8 +920,9 @@ void osx_latencycritical_end() {
 		if (osx_latencycritical_cnt == 0 && osx_latencycritical_activity != nil) {
 			a1logd(g_log, 7, "OS X - Latency Critical Activity end");
 			objc_msgSend(
-			             objc_msgSend(objc_getClass("NSProcessInfo"), sel_getUid("processInfo")), 
-			             sel_getUid("endActivity:"), osx_latencycritical_activity);
+			             objc_msgSend((id)objc_getClass("NSProcessInfo"),
+			             sel_getUid("processInfo")), sel_getUid("endActivity:"),
+			             osx_latencycritical_activity);
 			osx_latencycritical_activity = nil;
 		}
 	}
@@ -957,7 +978,7 @@ int nh		/* Highest index */
 }
 
 /* --------------------- */
-/* 2D Double vector malloc/free */
+/* 2D Double matrix malloc/free */
 double **dmatrix(
 int nrl,	/* Row low index */
 int nrh,	/* Row high index */
@@ -1179,7 +1200,7 @@ int nch
 }
 
 /* --------------------- */
-/* 2D vector copy */
+/* matrix copy */
 void copy_dmatrix(
 double **dst,
 double **src,
@@ -1713,6 +1734,19 @@ void matrix_trans(double **d, double **s, int nr,  int nc) {
 	}
 }
 
+/* Transpose a 0 base symetrical matrix in place */
+void sym_matrix_trans(double **m, int n) {
+	int i, j;
+
+	for (i = 0; i < n; i++) {
+		for (j = i+1; j < n; j++) {
+			double tt = m[j][i]; 
+			m[j][i] = m[i][j];
+			m[i][j] = tt;
+		}
+	}
+}
+
 /* Multiply two 0 based matricies */
 /* Return nz on matching error */
 int matrix_mult(
@@ -1746,20 +1780,260 @@ int matrix_mult(
 	return 0;
 }
 
-/* Diagnostic - print to g_log debug */
-void matrix_print(char *c, double **a, int nr,  int nc) {
+/* Matrix multiply transpose of s1 by s2 */
+/* 0 based matricies,  */
+/* This is usefull for using results of lu_invert() */
+int matrix_trans_mult(
+	double **d,  int nr,  int nc,
+	double **ts1, int nr1, int nc1,
+	double **s2, int nr2, int nc2
+) {
+	int i, j, k;
+
+	/* s1 and s2 must mesh */
+	if (nr1 != nr2)
+		return 1;
+
+	/* Output rows = s1 rows */
+	if (nr != nc1)
+		return 2;
+
+	/* Output colums = s2 columns */
+	if (nc != nc2)
+		return 2;
+
+	for (i = 0; i < nc1; i++) {
+		for (j = 0; j < nc2; j++) { 
+			d[i][j] = 0.0;  
+			for (k = 0; k < nr1; k++) {
+				d[i][j] += ts1[k][i] * s2[k][j];
+			}
+		}
+	}
+
+	return 0;
+}
+
+/* Multiply a 0 based matrix by a vector */
+/* d may be same as v */
+int matrix_vect_mult(
+	double *d, int nd,
+	double **m, int nr, int nc,
+	double *v, int nv
+) {
 	int i, j;
-	a1logd(g_log, 0, "%s, %d x %d\n",c,nr,nc);
+	double *_v = v, vv[20];
 
-	for (j = 0; j < nr; j++) {
-		a1logd(g_log, 0, " ");
-		for (i = 0; i < nc; i++) {
-			a1logd(g_log, 0, " %.2f",a[j][i]);
+	if (d == v) {
+		if (nv <= 20) {
+			_v = vv;
+		} else {
+			_v = dvector(0, nv-1);
 		}
+		for (j = 0; j < nv; j++)
+			_v[j]  = v[j];
+	}
+
+	/* Input vector must match matrix columns */
+	if (nv != nc)
+		return 1;
+
+	/* Output vector must match matrix rows */
+	if (nd != nr)
+		return 1;
+
+	for (i = 0; i < nd; i++) {
+		d[i] = 0.0;  
+		for (j = 0; j < nv; j++)
+			d[i] += m[i][j] * _v[j];
+	}
+
+	if (_v != v && _v != vv)
+		free_dvector(_v, 0, nv-1);
+
+	return 0;
+}
+
+
+/* Set zero based dvector */
+void vect_set(double *d, double v, int len) {
+	int i;
+	for (i = 0; i < len; i++)
+		d[i] = v;
+}
+
+/* Copy zero based dvector */
+void vect_cpy(double *d, double *s, int len) {
+	int i;
+	for (i = 0; i < len; i++)
+		d[i] = s[i];
+}
+
+
+/* Negate and copy a vector, d = -v */
+/* d may be same as v */
+void vect_neg(double *d, double *s, int len) {
+	int i;
+	for (i = 0; i < len; i++)
+		d[i] = -s[i];
+}
+
+/* Add two vectors */
+/* d may be same as v */
+void vect_add(
+	double *d,
+	double *v, int len
+) {
+	int i;
+
+	for (i = 0; i < len; i++)
+		d[i] += v[i];
+}
+
+/* Subtract two vectors, d -= v */
+/* d may be same as v */
+void vect_sub(
+	double *d, double *v, int len
+) {
+	int i;
+	for (i = 0; i < len; i++)
+		d[i] -= v[i];
+}
+
+
+/* - - - - - - - - - - - - - - - - - - - - - - */
+
+/* Print double matrix to g_log debug */
+/* id identifies matrix */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_dmatrix(a1log *log, char *id, char *pfx, double **a, int nr,  int nc) {
+	int i, j;
+	a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc);
+
+	for (j = 0; j < nr; j++) {
+		a1logd(g_log, 0, "%s ",pfx);
+		for (i = 0; i < nc; i++)
+			a1logd(g_log, 0, "%f%s",a[j][i], i < (nc-1) ? ", " : "");
+		a1logd(g_log, 0, "\n");
+	}
+}
+
+/* Print float matrix to g_log debug */
+/* id identifies matrix */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_fmatrix(a1log *log, char *id, char *pfx, float **a, int nr,  int nc) {
+	int i, j;
+	a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc);
+
+	for (j = 0; j < nr; j++) {
+		a1logd(g_log, 0, "%s ",pfx);
+		for (i = 0; i < nc; i++)
+			a1logd(g_log, 0, "%f%s",a[j][i], i < (nc-1) ? ", " : "");
 		a1logd(g_log, 0, "\n");
 	}
 }
 
+/* Print int matrix to g_log debug */
+/* id identifies matrix */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_imatrix(a1log *log, char *id, char *pfx, int **a, int nr,  int nc) {
+	int i, j;
+	a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc);
+
+	for (j = 0; j < nr; j++) {
+		a1logd(g_log, 0, "%s ",pfx);
+		for (i = 0; i < nc; i++)
+			a1logd(g_log, 0, "%d%s",a[j][i], i < (nc-1) ? ", " : "");
+		a1logd(g_log, 0, "\n");
+	}
+}
+
+/* Print short matrix to g_log debug */
+/* id identifies matrix */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_smatrix(a1log *log, char *id, char *pfx, short **a, int nr,  int nc) {
+	int i, j;
+	a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc);
+
+	for (j = 0; j < nr; j++) {
+		a1logd(g_log, 0, "%s ",pfx);
+		for (i = 0; i < nc; i++)
+			a1logd(g_log, 0, "%d%s",a[j][i], i < (nc-1) ? ", " : "");
+		a1logd(g_log, 0, "\n");
+	}
+}
+
+/* Print double vector to g_log debug */
+/* id identifies vector */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_dvector(a1log *log, char *id, char *pfx, double *a, int nc) {
+	int i;
+	a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc);
+	a1logd(g_log, 0, "%s ",pfx);
+	for (i = 0; i < nc; i++)
+		a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : "");
+	a1logd(g_log, 0, "\n");
+}
+
+/* Print float vector to g_log debug */
+/* id identifies vector */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_fvector(a1log *log, char *id, char *pfx, float *a, int nc) {
+	int i;
+	a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc);
+	a1logd(g_log, 0, "%s ",pfx);
+	for (i = 0; i < nc; i++)
+		a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : "");
+	a1logd(g_log, 0, "\n");
+}
+
+/* Print int vector to g_log debug */
+/* id identifies vector */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_ivector(a1log *log, char *id, char *pfx, int *a, int nc) {
+	int i;
+	a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc);
+	a1logd(g_log, 0, "%s ",pfx);
+	for (i = 0; i < nc; i++)
+		a1logd(g_log, 0, "%d%s",a[i], i < (nc-1) ? ", " : "");
+	a1logd(g_log, 0, "\n");
+}
+
+/* Print short vector to g_log debug */
+/* id identifies vector */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc) {
+	int i;
+	a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc);
+	a1logd(g_log, 0, "%s ",pfx);
+	for (i = 0; i < nc; i++)
+		a1logd(g_log, 0, "%d%s",a[i], i < (nc-1) ? ", " : "");
+	a1logd(g_log, 0, "\n");
+}
+
+/* Print C double matrix to g_log debug */
+/* id identifies matrix */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+void adump_C_dmatrix(a1log *log, char *id, char *pfx, double *a, int nr, int nc) {
+	int i, j;
+	a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc);
+
+	for (j = 0; j < nr; j++, a += nc) {
+		a1logd(g_log, 0, "%s ",pfx);
+		for (i = 0; i < nc; i++)
+			a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : "");
+		a1logd(g_log, 0, "\n");
+	}
+}
 
 /*******************************************/
 /* Platform independent IEE754 conversions */
@@ -2286,10 +2560,12 @@ void msec_sleep(unsigned int msec) {
 }
 
 
-#if defined(__APPLE__) && !defined(CLOCK_MONOTONIC)
+#if defined(__APPLE__) /* && !defined(CLOCK_MONOTONIC) */
 
 #include <mach/mach_time.h>
 
+/* Return the current time in msec */
+/* since the first invokation of msec_time() */
 unsigned int msec_time() {
     mach_timebase_info_data_t timebase;
     static uint64_t startup = 0;
@@ -2400,6 +2676,9 @@ char *debPiv(int di, int *p) {
 	int e;
 	char *bp;
 
+	if (p == NULL)
+		return "(null)";
+
 	if (++ix >= 5)
 		ix = 0;
 	bp = buf[ix];
@@ -2418,11 +2697,14 @@ char *debPiv(int di, int *p) {
 /* Print a double color vector to a string. */
 /* Returned static buffer is re-used every 5 calls. */
 char *debPdv(int di, double *p) {
-	static char buf[5][DEB_MAX_CHAN * 16];
+	static char buf[5][DEB_MAX_CHAN * 100];
 	static int ix = 0;
 	int e;
 	char *bp;
 
+	if (p == NULL)
+		return "(null)";
+
 	if (++ix >= 5)
 		ix = 0;
 	bp = buf[ix];
@@ -2441,11 +2723,14 @@ char *debPdv(int di, double *p) {
 /* Print a float color vector to a string. */
 /* Returned static buffer is re-used every 5 calls. */
 char *debPfv(int di, float *p) {
-	static char buf[5][DEB_MAX_CHAN * 16];
+	static char buf[5][DEB_MAX_CHAN * 100];
 	static int ix = 0;
 	int e;
 	char *bp;
 
+	if (p == NULL)
+		return "(null)";
+
 	if (++ix >= 5)
 		ix = 0;
 	bp = buf[ix];
diff --git a/numlib/numsup.h b/numlib/numsup.h
index 9d55daf..86ad6a8 100644..100755
--- a/numlib/numsup.h
+++ b/numlib/numsup.h
@@ -78,6 +78,14 @@
 # endif
 #endif
 
+#ifndef INLINE
+# ifdef _MSC_VER
+#  define INLINE __inline
+# else
+#  define INLINE inline
+# endif
+#endif
+
 #else  /* !__STDC_VERSION__ */
 
 #ifdef _MSC_VER
@@ -100,6 +108,10 @@
 # define ATTRIBUTE_NORETURN __declspec(noreturn)
 #endif
 
+#ifndef INLINE
+# define INLINE __inline
+#endif
+
 #else  /* !_MSC_VER */
 
 /* The following works on a lot of modern systems, including */
@@ -131,6 +143,9 @@
 #ifndef ATTRIBUTE_NORETURN
 # define ATTRIBUTE_NORETURN __attribute__((noreturn))
 #endif
+#ifndef INLINE
+#  define INLINE inline
+#endif /* INLINE */
 
 #endif /* !_MSC_VER */
 #endif /* !__STDC_VERSION__ */
@@ -299,6 +314,12 @@ a1log *new_a1log_d(a1log *log);
 /* Returns NULL */
 a1log *del_a1log(a1log *log);
 
+/* Set the debug logging level. */
+void a1log_debug(a1log *log, int level);
+
+/* Set the vebosity level. */
+void a1log_verb(a1log *log, int level);
+
 /* Set the tag. Note that the tag string is NOT copied, just referenced */
 void a1log_tag(a1log *log, char *tag);
 
@@ -398,6 +419,7 @@ void free_dhmatrix(double **m, int nrl, int nrh, int ncl, int nch);
 void copy_dmatrix(double **dst, double **src, int nrl, int nrh, int ncl, int nch);
 void copy_dmatrix_to3x3(double dst[3][3], double **src, int nrl, int nrh, int ncl, int nch);
 
+/* Convert from C matrix to matrix */
 double **convert_dmatrix(double *a,int nrl,int nrh,int ncl,int nch);
 void free_convert_dmatrix(double **m,int nrl,int nrh,int ncl,int nch);
 
@@ -437,6 +459,9 @@ void free_smatrix(short **m,int nrl,int nrh,int ncl,int nch);
 /* Transpose a 0 base matrix */
 void matrix_trans(double **d, double **s, int nr,  int nc);
 
+/* Transpose a 0 base symetrical matrix in place */
+void sym_matrix_trans(double **m, int n);
+
 /* Matrix multiply 0 based matricies */
 int matrix_mult(
 	double **d,  int nr,  int nc,
@@ -444,8 +469,63 @@ int matrix_mult(
 	double **s2, int nr2, int nc2
 );
 
-/* Diagnostic */
-void matrix_print(char *c, double **a, int nr,  int nc);
+/* Matrix multiply transpose of s1 by s2 */
+/* 0 based matricies,  */
+/* This is usefull for using results of lu_invert() */
+int matrix_trans_mult(
+	double **d,  int nr,  int nc,
+	double **ts1, int nr1, int nc1,
+	double **s2, int nr2, int nc2
+);
+
+/* Multiply a 0 based matrix by a vector */
+/* d may be same as v */
+int matrix_vect_mult(
+	double *d, int nd,
+	double **m, int nr, int nc,
+	double *v, int nv
+);
+
+/* Set zero based dvector */
+void vect_set(double *d, double v, int len);
+
+/* Copy zero based dvector */
+void vect_cpy(double *d, double *s, int len);
+
+/* Negate and copy a vector, d = -v */
+/* d may be same as v */
+void vect_neg(double *d, double *s, int len);
+
+/* Add two vectors, d += v */
+/* d may be same as v */
+void vect_add(
+	double *d, double *v, int len
+);
+
+/* Subtract two vectors, d -= v */
+/* d may be same as v */
+void vect_sub(
+	double *d, double *v, int len
+);
+
+
+/* Diagnostics */
+/* id identifies matrix/vector */
+/* pfx used at start of each line */
+/* Assumed indexed from 0 */
+
+void adump_dmatrix(a1log *log, char *id, char *pfx, double **a, int nr,  int nc);
+void adump_fmatrix(a1log *log, char *id, char *pfx, float **a, int nr,  int nc);
+void adump_imatrix(a1log *log, char *id, char *pfx, int **a, int nr,  int nc);
+void adump_smatrix(a1log *log, char *id, char *pfx, short **a, int nr,  int nc);
+
+void adump_dvector(a1log *log, char *id, char *pfx, double *a, int nc);
+void adump_fvector(a1log *log, char *id, char *pfx, float *a, int nc);
+void adump_ivector(a1log *log, char *id, char *pfx, int *a, int nc);
+void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc);
+
+/* Dump C type matrix */
+void adump_C_dmatrix(a1log *log, char *id, char *pfx, double *a, int nr,  int nc);
 
 /* =========================================================== */
 
@@ -547,11 +627,11 @@ double usec_time();
 /* Returned static buffer is re-used every 5 calls. */
 char *debPiv(int di, int *p);
 
-/* Print a double color vector to a string. */
+/* Print a double vector to a string. */
 /* Returned static buffer is re-used every 5 calls. */
 char *debPdv(int di, double *p);
 
-/* Print a float color vector to a string. */
+/* Print a float vector to a string. */
 /* Returned static buffer is re-used every 5 calls. */
 char *debPfv(int di, float *p);
 
diff --git a/numlib/powell.c b/numlib/powell.c
index 5d2adac..47acc15 100644..100755
--- a/numlib/powell.c
+++ b/numlib/powell.c
@@ -42,18 +42,34 @@
 
 #undef SLOPE_SANITY_CHECK		/* exermental */
 #undef ABSTOL					/* Make tollerance absolute */
-#undef DEBUG					/* Some debugging printfs (not comprehensive) */
+								/* Some debugging printfs (not comprehensive): */
+#undef PDEBUG					/* Powell debug */
+#undef CDEBUG					/* Conjgrad debug */
+#undef LDEBUG					/* Line min debug */
+
+#if defined(PDEBUG)
+# undef PDBG
+# define PDBG(xxx) printf xxx ;
+#else
+# undef PDBG
+# define PDBG(xxx) 
+#endif
 
-#ifdef DEBUG
-#undef DBG
-#define DBG(xxx) printf xxx ;
+#if defined(CDEBUG)
+# undef CDBG
+# define CDBG(xxx) printf xxx ;
 #else
-#undef DBG
-#define DBG(xxx) 
+# undef CDBG
+# define CDBG(xxx) 
 #endif
 
-static double linmin(double p[], double xi[], int n, double ftol,
-	double (*func)(void *fdata, double tp[]), void *fdata);
+#if defined(LDEBUG)
+# undef LDBG
+# define LDBG(xxx) printf xxx ;
+#else
+# undef LDBG
+# define LDBG(xxx) 
+#endif
 
 /* Standard interface for powell function */
 /* return 0 on sucess, 1 on failure due to excessive itterions */
@@ -120,7 +136,7 @@ void *pdata				/* Opaque data needed by prog() */
 		/* Loop over all directions in the set */
 		for (i = 0; i < di; i++) {
 
-			DBG(("Looping over direction %d\n",i))
+			PDBG(("Looping over direction %d\n",i))
 
 			for (j = 0; j < di; j++)	/* Extract this direction to make search vector */
 				svec[j] = dmtx[j][i];
@@ -161,10 +177,10 @@ void *pdata				/* Opaque data needed by prog() */
 		/* reached a suitable tollerance, then finish */
 		if (iter > 1 && curdel <= stopth) {
 //printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth);
-			DBG(("Reached stop tollerance because curdel %f <= stopth %f\n",curdel,stopth))
+			PDBG(("Reached stop tollerance because curdel %f <= stopth %f\n",curdel,stopth))
 			break;
 		}
-		DBG(("Not stopping because curdel %f > stopth %f\n",curdel,stopth))
+		PDBG(("Not stopping because curdel %f > stopth %f\n",curdel,stopth))
 
 //printf("~1 ### recomputing direction\n");
 		for (i = 0; i < di; i++) {
@@ -212,7 +228,7 @@ void *pdata				/* Opaque data needed by prog() */
 	if (iter < maxit)
 		return 0;
 
-	DBG(("powell: returning 1 due to excessive itterations\n"))
+	PDBG(("powell: returning 1 due to excessive itterations\n"))
 	return 1;		/* Failed due to execessive itterations */
 }
 
@@ -225,7 +241,7 @@ void *pdata				/* Opaque data needed by prog() */
 int conjgrad(
 double *rv,				/* If not NULL, return the residual error */
 int di,					/* Dimentionality */
-double cp[],			/* Initial starting point */
+double cp[],			/* Initial starting point and return value */
 double s[],				/* Size of initial search area */
 #ifdef ABSTOL
 double ftol,			/* Absolute tollerance of error change to stop on */
@@ -257,7 +273,7 @@ void *pdata				/* Opaque data needed by prog() */
 	if (prog != NULL)		/* Report initial progress */
 		prog(pdata, pc);
 
-	/* Initial function evaluation */
+	/* Initial gradient function evaluation */
 	retv = (*dfunc)(fdata, svec, cp);
 
 	/* svec[] seems to be large after this. */
@@ -272,21 +288,22 @@ void *pdata				/* Opaque data needed by prog() */
 	else
 		svec_sca = 1.0/svec_sca;
 
-//printf("~1 ### initial dir = %f %f\n", svec[0],svec[1]);
-//printf("~1 ### initial retv = %f\n",retv);
+	CDBG((" initial dir = %s\n", debPdv(di,svec)));
+	CDBG((" initial retv = %f\n",retv));
+
 	/* Initial vector setup */
 	for (i = 0; i < di; i++) {
 		gvec[i] = hvec[i] = -svec[i];			/* Inverse gradient */
 		svec[i] = s[i] * -svec[i] * svec_sca;	/* Scale the search vector */
 	}
-//printf("~1 ### svec = %f %f\n", svec[0],svec[1]);
+	CDBG(("Initial svec = %s\n", debPdv(di,svec)));
 
 	/* Itterate untill we converge on a solution, or give up. */
 	for (iter = 1; iter < maxit; iter++) {
 		double gamden, gamnum, gam;
 		double pretv;			/* Previous function return value */
 
-		DBG(("conjrad: about to do linmin\n"))
+		CDBG(("conjrad: about to do linmin\n"))
 		pretv = retv;
 		retv = linmin(cp, svec, di, ftol, func, fdata);
 
@@ -296,7 +313,7 @@ void *pdata				/* Opaque data needed by prog() */
 		stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON);		// Old code
 #endif
 		curdel = fabs(pretv - retv);
-//printf("~1 ### this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel);
+		CDBG((" this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel));
 		if (startdel < 0.0) {
 			startdel = curdel;
 		} else {
@@ -312,31 +329,30 @@ void *pdata				/* Opaque data needed by prog() */
 		/* If we have had at least one change of direction and */
 		/* reached a suitable tollerance, then finish */
 		if (iter > 1 && curdel <= stopth) {
-//printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth);
+			CDBG((" stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth));
 			break;
 		}
-//printf("~1 ### Not stopping on itter %d because curdel %f > stopth %f\n",iter, curdel,stopth);
+		CDBG((" not stopping on itter %d because curdel %f > stopth %f\n",iter, curdel,stopth));
 
-		DBG(("conjrad: recomputing direction\n"))
-//printf("~1 ### recomputing direction\n");
+		CDBG(("conjrad: recomputing direction\n"))
 		(*dfunc)(fdata, svec, cp);		/* (Don't use retv as it wrecks stop test) */
+		CDBG((" pderiv = %s\n", debPdv(di,svec)));
 
-//printf("~1 ### pderiv = %f %f\n", svec[0],svec[1]);
 		/* Compute gamma */
 		for (gamnum = gamden = 0.0, i = 0; i < di; i++) {
 			gamnum += svec[i] * (gvec[i] + svec[i]);
 			gamden += gvec[i] * gvec[i];
 		}
 
-//printf("~1 ### gamnum = %f, gamden = %f\n", gamnum,gamden);
+		CDBG((" gamnum = %f, gamden = %f\n", gamnum,gamden));
 		if (gamden == 0.0) {		/* Gradient is exactly zero */
-			DBG(("conjrad: gradient is exactly zero\n"))
+			CDBG(("conjrad: gradient is exactly zero\n"))
 			break;
 		}
 
 		gam = gamnum/gamden;
-		DBG(("conjrad: gamma = %f = %f/%f\n",gam,gamnum,gamden))
-//printf("~1 ### gvec[] = %f %f, gamma = %f, hvec = %f %f\n", gvec[0],gvec[1],gam,hvec[0],hvec[1]);
+		CDBG(("conjrad: gamma = %f = %f/%f\n",gam,gamnum,gamden));
+		CDBG((" gvec[] = %s, hvec = %s\n", debPdv(di,gvec),debPdv(di,hvec)));
 
 		/* Adjust seach direction */
 		for (i = 0; i < di; i++) {
@@ -357,8 +373,7 @@ void *pdata				/* Opaque data needed by prog() */
 			svec_sca = 1.0/svec_sca;
 		for (i = 0; i < di; i++)
 			svec[i] = svec[i] * s[i] * svec_sca;
-//printf("~1 ### svec = %f %f\n", svec[0],svec[1]);
-
+		CDBG((" svec = %s\n", debPdv(di,svec)));
 	}
 	/* Free up all the temporary vectors and matrix */
 	free_dvector(hvec,0,di-1);
@@ -371,7 +386,7 @@ void *pdata				/* Opaque data needed by prog() */
 	if (rv != NULL)
 		*rv = retv;
 
-//printf("~1 ### done\n");
+	CDBG((" conjgrad returning = %f\n", retv));
 
 	if (iter < maxit)
 		return 0;
@@ -386,7 +401,7 @@ void *pdata				/* Opaque data needed by prog() */
 
 /* Line bracketing and minimisation routine. */
 /* Return value at minimum. */
-static double linmin(
+double linmin(
 double cp[],		/* Start point, and returned value */
 double xi[],		/* Search vector */
 int di,				/* Dimensionality */
@@ -411,7 +426,7 @@ void *fdata)		/* Opaque data for func() */
 	/* -------------------------- */
 	/* First bracket the solution */
 
-	DBG(("linmin: Bracketing solution\n"))
+	LDBG(("linmin: Bracketing solution\n"))
 
 	/* The line is measured as startpoint + offset * search vector. */
 	/* (Search isn't symetric, but it seems to depend on cp being */
@@ -427,7 +442,7 @@ void *fdata)		/* Opaque data for func() */
 		xt[i] = cp[i] + xx * xi[i];
 	xf = (*func)(fdata, xt);
 
-	DBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
+	LDBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
 
 	/* Fix it so that we are decreasing from point a -> x */
 	if (xf > af) {
@@ -435,21 +450,21 @@ void *fdata)		/* Opaque data for func() */
 		tt = ax; ax = xx; xx = tt;
 		tt = af; af = xf; xf = tt;
 	}
-	DBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
+	LDBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
 
 	bx = xx + POWELL_GOLD * (xx-ax);	/* Guess b beyond a -> x */
 	for (i = 0; i < di; i++)
 		xt[i] = cp[i] + bx * xi[i];
 	bf = (*func)(fdata, xt);
 
-	DBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+	LDBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
 
 #ifdef SLOPE_SANITY_CHECK
 	/* If we're not seeing a slope indicitive of progress */
 	/* of order ftol, give up straight away */
 	if (2000.0 * fabs(xf - bf) <= ftol * (fabs(xf) + fabs(bf))
 	 && 2000.0 * fabs(af - xf) <= ftol * (fabs(af) + fabs(xf))) {
-		DBG(("linmin: giving up because slope is too shallow\n"))
+		LDBG(("linmin: giving up because slope is too shallow\n"))
 		if (xt != XT)
 			free_dvector(xt,0,di-1);
 
@@ -470,9 +485,9 @@ void *fdata)		/* Opaque data for func() */
 		double ulim, ux, uf;
 		double tt, r, q;
 
-//		DBG(("linmin: Not bracketed a:%f:%f x:%f%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
-		DBG(("linmin: Not bracketed because xf %f > bf %f\n",xf, bf))
-		DBG(("        ax = %f, xx = %f, bx = %f\n",ax,xx,bx))
+//		LDBG(("linmin: Not bracketed a:%f:%f x:%f%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+		LDBG(("linmin: Not bracketed because xf %f > bf %f\n",xf, bf))
+		LDBG(("        ax = %f, xx = %f, bx = %f\n",ax,xx,bx))
 
 		/* Compute ux by parabolic interpolation from a, x & b */
 		q = (xx - bx) * (xf - af);
@@ -544,7 +559,7 @@ void *fdata)		/* Opaque data for func() */
 		bx = ux; bf = uf;
 //printf("~1 move along to the right (a<-x, x<-b, b-<u)\n");
 	}
-	DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+	LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
 	/* Got bracketed minimum between a -> x -> b */
 //printf("~1 got bracketed minimum at %f (%f), %f (%f), %f (%f)\n",ax,af,xx,xf,bx,bf);
 
@@ -581,12 +596,12 @@ void *fdata)		/* Opaque data for func() */
 #endif
 			double tol2 = 2.0 * tol1;
 
-			DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+			LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
 
 			/* See if we're done */
 //printf("~1 linmin check %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax));
 			if (fabs(xx - mx) <= (tol2 - 0.5 * (bx - ax))) {
-				DBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)))
+				LDBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)))
 				break;
 			}
 
@@ -603,13 +618,13 @@ void *fdata)		/* Opaque data for func() */
 				te = e;				/* Save previous e value */
 				e = de;				/* Previous steps distance moved */
 
-				DBG(("linmin: Trial parabolic fit\n" ))
+				LDBG(("linmin: Trial parabolic fit\n" ))
 
 				if (fabs(p) >= fabs(0.5 * q * te) || p <= q * (ax-xx) || p >= q * (bx-xx)) {
 					/* Give up on the parabolic fit, and use the golden section search */
 					e = ((xx >= mx) ? ax-xx : bx-xx);	/* Override previous distance moved */
 					de = POWELL_CGOLD * e;
-					DBG(("linmin: Moving to golden section search\n" ))
+					LDBG(("linmin: Moving to golden section search\n" ))
 				} else {	/* Use parabolic fit */
 					de = p/q;			/* Change in xb */
 					ux = xx + de;		/* Trial point according to parabolic fit */
@@ -619,24 +634,24 @@ void *fdata)		/* Opaque data for func() */
 						else
 							de = -tol1;
 					}
-					DBG(("linmin: Using parabolic fit\n" ))
+					LDBG(("linmin: Using parabolic fit\n" ))
 				}
 			} else {	/* Keep using the golden section search */
 				e = ((xx >= mx) ? ax-xx : bx-xx);	/* Override previous distance moved */
 				de = POWELL_CGOLD * e;
-				DBG(("linmin: Continuing golden section search\n" ))
+				LDBG(("linmin: Continuing golden section search\n" ))
 			}
 
 			if (fabs(de) >= tol1) {		/* If de moves as much as tol1 would */
 				ux = xx + de;			/* use it */
-				DBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de))
+				LDBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de))
 			} else {					/* else move by tol1 in direction de */
 				if (de > 0.0) {
 					ux = xx + tol1;
-					DBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1))
+					LDBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1))
 				} else {
 					ux = xx - tol1;
-					DBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1))
+					LDBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1))
 				}
 			}
 
@@ -646,6 +661,7 @@ void *fdata)		/* Opaque data for func() */
 			uf = (*func)(fdata, xt);
 
 			if (uf <= xf) {					/* Found new best solution */
+				LDBG(("linmin: found new best solution at %f val %f\n",ux,uf))
 				if (ux >= xx) {	
 					ax = xx; af = xf;		/* New lower bracket */
 				} else {
@@ -654,8 +670,9 @@ void *fdata)		/* Opaque data for func() */
 				vx = wx; vf = wf;			/* New previous 2nd best solution */
 				wx = xx; wf = xf;			/* New 2nd best solution from previous best */
 				xx = ux; xf = uf;			/* New best solution from latest */
-				DBG(("linmin: found new best solution\n"))
 			} else {						/* Found a worse solution */
+				LDBG(("linmin: found new worse solution at %f val %f\n",ux,uf))
+				LDBG(("linmin:             current best at %f val %f\n",xx,xf))
 				if (ux < xx) {
 					ax = ux; af = uf;		/* New lower bracket */
 				} else {
@@ -667,13 +684,13 @@ void *fdata)		/* Opaque data for func() */
 				} else if (uf <= vf || vx == xx || vx == wx) {	/* New 3rd best, or equal 1st & 2nd */
 					vx = ux; vf = uf;		/* New previous 2nd best from latest */
 				}
-				DBG(("linmin: found new worse solution\n"))
 			}
 		}
 		/* !!! should do something if iter > POWELL_MAXIT !!!! */
 		/* Solution is at xx, xf */
 
 		/* Compute solution vector */
+		LDBG(("linmin: computing soln from best at %f val %f\n",xx,xf))
 		for (i = 0; i < di; i++) 
 			cp[i] += xx * xi[i];
 	}
diff --git a/numlib/powell.h b/numlib/powell.h
index a1db8b6..4b86573 100644..100755
--- a/numlib/powell.h
+++ b/numlib/powell.h
@@ -54,18 +54,20 @@ double powell_funk(		/* Return function value */
 	void *fdata,		/* Opaque data pointer */
 	double tp[]);		/* Multivriate input value */
 
-/* Line in multi-dimensional space minimiser */
-double brentnd(			/* vector multiplier return value */
-double ax,				/* Minimum of multiplier range */
-double bx,				/* Starting point multiplier of search */
-double cx,				/* Maximum of multiplier range */
-double ftol,			/* Tollerance to stop search */
-double *xmin,			/* Return value of multiplier at minimum */		
-int n,					/* Dimensionality */
+/* Line bracketing and minimisation routine. */
+/* Return value at minimum. */
+double linmin(
+double cp[],		/* Start point, and returned value */
+double xi[],		/* Search vector */
+int di,				/* Dimensionality */
+#ifdef ABSTOL
+double ftol,		/* Absolute tolerance to stop on */
+#else
+double ftol,		/* Relative tolerance to stop on */
+#endif
 double (*func)(void *fdata, double tp[]),		/* Error function to evaluate */
-void *fdata,			/* Opaque data */
-double pcom[],			/* Base vector point */
-double xicom[]);		/* Vector that will be multiplied and added to pcom[] */
+void *fdata			/* Opaque data for func() */
+);
 
 #ifdef __cplusplus
 	}
diff --git a/numlib/qptest.c b/numlib/qptest.c
new file mode 100755
index 0000000..cc826f1
--- /dev/null
+++ b/numlib/qptest.c
@@ -0,0 +1,105 @@
+/*
+
+ This file contains just an example on how to set-up the matrices for using with
+ the quadprog() function.
+ 
+ The test problem is the following:
+ 
+ Given:
+ G =  4 -2   g0^T = [6 0]
+     -2  4       
+ 
+ Solve:
+ min f(x) = 1/2 x G x + g0 x
+ s.t.
+   x_1 + x_2 = 3
+   x_1 >= 0
+   x_2 >= 0
+   x_1 + x_2 >= 2
+ 
+ The solution is x^T = [1 2] and f(x) = 12
+ 
+*/
+
+#include "stdio.h"
+#include "numlib.h"
+#include "quadprog.h"
+
+int main() {
+	double **GG, **G, **CE, **CI;
+	double *g0, *ce0, *ci0, *x;
+	int n, m, p;
+	int i, j;
+	double f, sum = 0.0;
+	
+	n = 2;
+
+	/* Orginal, unchanged G */
+	GG = dmatrix(0, n-1, 0, n-1);
+	GG[0][0] = 4.0;
+	GG[0][1] = -2.0;
+	GG[1][0] = -2.0;
+	GG[1][1] = 4.0;
+
+	/* Working copy - gets modified */
+	G = dmatrix(0, n-1, 0, n-1);
+	copy_dmatrix(G, GG, 0, n-1, 0, n-1);
+
+	g0 = dvector(0, n-1);
+	g0[0] = 6.0;
+	g0[1] = 0.0;
+
+	p = 1;
+
+	CE = dmatrix(0, n-1, 0, p-1);
+	CE[0][0] = 1.0;
+	CE[1][0] = 1.0;
+
+	ce0 = dvector(0, p-1);
+	ce0[0] = -3.0;
+	
+	m = 3;
+	CI = dmatrix(0, n-1, 0, m-1);
+	CI[0][0] = 1.0;
+	CI[0][1] = 0.0;
+	CI[0][2] = 1.0;
+	CI[1][0] = 0.0;
+	CI[1][1] = 1.0;
+	CI[1][2] = 1.0;
+	
+	ci0 = dvector(0, m-1);
+	ci0[0] = 0.0;
+	ci0[1] = 0.0;
+	ci0[2] = -2.0;
+
+	x = dvector(0, n-1);
+
+	f = quadprog(x, G, g0, CE, ce0, CI, ci0, n, p, m);
+
+	if (f == QP_INFEASIBLE)
+		printf("Failed to find solution\n");
+	else {
+		printf("Function value at %f %f = %f\n",x[0],x[1],f);
+	
+		for (i = 0; i < n; i++)
+			for (j = 0; j < n; j++)
+				sum += x[i] * GG[i][j] * x[j];
+		sum *= 0.5;	
+		
+		for (i = 0; i < n; i++)
+			sum += g0[i] * x[i];
+	
+		printf("Double check funtion value %f\n",sum);
+	}
+
+	free_dmatrix(GG, 0, n-1, 0, n-1);
+	free_dmatrix(G, 0, n-1, 0, n-1);
+	free_dvector(g0, 0, n-1);
+	free_dmatrix(CE, 0, n-1, 0, p-1);
+	free_dvector(ce0, 0, p-1);
+	free_dmatrix(CI, 0, n-1, 0, m-1);
+	free_dvector(ci0, 0, m-1);
+	free_dvector(x, 0, n-1);
+
+	return 0;
+}
diff --git a/numlib/quadprog.c b/numlib/quadprog.c
new file mode 100755
index 0000000..847601f
--- /dev/null
+++ b/numlib/quadprog.c
@@ -0,0 +1,845 @@
+
+/* 
+	Quadradic Programming soliution.
+
+	Based on Luca Di Gaspero's QuadProgpp++, made available with
+	the following license:
+
+		MIT License
+		
+		Copyright (c) 2007-2016 Luca Di Gaspero
+		
+		Permission is hereby granted, free of charge, to any person obtaining a copy
+		of this software and associated documentation files (the "Software"), to deal
+		in the Software without restriction, including without limitation the rights
+		to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+		copies of the Software, and to permit persons to whom the Software is
+		furnished to do so, subject to the following conditions:
+		
+		The above copyright notice and this permission notice shall be included in all
+		copies or substantial portions of the Software.
+		
+		THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+		IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+		FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+		AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+		LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+		OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+		SOFTWARE.
+
+	Translation to C by Graeme W. Gill, Copyright 2017, also licensed under the
+	above license.
+*/
+
+/* 
+
+ The quadprog_solve() function implements the algorithm of Goldfarb and Idnani 
+ for the solution of a (convex) Quadratic Programming problem
+ by means of an active-set dual method.
+	 
+The problem is in the form:
+
+min 0.5 * x G x + g0 x
+s.t.
+    CE^T x + ce0 = 0
+    CI^T x + ci0 >= 0
+	 
+ The matrix and vectors dimensions are as follows:
+     G: n * n
+		g0: n
+				
+		CE: n * p
+	 ce0: p
+				
+	  CI: n * m
+   ci0: m
+
+     x: n
+ 
+ References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
+             strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
+
+*/
+
+#include "numsup.h"
+#include "quadprog.h"
+
+#undef TRACE_SOLVER		/* Print progress */
+
+/* Utility functions for updating some data needed by the solution method  */
+INLINE static void compute_d(double *d, double **J, double *np, int n);
+INLINE static void update_z(double *z, double **J, double *d, int iq, int n);
+INLINE static void update_r(double **R, double *r, double *d, int iq, int n);
+static int add_constraint(double **R, double **J, double *d, int *piq, double *prnorm, int n);
+INLINE static void delete_constraint(double **R, double **J, int *A, double *u,
+	                   int n, int p, int *piq, int l);
+
+/* Utility functions for computing the Cholesky decomposition and solving */
+/* linear systems */
+static void cholesky_decomposition(double **A, int n);
+static void cholesky_solve(double **L, double *x,  double *b, int n);
+INLINE static void forward_elimination(double **L, double *y, double *b, int n);
+INLINE static void backward_elimination(double **U, double *x, double *y, int n);
+
+/* Utility functions for computing the scalar product and the euclidean  */
+/* distance between two numbers */
+INLINE static double scalar_product(double *x, double *y, int n);
+INLINE static double distance(double a, double b);
+
+/* Utility functions for printing vectors and matrices */
+static void print_matrix(char* name, double **A, int n, int m);
+
+static void print_vector(char* name, double *v, int n);
+static void print_ivector(char* name, int *v, int n);
+
+#define FMIN(A,B) ((A) < (B) ? (A) : (B))
+
+#define FMAX(A,B) ((A) > (B) ? (A) : (B))
+
+#define ASZ 10
+#define VSZ 20
+
+double quadprog(	/* Return solution cost, QP_INFEASIBLE if infeasible/error */
+    double *x,		/* Return x[n] value */
+	double **G,		/* G[n][n]	Quadratic combination matrix - modified */
+	double *g0,		/* g0[n]    Direct vector */
+    double **CE,	/* CE[n][p]	Equality constraint matrix */
+	double *ce0, 	/* ce0[p]   Equality constraing constants */ 
+	double **CI,	/* CI[n][m]	Constraint matrix */
+    double *ci0,	/* cie[m]	Constraint constants */ 
+	int n,			/* Number of variables */ 
+	int p,			/* Number of equalities */ 
+	int m			/* Number of constraints */ 
+) {
+	int i, j, k, l;	/* indices */
+	int ip;					 	/* index of the constraint to be added to the active set */
+	double f_value = QP_INFEASIBLE, psi, c1, c2, sum, ss, R_norm;
+	double inf;
+	double t, t1, t2;		/* t is the step length, which is the minimum of the partial */
+							/* step length t1 and the full step length t2 */
+	int q, iq, iter = 0;
+
+	double **R, **J;
+	double *s, *z, *r, *d, *np, *u, *x_old, *u_old;
+	int *A, *A_old, *iai;
+	short *iaexcl;
+	
+	double *_R[ASZ], __R[ASZ][ASZ], *_J[ASZ], __J[ASZ][ASZ];
+	double _s[VSZ], _z[VSZ], _r[VSZ], _d[VSZ], _np[VSZ], _u[VSZ], _x_old[VSZ], _u_old[VSZ];
+	int _A[VSZ], _A_old[VSZ], _iai[VSZ];
+	short _iaexcl[VSZ];
+	
+	/* Avoid malloc's if we can.. */
+	if (n <= ASZ) {
+		R = _R;
+		J = _J;
+		for (i = 0; i < n; i++) {
+			_R[i] = __R[i];
+			_J[i] = __J[i];
+		}
+	} else {
+		R = dmatrix(0, n-1, 0, n-1);
+		J = dmatrix(0, n-1, 0, n-1);
+	}
+
+	if (n <= VSZ) {
+		x_old = _x_old;
+		z = _z;
+		d = _d;
+		np = _np;
+	} else {
+		x_old = dvector(0, n-1);
+		z = dvector(0, n-1);
+		d = dvector(0, n-1);
+		np = dvector(0, n-1);
+	}
+
+	if ((m + p) <= VSZ) {
+		s = _s;
+		r = _r;
+		u = _u;
+		u_old = _u_old;
+		A = _A;
+		A_old = _A_old;
+		iai = _iai;
+		iaexcl = _iaexcl;
+	} else {
+		s = dvector(0, m + p-1);
+		r = dvector(0, m + p-1);
+		u = dvector(0, m + p-1);
+		u_old = dvector(0, m + p-1);
+		A = ivector(0, m + p-1);
+		A_old = ivector(0, m + p-1);
+		iai = ivector(0, m + p-1);
+		iaexcl = svector(0, m + p-1);
+	}
+
+	q = 0;	/* size of the active set A (containing the indices of the active constraints) */
+
+#ifdef TRACE_SOLVER
+	printf("\nStarting solve_quadprog\n");
+	print_matrix("G", G, n, n);
+	print_vector("g0", g0, n);
+	print_matrix("CE", CE, n, p);
+	print_vector("ce0", ce0, p);
+	print_matrix("CI", CI, n, m);
+	print_vector("ci0", ci0, m);
+#endif	
+	
+	/*
+	 * Preprocessing phase
+	 */
+	
+	/* compute the trace of the original matrix G */
+	c1 = 0.0;
+	for (i = 0; i < n; i++)
+		c1 += G[i][i];
+	/* decompose the matrix G in the form L^T L */
+	cholesky_decomposition(G, n);
+#ifdef TRACE_SOLVER
+	print_matrix("G", G, n, n);
+#endif
+
+	/* initialize the matrix R */
+	for (i = 0; i < n; i++) {
+		d[i] = 0.0;
+		for (j = 0; j < n; j++)
+			R[i][j] = 0.0;
+	}
+	R_norm = 1.0;	/* this variable will hold the norm of the matrix R */
+	
+	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
+	c2 = 0.0;
+	for (i = 0; i < n; i++) {
+		d[i] = 1.0;
+		forward_elimination(G, z, d, n);
+		for (j = 0; j < n; j++)
+			J[i][j] = z[j];
+		c2 += z[i];
+		d[i] = 0.0;
+	}
+#ifdef TRACE_SOLVER
+	print_matrix("J", J, n, n);
+#endif
+	
+	/* c1 * c2 is an estimate for cond(G) */
+	
+	/* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x  */
+	/* this is a feasible point in the dual space */
+	/* x = G^-1 * g0 */
+	cholesky_solve(G, x, g0, n);
+	for (i = 0; i < n; i++)
+		x[i] = -x[i];
+	/* and compute the current solution value */ 
+	f_value = 0.5 * scalar_product(g0, x, n);
+#ifdef TRACE_SOLVER
+	printf("Unconstrained solution: %f",f_value);
+	print_vector("x", x, n);
+#endif
+	
+	/* Add equality constraints to the working set A */
+	iq = 0;
+	for (i = 0; i < p; i++) {
+		for (j = 0; j < n; j++)
+			np[j] = CE[j][i];
+		compute_d(d, J, np, n);
+		update_z(z, J, d, iq, n);
+		update_r(R, r, d, iq, n);
+#ifdef TRACE_SOLVER
+		print_matrix("R", R, n, n);
+		print_vector("z", z, n);
+		print_vector("r", r, iq);
+		print_vector("d", d, n);
+#endif
+		
+		/* compute full step length t2: i.e., the minimum step in primal space s.t. */
+		/* the contraint becomes feasible */
+		t2 = 0.0;
+		if (fabs(scalar_product(z, z, n)) > DBL_EPSILON) /* i.e. z != 0 */
+			t2 = (-scalar_product(np, x, n) - ce0[i]) / scalar_product(z, np, n);
+		
+		/* set x = x + t2 * z */
+		for (k = 0; k < n; k++)
+			x[k] += t2 * z[k];
+		
+		/* set u = u+ */
+		u[iq] = t2;
+		for (k = 0; k < iq; k++)
+			u[k] -= t2 * r[k];
+		
+		/* compute the new solution value */
+		f_value += 0.5 * (t2 * t2) * scalar_product(z, np, n);
+		A[i] = -i - 1;
+		
+		if (!add_constraint(R, J, d, &iq, &R_norm, n)) {		
+			/* Equality constraints are linearly dependent */
+#ifdef TRACE_SOLVER
+			printf("Constraints are linearly dependent\n");
+#endif
+			goto done;
+		}
+	}
+	
+	/* set iai = K \ A */
+	for (i = 0; i < m; i++)
+		iai[i] = i;
+
+	/* Hmm. Use of crossing goto's is a little Fortran like... */
+
+l1:	iter++;
+#ifdef TRACE_SOLVER
+	print_vector("x", x, n);
+#endif
+	/* step 1: choose a violated constraint */
+	for (i = p; i < iq; i++) {
+		ip = A[i];
+		iai[ip] = -1;
+	}
+	
+	/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
+	ss = 0.0;
+	psi = 0.0;	/* this value will contain the sum of all infeasibilities */
+	ip = 0;		/* ip will be the index of the chosen violated constraint */
+	for (i = 0; i < m; i++) {
+		iaexcl[i] = 1;
+		sum = 0.0;
+		for (j = 0; j < n; j++)
+			sum += CI[j][i] * x[j];
+		sum += ci0[i];
+		s[i] = sum;
+		psi += FMIN(0.0, sum);
+	}
+#ifdef TRACE_SOLVER
+	print_vector("s", s, m);
+#endif
+	
+	if (fabs(psi) <= m * DBL_EPSILON * c1 * c2* 100.0) {
+		/* numerically there are not infeasibilities anymore */
+		q = iq;
+		
+		goto done;
+	}
+	
+	/* save old values for u and A */
+	for (i = 0; i < iq; i++) {
+		u_old[i] = u[i];
+		A_old[i] = A[i];
+	}
+	
+	/* and for x */
+	for (i = 0; i < n; i++)
+		x_old[i] = x[i];
+	
+l2:	/* Step 2: check for feasibility and determine a new S-pair */
+	for (i = 0; i < m; i++) {
+		if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
+			ss = s[i];
+			ip = i;
+		}
+	}
+	if (ss >= 0.0) {
+		q = iq;
+		
+		goto done;
+	}
+	
+	/* set np = n[ip] */
+	for (i = 0; i < n; i++)
+		np[i] = CI[i][ip];
+	/* set u = [u 0]^T */
+	u[iq] = 0.0;
+	/* add ip to the active set A */
+	A[iq] = ip;
+	
+#ifdef TRACE_SOLVER
+	printf("Trying with constraint %d\n",ip);
+	print_vector("np", np, n);
+#endif
+	
+l2a:
+	/* Step 2a: determine step direction */
+	/* compute z = H np: the step direction in the primal space (through J, see the paper) */
+	compute_d(d, J, np, n);
+	update_z(z, J, d, iq, n);
+	/* compute N* np (if q > 0): the negative of the step direction in the dual space */
+	update_r(R, r, d, iq, n);
+#ifdef TRACE_SOLVER
+	printf("Step direction z\n");
+	print_vector("z", z, n);
+	print_vector("r", r, iq + 1);
+	print_vector("u", u, iq + 1);
+	print_vector("d", d, n);
+	print_ivector("A", A, iq + 1);
+#endif
+	
+	/* Step 2b: compute step length */
+	l = 0;
+	/* Compute t1: partial step length (maximum step in dual space */
+	/* without violating dual feasibility */
+	t1 = QP_INFEASIBLE; /* +inf */
+	/* find the index l s.t. it reaches the minimum of u+[x] / r */
+	for (k = p; k < iq; k++) {
+		if (r[k] > 0.0) {
+			if (u[k] / r[k] < t1) {
+				t1 = u[k] / r[k];
+				l = A[k];
+			}
+		}
+	}
+	/* Compute t2: full step length (minimum step in primal space */
+	/* such that the constraint ip becomes feasible */
+	if (fabs(scalar_product(z, z, n)) > DBL_EPSILON) { /* i.e. z != 0 */
+		t2 = -s[ip] / scalar_product(z, np, n);
+		if (t2 < 0) /* patch suggested by Takano Akio for handling numerical inconsistencies */
+			t2 = QP_INFEASIBLE;
+	} else
+		t2 = QP_INFEASIBLE; /* +inf */
+	
+	/* the step is chosen as the minimum of t1 and t2 */
+	t = FMIN(t1, t2);
+#ifdef TRACE_SOLVER
+	printf("Step sizes: %f (t1 = %f, t2 = %f\n",t,t1,t2);
+#endif
+	
+	/* Step 2c: determine new S-pair and take step: */
+	
+	/* case (i): no step in primal or dual space */
+	if (t >= QP_INFEASIBLE) {
+		/* QPP is infeasible */
+		// FIXME: unbounded to raise
+		q = iq;
+		f_value = QP_INFEASIBLE;
+		goto done;
+	}
+	/* case (ii): step in dual space */
+	if (t2 >= QP_INFEASIBLE) {
+		/* set u = u +	t * [-r 1] and drop constraint l from the active set A */
+		for (k = 0; k < iq; k++)
+			u[k] -= t * r[k];
+		u[iq] += t;
+		iai[l] = l;
+		delete_constraint(R, J, A, u, n, p, &iq, l);
+#ifdef TRACE_SOLVER
+		printf(" in dual space: %f\n",f_value);
+		print_vector("x", x, n);
+		print_vector("z", z, n);
+		print_ivector("A", A, iq + 1);
+#endif
+		goto l2a;
+	}
+	
+	/* case (iii): step in primal and dual space */
+	
+	/* set x = x + t * z */
+	for (k = 0; k < n; k++)
+		x[k] += t * z[k];
+	/* update the solution value */
+	f_value += t * scalar_product(z, np, n) * (0.5 * t + u[iq]);
+	/* u = u + t * [-r 1] */
+	for (k = 0; k < iq; k++)
+		u[k] -= t * r[k];
+	u[iq] += t;
+#ifdef TRACE_SOLVER
+	printf(" in both spaces: %f\n",f_value);
+	print_vector("x", x, n);
+	print_vector("u", u, iq + 1);
+	print_vector("r", r, iq + 1);
+	print_ivector("A", A, iq + 1);
+#endif
+	
+	if (fabs(t - t2) < DBL_EPSILON) {
+#ifdef TRACE_SOLVER
+		printf("Full step has taken %f\n",t);
+		print_vector("x", x, n);
+#endif
+		/* full step has taken */
+		/* add constraint ip to the active set*/
+		if (!add_constraint(R, J, d, &iq, &R_norm, n)) {
+			iaexcl[ip] = 0;
+			delete_constraint(R, J, A, u, n, p, &iq, ip);
+#ifdef TRACE_SOLVER
+			print_matrix("R", R, n, n);
+			print_ivector("A", A, iq);
+			print_ivector("iai", iai, iq);		// ?? iq size of m+p ?
+#endif
+			for (i = 0; i < m; i++)
+				iai[i] = i;
+			for (i = p; i < iq; i++) {
+				A[i] = A_old[i];
+				u[i] = u_old[i];
+				iai[A[i]] = -1;
+			}
+			for (i = 0; i < n; i++)
+				x[i] = x_old[i];
+			goto l2; /* go to step 2 */
+		} else
+			iai[ip] = -1;
+#ifdef TRACE_SOLVER
+		print_matrix("R", R, n, n);
+		print_ivector("A", A, iq);
+		print_ivector("iai", iai, iq);		// ?? iq size of m+p ?
+#endif
+		goto l1;
+	}
+	
+	/* a patial step has taken */
+#ifdef TRACE_SOLVER
+	printf("Partial step has taken %f\n",t);
+	print_vector("x", x, n);
+#endif
+	/* drop constraint l */
+	iai[l] = l;
+	delete_constraint(R, J, A, u, n, p, &iq, l);
+#ifdef TRACE_SOLVER
+	print_matrix("R", R, n, n);
+	print_ivector("A", A, iq);
+#endif
+	
+	/* update s[ip] = CI * x + ci0 */
+	sum = 0.0;
+	for (k = 0; k < n; k++)
+		sum += CI[k][ip] * x[k];
+	s[ip] = sum + ci0[ip];
+	
+#ifdef TRACE_SOLVER
+	print_vector("s", s, m);
+#endif
+	goto l2a;
+
+  done:;
+
+	/* Cleanup and return f value */
+	if (n > ASZ) {
+		free_dmatrix(R, 0, n-1, 0, n-1);
+		free_dmatrix(J, 0, n-1, 0, n-1);
+	}
+
+	if (n > VSZ) {
+		free_dvector(x_old, 0, n-1);
+		free_dvector(z, 0, n-1);
+		free_dvector(d, 0, n-1);
+		free_dvector(np, 0, n-1);
+	}
+
+	if ((m + p) > VSZ) {
+		free_dvector(s, 0, m + p-1);
+		free_dvector(r, 0, m + p-1);
+		free_dvector(u, 0, m + p-1);
+		free_dvector(u_old, 0, m + p-1);
+		free_ivector(A, 0, m + p-1);
+		free_ivector(A_old, 0, m + p-1);
+		free_ivector(iai, 0, m + p-1);
+		free_svector(iaexcl, 0, m + p-1);
+	}
+
+	return f_value;
+}
+
+INLINE static void compute_d(double *d, double **J, double *np, int n) {
+	int i, j;
+	double sum;
+	
+	/* compute d = H^T * np */
+	for (i = 0; i < n; i++) {
+		sum = 0.0;
+		for (j = 0; j < n; j++)
+			sum += J[j][i] * np[j];
+		d[i] = sum;
+	}
+}
+
+INLINE static void update_z(double *z, double **J, double *d, int iq, int n) {
+	int i, j;
+	
+	/* setting of z = H * d */
+	for (i = 0; i < n; i++) {
+		z[i] = 0.0;
+		for (j = iq; j < n; j++)
+			z[i] += J[i][j] * d[j];
+	}
+}
+
+INLINE static void update_r(double **R, double *r, double *d, int iq, int n) {
+	int i, j;
+	double sum;
+	
+	/* setting of r = R^-1 d */
+	for (i = iq - 1; i >= 0; i--) {
+		sum = 0.0;
+		for (j = i + 1; j < iq; j++)
+			sum += R[i][j] * r[j];
+		r[i] = (d[i] - sum) / R[i][i];
+	}
+}
+
+static int add_constraint(double **R, double **J, double *d, int *piq, double *prnorm, int n) {
+	int iq = *piq;
+	int i, j, k;
+	double cc, ss, h, t1, t2, xny;
+	
+#ifdef TRACE_SOLVER
+	printf("Add constraint %d",iq);
+#endif
+
+	/* we have to find the Givens rotation which will reduce the element */
+	/* d[j] to zero. if it is already zero we don't have to do anything, except */
+	/* of decreasing j */	
+	for (j = n - 1; j >= iq + 1; j--) {
+		/* The Givens rotation is done with the matrix (cc cs, cs -cc). */
+		/* If cc is one, then element (j) of d is zero compared with element */
+		/* (j - 1). Hence we don't have to do anything.  */
+		/* If cc is zero, then we just have to switch column (j) and column (j - 1)  */
+		/* of J. Since we only switch columns in J, we have to be careful how we */
+		/* update d depending on the sign of gs. */
+		/* Otherwise we have to apply the Givens rotation to these columns. */
+		/* The i - 1 element of d has to be updated to h. */
+		cc = d[j - 1];
+		ss = d[j];
+		h = distance(cc, ss);
+		if (fabs(h) < DBL_EPSILON)		/* h == 0 */
+			continue;
+		d[j] = 0.0;
+		ss = ss / h;
+		cc = cc / h;
+		if (cc < 0.0) {
+			cc = -cc;
+			ss = -ss;
+			d[j - 1] = -h;
+		} else
+			d[j - 1] = h;
+		xny = ss / (1.0 + cc);
+		for (k = 0; k < n; k++) {
+			t1 = J[k][j - 1];
+			t2 = J[k][j];
+			J[k][j - 1] = t1 * cc + t2 * ss;
+			J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
+		}
+	}
+	/* update the number of constraints added*/
+	*piq = ++iq;
+	/* To update R we have to put the iq components of the d vector into column iq - 1 of R */
+	for (i = 0; i < iq; i++)
+		R[i][iq - 1] = d[i];
+#ifdef TRACE_SOLVER
+	printf("/%d\n", iq);
+	print_matrix("R", R, iq, iq);
+	print_matrix("J", J, n, n);
+	print_vector("d", d, iq);
+#endif
+	
+	if (fabs(d[iq - 1]) <= DBL_EPSILON * *prnorm) {
+		/* problem degenerate */
+		return 0;
+	}
+	*prnorm = (double)FMAX(*prnorm, fabs(d[iq - 1]));
+
+	return 1;
+}
+
+static void delete_constraint(double **R, double **J, int *A, double *u,
+	                   int n, int p, int *piq, int l) {
+	int iq = *piq;
+	int i, j, k, qq = -1; // just to prevent warnings from smart compilers
+	double cc, ss, h, xny, t1, t2;
+	
+#ifdef TRACE_SOLVER
+	printf("Delete constraint %d %d",l,iq);
+#endif
+
+	/* Find the index qq for active constraint l to be removed */
+	for (i = p; i < iq; i++) {
+		if (A[i] == l) {
+			qq = i;
+			break;
+		}
+	}
+			
+	/* remove the constraint from the active set and the duals */
+	for (i = qq; i < iq - 1; i++) {
+		A[i] = A[i + 1];
+		u[i] = u[i + 1];
+		for (j = 0; j < n; j++)
+			R[j][i] = R[j][i + 1];
+	}
+			
+	A[iq - 1] = A[iq];
+	u[iq - 1] = u[iq];
+	A[iq] = 0; 
+	u[iq] = 0.0;
+	for (j = 0; j < iq; j++)
+		R[j][iq - 1] = 0.0;
+
+	/* constraint has been fully removed */
+	*piq = --iq;
+
+#ifdef TRACE_SOLVER
+	printf("/%d\n",iq);
+#endif 
+	
+	if (iq == 0)
+		return;
+	
+	for (j = qq; j < iq; j++) {
+		cc = R[j][j];
+		ss = R[j + 1][j];
+		h = distance(cc, ss);
+		if (fabs(h) < DBL_EPSILON) // h == 0
+			continue;
+		cc = cc / h;
+		ss = ss / h;
+		R[j + 1][j] = 0.0;
+		if (cc < 0.0) {
+			R[j][j] = -h;
+			cc = -cc;
+			ss = -ss;
+		} else
+			R[j][j] = h;
+		
+		xny = ss / (1.0 + cc);
+		for (k = j + 1; k < iq; k++) {
+			t1 = R[j][k];
+			t2 = R[j + 1][k];
+			R[j][k] = t1 * cc + t2 * ss;
+			R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
+		}
+		for (k = 0; k < n; k++) {
+			t1 = J[k][j];
+			t2 = J[k][j + 1];
+			J[k][j] = t1 * cc + t2 * ss;
+			J[k][j + 1] = xny * (J[k][j] + t1) - t2;
+		}
+	}
+}
+
+INLINE static double distance(double a, double b) {
+	double a1, b1, t;
+	a1 = fabs(a);
+	b1 = fabs(b);
+	if (a1 > b1) {
+		t = (b1 / a1);
+		return a1 * sqrt(1.0 + t * t);
+	} else if (b1 > a1) {
+		t = (a1 / b1);
+		return b1 * sqrt(1.0 + t * t);
+	}
+	return a1 * sqrt(2.0);
+}
+
+
+INLINE static double scalar_product(double *x, double *y, int n) {
+	int i;
+	double sum;
+	
+	sum = 0.0;
+	for (i = 0; i < n; i++)
+		sum += x[i] * y[i];
+	return sum;			
+}
+
+// !!! this doesn't work for semi-definite matricies, ie.
+// they will jave diagonal sum == 0.0 !!!
+static void cholesky_decomposition(double **A, int n) {
+	int i, j, k;
+	double sum;
+	
+	for (i = 0; i < n; i++) {
+		for (j = i; j < n; j++) {
+			sum = A[i][j];
+			for (k = i - 1; k >= 0; k--)
+				sum -= A[i][k] * A[j][k];
+#ifdef TRACE_SOLVER
+			printf("sum[%d][%d] = %f\n",i,j,sum);
+#endif
+			if (i == j) {
+				if (sum <= 0.0)
+				{
+					// raise error
+					print_matrix("A", A, n, n);
+					error("QuadProg:cholesky decomposition, matrix is not postive definite, sum: %e",sum);
+				}
+					A[i][i] = sqrt(sum);
+			} else
+				A[j][i] = sum / A[i][i];
+		}
+		for (k = i + 1; k < n; k++)
+			A[i][k] = A[k][i];
+	} 
+}
+
+static void cholesky_solve(double **L, double *x,  double *b, int n) {
+	double *y, _y[VSZ];
+	
+	if (n <= VSZ)
+		y = _y;
+	else
+		y = dvector(0, n-1);
+
+	/* Solve L * y = b */
+	forward_elimination(L, y, b, n);
+
+	/* Solve L^T * x = y */
+	backward_elimination(L, x, y, n);
+
+	if (n > VSZ)
+		free_dvector(y, 0, n-1);
+}
+
+INLINE static void forward_elimination(double **L, double *y, double *b, int n) {
+	int i, j;
+	
+	y[0] = b[0] / L[0][0];
+	for (i = 1; i < n; i++) {
+		y[i] = b[i];
+		for (j = 0; j < i; j++)
+			y[i] -= L[i][j] * y[j];
+		y[i] = y[i] / L[i][i];
+	}
+}
+
+INLINE static void backward_elimination(double **U, double *x, double *y, int n) {
+	int i, j;
+	
+	x[n - 1] = y[n - 1] / U[n - 1][n - 1];
+	for (i = n - 2; i >= 0; i--) {
+		x[i] = y[i];
+		for (j = i + 1; j < n; j++)
+			x[i] -= U[i][j] * x[j];
+		x[i] = x[i] / U[i][i];
+	}
+}
+
+/* --------------------------------------------------- */
+
+static void print_matrix(char *name, double **A, int n, int m) {
+	int i, j;
+
+	printf("%s: \n",name);
+	for (i = 0; i < n; i++) {
+		printf(" ");
+		for (j = 0; j < m; j++)
+			printf("%f%s", A[i][j], j < (m-1) ? ", " : "");
+		printf("\n");
+	}
+	printf("\n");
+}
+
+static void print_vector(char *name, double *v, int n) {
+	int i;
+
+	printf("%s: \n",name);
+	printf(" ");
+	for (i = 0; i < n; i++)
+		printf("%f%s", v[i], i < (n-1) ? ", " : "");
+	printf("\n\n");
+}
+
+static void print_ivector(char *name, int *v, int n) {
+	int i;
+
+	printf("%s: \n",name);
+	printf(" ");
+	for (i = 0; i < n; i++)
+		printf("%d%s", v[i], i < (n-1) ? ", " : "");
+	printf("\n\n");
+}
+
diff --git a/numlib/quadprog.h b/numlib/quadprog.h
new file mode 100755
index 0000000..7d28ec8
--- /dev/null
+++ b/numlib/quadprog.h
@@ -0,0 +1,67 @@
+
+#ifndef QUADPROG_H
+#define QUADPROG_H
+
+#ifdef __cplusplus
+	extern "C" {
+#endif
+
+/* 
+	Quadradic Programming solution.
+
+	Based on Luca Di Gaspero's QuadProgpp++, made available under the MIT license.
+
+	Translation to C by Graeme W. Gill, Copyright 2017, also licensed under the
+	MIT license.
+*/
+
+/* 
+
+ The quadprog_solve() function implements the algorithm of Goldfarb and Idnani 
+ for the solution of a (convex) Quadratic Programming problem
+ by means of an active-set dual method.
+	 
+The problem is in the form:
+
+min 0.5 * x G x + g0 x
+s.t.
+    CE^T x + ce0 = 0
+    CI^T x + ci0 >= 0
+	 
+ The matrix and vectors dimensions are as follows:
+     G: n * n
+		g0: n
+				
+		CE: n * p
+	 ce0: p
+				
+	  CI: n * m
+   ci0: m
+
+     x: n
+ 
+ References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
+             strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
+
+*/
+
+#define QP_INFEASIBLE 1.0E300
+
+double quadprog(	/* Return solution cost, QP_INFEASIBLE if infeasible/error */
+    double *x,		/* Return x[n] value */
+	double **G,		/* G[n][n]	Quadratic combination matrix - modified */
+	double *g0,		/* g0[n]    Direct vector */
+    double **CE,	/* CE[n][p]	Equality constraint matrix */
+	double *ce0, 	/* ce0[p]   Equality constraing constants */ 
+	double **CI,	/* CI[n][m]	Constraint matrix */
+    double *ci0,	/* cie[m]	Constraint constants */ 
+	int n,			/* Number of variables */ 
+	int p,			/* Number of equalities */ 
+	int m			/* Number of constraints */ 
+);
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif /*define QUADPROG_H */
diff --git a/numlib/rand.c b/numlib/rand.c
index 436de52..9e58de3 100644..100755
--- a/numlib/rand.c
+++ b/numlib/rand.c
@@ -19,84 +19,128 @@
 /* (From Knuth & H.W.Lewis) */
 #define PSRAND32L(S) ((S) * 1664525L + 1013904223L)
 
+/* - - - - - - - - - - - - - - - */
+/* Global state random generator */
+
+static rand_state g_rand = { 0 }; /* Use default seed for global state generator */
+
 /* Return a 32 bit number between 0 and 4294967295 */
 /* Use Knuth shuffle to improve PSRAND32 sequence */
 unsigned int
 rand32(					/* Return 32 bit random number */
 unsigned int seed		/* Optional seed. Non-zero re-initialized with that seed */
 ) {
-#define TSIZE 2843		/* Prime */
-	static unsigned int last, ran = 0x12345678;	/* Default seed */
-	static unsigned int pvs[TSIZE];
-	static int pvs_inited = 0;
-	int i;
-
-	if (seed != 0)
-		{
-		pvs_inited = 0;
-		ran = seed;;
-		}
-
-	/* Init random storage locations */
-	if (pvs_inited == 0)
-		{
-		for (i = 0; i < TSIZE; i++)
-  			pvs[i] = ran = PSRAND32(ran);
-		last = ran;
-		pvs_inited = 1;
-		}
-	i = last % TSIZE;		/* New location */
-	last = pvs[i];			/* Value generated */
-  	pvs[i] = ran = PSRAND32(ran);		/* New value */
-
-	return last-1;
+	return rand32_th(NULL, seed);
 }
 
 /* return a random number between 0.0 and 1.0 */
 /* based on rand32 */
 double ranno(void) {
-	return rand32(0) / 4294967295.0;
+	return rand32_th(NULL, 0) / 4294967295.0;
 }
 
 /* Return a random double in the range min to max */
 double
 d_rand(double min, double max) {
-	double tt;
-	tt = ranno();
-	return min + (max - min) * tt;
+	return d_rand_th(NULL, min, max);
 }
 
 /* Return a random integer in the range min to max inclusive */
 int
 i_rand(int min, int max) {
-	double tt;
-	tt = ranno();
-	return min + (int)floor(0.5 + ((double)(max - min)) * tt);
+	return i_rand_th(NULL, min, max);
 }
 
-
 /* Return a random floating point number with a gausian/normal */
 /* distribution, centered about 0.0, with standard deviation 1.0 */
 /* This uses the Box-Muller transformation */
 double norm_rand(void) {
-	static int r2 = 0;		/* Can use 2nd number generated */
-	static double nr2;
+	return norm_rand_th(NULL);
+}
+
+/* - - - - - - - - - - - - - - - */
+/* Explicit state random generator */
+
+/* Init rand_state to default */
+void rand_init(rand_state *p) {
+	if (p == NULL)
+		p = &g_rand;
+	p->pvs_inited = 0;
+	p->ran = 0;
+}
+
+/* Return a 32 bit number between 0 and 4294967295 */
+/* Use Knuth shuffle to improve PSRAND32 sequence */
+unsigned int
+rand32_th(rand_state *p,
+unsigned int seed		/* Optional seed. Non-zero re-initialized with that seed */
+) {
+	int i;
+
+	if (p == NULL)
+		p = &g_rand;
+
+	if (seed != 0) {
+		p->pvs_inited = 0;
+		p->ran = seed;
+	}
+
+	/* Init random storage locations */
+	if (p->pvs_inited == 0) {
+		if (p->ran == 0)
+			p->ran = RAND_SEED;
+		for (i = 0; i < RAND_TSIZE; i++)
+  			p->pvs[i] = p->ran = PSRAND32(p->ran);
+		p->last = p->ran;
+		p->pvs_inited = 1;
+	}
+	i = p->last % RAND_TSIZE;				/* New location */
+	p->last = p->pvs[i];					/* Value generated */
+  	p->pvs[i] = p->ran = PSRAND32(p->ran);	/* New value */
+
+	return p->last-1;
+}
+
+/* return a random number between 0.0 and 1.0 */
+/* based on rand32 */
+double ranno_th(rand_state *p) {
+	return rand32_th(NULL, 0) / 4294967295.0;
+}
+
+/* Return a random double in the range min to max */
+double
+d_rand_th(rand_state *p, double min, double max) {
+	return min + (max - min) * ranno_th(p);
+}
+
+/* Return a random integer in the range min to max inclusive */
+int
+i_rand_th(rand_state *p, int min, int max) {
+	return min + (int)floor(0.5 + ((double)(max - min)) * ranno_th(p));
+}
+
+/* Return a random floating point number with a gausian/normal */
+/* distribution, centered about 0.0, with standard deviation 1.0 */
+/* This uses the Box-Muller transformation */
+double norm_rand_th(rand_state *p) {
+	if (p == NULL)
+		p = &g_rand;
 
-	if (r2 == 0) {
+	if (p->r2 == 0) {
 		double v1, v2, t1, t2, r1;
 		do {
-			v1 = d_rand(-1.0, 1.0);
-			v2 = d_rand(-1.0, 1.0);
+			v1 = d_rand_th(p, -1.0, 1.0);
+			v2 = d_rand_th(p, -1.0, 1.0);
 			t1 = v1 * v1 + v2 * v2;
 		} while (t1 == 0.0 || t1 >= 1.0);
 		t2 = sqrt(-2.0 * log(t1)/t1);
-		nr2 = v2 * t2;			/* One for next time */
-		r2 = 1;
+		p->nr2 = v2 * t2;			/* One for next time */
+		p->r2 = 1;
 		r1 = v1 * t2;
 		return r1;
 	} else {
-		r2 = 0;
-		return nr2;
+		p->r2 = 0;
+		return p->nr2;
 	}
 }
 
diff --git a/numlib/rand.h b/numlib/rand.h
index 46f79f2..e190fcd 100644..100755
--- a/numlib/rand.h
+++ b/numlib/rand.h
@@ -13,6 +13,9 @@
 	extern "C" {
 #endif
 
+/* - - - - - - - - - - - - - - - */
+/* Global state random generator */
+
 /* Return a random number between 0 and 4294967294 */
 unsigned int
 rand32(						/* Return 32 bit random number */
@@ -29,6 +32,43 @@ double d_rand(double min, double max);
 /* and an average deviation of 0.564 */
 double norm_rand(void);
 
+/* - - - - - - - - - - - - - - - */
+/* Explicit state random generator */
+/* Use NULL for global state */
+
+#define RAND_TSIZE 2843      	/* Prime */
+#define RAND_SEED 0x12345678    /* Default seed */
+
+/* Should set to 0 to default intialize */
+typedef struct {
+	int pvs_inited;
+	unsigned int ran, last;
+	unsigned int pvs[RAND_TSIZE];
+
+	/* normal distribution 2nd value */
+	int r2;			/* 2nd value available */
+	double nr2;		/* 2nd value */
+} rand_state;
+
+/* Init rand_state to default */
+void rand_init(rand_state *p);
+
+/* Return a random number between 0 and 4294967294 */
+unsigned int
+rand32_th(rand_state *p,
+unsigned int seed);			/* Optional seed. Non-zero re-initialized with that seed */
+
+/* Return a random integer in the range min to max inclusive */
+int i_rand_th(rand_state *p, int min, int max);
+
+/* Return a random double in the range min to max inclusive */
+double d_rand_th(rand_state *p, double min, double max);
+
+/* Return a random floating point number with a gausian/normal */
+/* distribution, centered about 0.0, with standard deviation 1.0 */
+/* and an average deviation of 0.564 */
+double norm_rand_th(rand_state *p);
+
 #ifdef __cplusplus
 	}
 #endif
diff --git a/numlib/sobol.c b/numlib/sobol.c
index 40a2c38..40a2c38 100644..100755
--- a/numlib/sobol.c
+++ b/numlib/sobol.c
diff --git a/numlib/sobol.h b/numlib/sobol.h
index 08ac5c0..08ac5c0 100644..100755
--- a/numlib/sobol.h
+++ b/numlib/sobol.h
diff --git a/numlib/soboltest.c b/numlib/soboltest.c
index 32c26ee..32c26ee 100644..100755
--- a/numlib/soboltest.c
+++ b/numlib/soboltest.c
diff --git a/numlib/svd.c b/numlib/svd.c
index fdb8edd..fdb8edd 100644..100755
--- a/numlib/svd.c
+++ b/numlib/svd.c
diff --git a/numlib/svd.h b/numlib/svd.h
index 59b080d..6a89210 100644..100755
--- a/numlib/svd.h
+++ b/numlib/svd.h
@@ -13,13 +13,44 @@
 	extern "C" {
 #endif
 
+/*
+
+	U[] decomposes A[]'s columns into orthogonal, singular vectors.
+	U[]'s columns are vectors that sum to 1.0, i.e. they leave a vectors normal unchanged.
+	The inverse of U[] is its transpose.
+	U[]'s columns corresponding to non-zero W[] are the orthonormal vectors that span
+	the (input) RANGE space. Columns corresponding to zero W[] are zero.
+
+	W[] will return singlular values, i.e. the weighting of the singular vectors.
+	It's inverse is is the reciprical of its elements.
+
+	V[] composes the singular vectors back into A[]'s rows.
+	V[]'s columns and rows are orthonormal.
+	V[]'s columns corresponding to non-zero W[] are the orthonormal vectors that span
+	the (output) RANGE space. 
+	v[]'s columns corresponding to zero W[] are the (output) othonormal vectors that span
+	the NULL space.
+	The inverse of V[] is its transpose.
+
+	To re-form, A = U.W.Vt, i.e. multiply by transpose of V.
+
+	i.e. mult. input vector[m] by U[] converts to [n] compact, orthogonal
+	basis vectors. W then scales them appropiately, setting null space
+	vectors to 0. V[] then transforms from the orthogonal basis vectors
+	to A[]'s output space.
+
+	To reveal NULL space, make sure n >= m, since U[] vectors corrsponding
+	to zero's are set to zero.
+
+*/
+
 /* Compute Singular Value Decomposition of A = U.W.Vt */
 /* Return status value: */
 /* 0 - no error */
 /* 1 - m < n error */
 int svdecomp(
-double **a,		/* A[0..m-1][0..n-1], return U[][] */
-double  *w,		/* return W[0..n-1] */
+double **a,		/* A[0..m-1][0..n-1], return U[0..m-1][0..n-1] */
+double  *w,		/* return W[0..n-1] = singular values */
 double **v,		/* return V[0..n-1][0..n-1] (not transpose!) */
 int      m,		/* Number of equations */
 int      n		/* Number of unknowns */
diff --git a/numlib/svdtest.c b/numlib/svdtest.c
index 100120d..100120d 100644..100755
--- a/numlib/svdtest.c
+++ b/numlib/svdtest.c
diff --git a/numlib/tdhsx.c b/numlib/tdhsx.c
index d1738c4..d1738c4 100644..100755
--- a/numlib/tdhsx.c
+++ b/numlib/tdhsx.c
diff --git a/numlib/tpowell.c b/numlib/tpowell.c
index f7b0aad..f7b0aad 100644..100755
--- a/numlib/tpowell.c
+++ b/numlib/tpowell.c
diff --git a/numlib/ui.c b/numlib/ui.c
index 390a332..390a332 100644..100755
--- a/numlib/ui.c
+++ b/numlib/ui.c
diff --git a/numlib/ui.h b/numlib/ui.h
index a282c26..a282c26 100644..100755
--- a/numlib/ui.h
+++ b/numlib/ui.h
diff --git a/numlib/varmet.c b/numlib/varmet.c
new file mode 100755
index 0000000..3c4b61a
--- /dev/null
+++ b/numlib/varmet.c
@@ -0,0 +1,290 @@
+
+/* Multi-dimentional minizer using Variable Metric method */
+/* This is good for smoother, well behaved functions. */
+
+/* Code is an original expression of the algorithms decsribed in */
+/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */
+/* S.A.Teukolsky & W.T.Vetterling. */
+
+/*
+ * Copyright 2000, 2006, 2007, 2017 Graeme W. Gill
+ * All rights reserved.
+ *
+ * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
+ * see the License.txt file for licencing details.
+ */
+
+/* TTBD:
+   Fix error handling to return status (malloc, excessive itters)
+   Create to "safe" library ?
+   Make standalone - ie remove numsup ?
+ */
+
+/* Note that all arrays are indexed from 0 */
+
+#include "numsup.h"
+#include "powell.h"
+#include "varmet.h"
+
+#undef DEBUG					/* Debug */
+
+#if defined(PDEBUG) || defined(CDEBUG)
+# undef DBG
+# define DBG(xxx) printf xxx ;
+#else
+# undef DBG
+# define DBG(xxx) 
+#endif
+
+#define FMAX(A,B) ((A) > (B) ? (A) : (B))
+#define EPS   1.0e-10	/*  Machine precision. */
+#define TOLX (4 * EPS)	/*  X value stop value */
+#define MAXLEN 100.0	/*  Maximum step length */
+
+void linesearch(int di, double cpold[], double fpold, double g[], double p[], double cpnew[],
+	double *pfp, double maxstep, double (*func)(void *fdata, double tp[]), void *fdata);
+
+/* return 0 on sucess, 1 on failure due to excessive itterions */
+/* Result will be in cp */
+/* Note that we could use gradient in line minimiser, */
+/* but haven't bothered yet. */
+int varmet(
+double *rv,				/* If not NULL, return the residual error */
+int di,					/* Dimentionality */
+double cp[],			/* Initial starting point */
+double s[],				/* Size of initial search area */
+double ftol,			/* Tollerance of error change to stop on */
+int maxit,				/* Maximum iterations allowed */
+double (*func)(void *fdata, double tp[]),		/* Error function to evaluate */
+double (*dfunc)(void *fdata, double dp[], double tp[]),		/* Gradient function to evaluate */
+void *fdata				/* Opaque data needed by function */
+) {
+	int iter;
+	double fp, sumsq, maxstep;
+	double *sdir, sumsdir;			/* Search direction */
+	double *dp, *lastdp;
+	double **hessian;				/* Hessian matrix */
+	double *hlastdp;				/* Hessian times lastdp */
+	double *cpnew;					/* new cp value from linemin */
+	double test;
+
+	double den, fac, fad, fae;
+	double sumdg;
+	int i, j;
+
+	sdir   = dvector(0, di-1);
+	dp     = dvector(0, di-1);
+	lastdp = dvector(0, di-1);
+	hessian = dmatrix(0, di-1, 0, di-1);
+	hlastdp = dvector(0, di-1);
+	cpnew   = dvector(0, di-1);
+
+	fp = (*dfunc)(fdata, dp, cp);
+
+	/* Initial line direction and pde squared */
+	sumsq = 0.0;
+	for (i = 0; i < di ;i++) {
+		sdir[i] = -dp[i];				
+		sumsq += cp[i] * cp[i];
+	}
+
+	DBG((" initial fp %f dp %s\n", fp, debPdv(di, dp)));
+
+	/* Initialize inverse Hessian to unity */
+	for (i = 0; i < di ;i++) {
+	 	for (j = 0; j < di ; j++) {
+			if (i == j)
+				hessian[i][j] = 1.0;
+			else
+				hessian[i][j] = 0.0;
+		}
+	}
+
+	/* Maximum line search step size */
+	maxstep = MAXLEN * FMAX(sqrt(sumsq), (double)di);
+
+	/* Until we give up */
+	for (iter = 0; iter < maxit; iter++) {
+
+		/* Search in direction sdir */
+		linesearch(di, cp, fp, dp, sdir, cpnew, &fp, maxstep, func, fdata);
+
+		for (i = 0; i < di; i++) {
+			sdir[i] = cpnew[i] - cp[i];			/* Update the line direction, */
+			cp[i] = cpnew[i];					/* and the current point. */
+		}
+
+		/* Test for convergence on x */
+		test = 0.0;
+		for (i = 0 ; i < di; i++) {
+			double tt = fabs(sdir[i]) / FMAX(fabs(cp[i]), 1.0);
+			if (tt > test)
+				test = tt;
+		}
+
+		if (test < TOLX) {
+			break;
+		}
+
+		for (i = 0; i < di; i++)		/* Save previous partial deriv. */
+			lastdp[i] = dp[i];
+
+		(*dfunc)(fdata, dp, cp);		/* and get the new gradient. */
+
+		test = 0.0;					/* Test for convergence on zero gradient. */
+		den = FMAX(fp, 1.0);
+
+		for (i = 0; i < di; i++) {
+			double tt = fabs(dp[i]) * FMAX(fabs(cp[i]),1.0) / den;
+			if (tt > test)
+				test = tt;
+		}
+
+		if (test < ftol) {
+			break;
+		}
+
+	for (i = 0 ; i < di; i++)
+		lastdp[i] = dp[i] - lastdp[i];	/* Compute diference of gradients, */
+
+	for (i = 0; i < di; i++) {	/* and difernce times current matrix. */
+		hlastdp[i] = 0.0;
+		for (j = 0; j < di; j++)
+			hlastdp[i] += hessian[i][j] * lastdp[j];
+	}
+
+	/* Calculate dot products for the denominator */
+	fac = fae = sumdg = sumsdir = 0.0;
+	for (i = 0; i < di; i++) { 
+		fac += lastdp[i] * sdir[i];
+		fae += lastdp[i] * hlastdp[i];
+		sumdg += lastdp[i] * lastdp[i];
+		sumsdir += sdir[i] * sdir[i];
+	}
+	if (fac > sqrt(EPS * sumdg * sumsdir)) { 	/* Skip update if fac not sufficiently posive */
+		fac = 1.0/fac;
+		fad = 1.0/fae;
+		/* The vector that makes BFGS diferent from DFP: */
+		for (i = 0; i < di;i++)
+			lastdp[i] = fac * sdir[i] - fad * hlastdp[i];
+		for (i = 0; i < di;i++) {	/* The BFGS updating formula: */
+			for (j = i; j < di; j++) {
+				hessian[i][j] += fac * sdir[i] * sdir[j]
+				              - fad * hlastdp[i] * hlastdp[j] + fae * lastdp[i] * lastdp[j];
+				hessian[j][i] = hessian[i][j];
+			}
+		}
+	}
+	for (i = 0; i < di; i++) {	/* Now calculate the next direction to go, */
+		sdir[i] = 0.0;
+		for (j = 0; j < di; j++)
+			sdir[i] -= hessian[i][j] * dp[j];
+		}
+	}
+
+	free_dvector(cpnew, 0, di-1);
+	free_dvector(hlastdp, 0, di-1);
+	free_dmatrix(hessian, 0, di-1, 0, di-1);
+	free_dvector(lastdp, 0, di-1);
+	free_dvector(dp, 0, di-1);
+	free_dvector(sdir, 0, di-1);
+	if (rv != NULL)
+		*rv = fp;
+	return 0;
+}
+
+#define ALPHA 1.0e-4 			/* Ensures sucesscient decrease in function value. */
+#define LTOLX 1.0e-7		/* Convergence criterion on linesearch. */
+
+void linesearch(
+int di,
+double cpold[],
+double fpold,
+double dp[],		/* Partial derivative */
+double sdir[],		/* Current value */
+double cpnew[],
+double *pfp,		/* Return value */
+double maxstep,
+double (*func)(void *fdata, double tp[]),
+void *fdata
+) {
+	double sum, slope;
+	double slen, slen_min;
+	double test, fval;
+	int i;
+
+	for (sum = 0.0, i = 0; i < di; i++)
+		sum += sdir[i] * sdir[i];
+	sum = sqrt(sum);
+
+	if (sum > maxstep) {
+		for (i = 0; i < di; i++)
+			sdir[i] *= maxstep/sum; 		/* Scale if attempted step is too big. */
+	}
+	for (slope = 0.0, i = 0; i < di; i++)
+		slope += dp[i] * sdir[i];
+
+	if (slope >= 0.0)
+		error("Roundoff problem in linesearch.");
+
+	test = 0.0;
+	for (i = 0;i < di; i++) {
+		double tt = fabs(sdir[i])/FMAX(fabs(cpold[i]), 1.0);
+		if (tt > test)
+			test = tt;
+	}
+
+	slen_min = TOLX/test;
+	slen = 1.0;				/* Try full step */
+
+	/* Start of iteration loop. */
+	for (;;) {
+		double slen_2 = slen, slen_t;
+
+		for (i = 0; i < di;i++)
+			cpnew[i] = cpold[i] + slen * sdir[i];
+
+		*pfp = (*func)(fdata, cpnew);
+
+		if (slen < slen_min) {
+			for (i = 0; i < di; i++)
+				cpnew[i] = cpold[i];
+			return;
+
+		} else if (*pfp <= (fpold + ALPHA * slen * slope))
+			return;	
+
+		/* Backtracking */
+		else {
+			/* First time through */
+			if (slen == 1.0)
+				slen_t = -slope/(2.0 * (*pfp - fpold - slope));
+			/* 2nd and subsequent times through */
+			else {
+				double aa, bb;
+				double rhs_1, rhs_2;
+
+				rhs_1 = *pfp - fpold - slen * slope;
+				rhs_2 = fval - fpold - slen_2 * slope;
+				aa = (rhs_1/(slen * slen) - rhs_2/(slen_2 * slen_2))/(slen - slen_2);
+				bb = (-slen_2 * rhs_1/(slen * slen)+slen * rhs_2/(slen_2 * slen_2))/(slen - slen_2);
+				if (aa == 0.0)
+					slen_t = -slope/(2.0 * bb);
+				else {
+					double dd = bb * bb - 3.0 * aa * slope;
+					if (dd < 0.0)
+						slen_t = 0.5 * slen;
+					else if (bb <= 0.0)
+						slen_t = (-bb + sqrt(dd))/(3.0 * aa);
+					else
+						slen_t = -slope/(bb + sqrt(dd));
+				}
+				if (slen_t > 0.5 * slen)
+					slen_t = 0.5 * slen;
+			}
+		}
+		slen_2 = slen;
+		fval = *pfp;
+		slen = FMAX(slen_t, 0.1 * slen);
+	}
+}
diff --git a/numlib/varmet.h b/numlib/varmet.h
new file mode 100755
index 0000000..5f24445
--- /dev/null
+++ b/numlib/varmet.h
@@ -0,0 +1,55 @@
+#ifndef VARMET_H
+#define VARMET_H
+
+/* Variable Metric multivariate minimiser */
+
+/*
+ * Copyright 2000, 2007 Graeme W. Gill
+ * All rights reserved.
+ *
+ * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
+ * see the License.txt file for licencing details.
+ */
+
+#ifdef __cplusplus
+	extern "C" {
+#endif
+
+/* Variable Metric Gradient optimiser */
+/* return 0 on sucess, 1 on failure due to excessive itterations */
+/* Result will be in cp */
+int varmet(
+double *rv,				/* If not NULL, return the residual error */
+int di,					/* Dimentionality */
+double cp[],			/* Initial starting point */
+double s[],				/* Size of initial search area */
+double ftol,			/* Tollerance of error change to stop on */
+int maxit,				/* Maximum iterations allowed */
+double (*func)(void *fdata, double tp[]),		/* Error function to evaluate */
+double (*dfunc)(void *fdata, double dp[], double tp[]),		/* Gradient function to evaluate */
+void *fdata				/* Opaque data needed by function */
+);
+
+/* Example user function declarations */
+double varmet_funk(		/* Return function value */
+	void *fdata,		/* Opaque data pointer */
+	double tp[]);		/* Multivriate input value */
+
+/* Line in multi-dimensional space minimiser */
+double brentnd(			/* vector multiplier return value */
+double ax,				/* Minimum of multiplier range */
+double bx,				/* Starting point multiplier of search */
+double cx,				/* Maximum of multiplier range */
+double ftol,			/* Tollerance to stop search */
+double *xmin,			/* Return value of multiplier at minimum */		
+int n,					/* Dimensionality */
+double (*func)(void *fdata, double tp[]),		/* Error function to evaluate */
+void *fdata,			/* Opaque data */
+double pcom[],			/* Base vector point */
+double xicom[]);		/* Vector that will be multiplied and added to pcom[] */
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif /* VARMET_H */
diff --git a/numlib/zbrent.c b/numlib/zbrent.c
index a811818..a811818 100644..100755
--- a/numlib/zbrent.c
+++ b/numlib/zbrent.c
diff --git a/numlib/zbrent.h b/numlib/zbrent.h
index 2f8aac4..2f8aac4 100644..100755
--- a/numlib/zbrent.h
+++ b/numlib/zbrent.h
diff --git a/numlib/zbrenttest.c b/numlib/zbrenttest.c
index d7b44d3..d7b44d3 100644..100755
--- a/numlib/zbrenttest.c
+++ b/numlib/zbrenttest.c