#include <gpk_std.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <numerics.h>
#include <ctype.h>
#include "err.h"
#include <string.h>
#include <mylib.h>
#include <unistd.h>
#include <ieeefp.h>
# include <sunmath.h>
# include <ieeefp.h>
#include <errno.h>


#define D	if(1)
#define DD	if(0)
#undef TEST



C char sccs[] = "%W% %I%";
C char dir[] = SOURCEDIR;
static C int EXPSZ = 30;
static double T0 = quiet_nan(0);


LOCAL double max(double a, double b)	{return a>b ? a : b;}
LOCAL double xx(double x)	{return x*x;}



LOCAL void copya(double *a, C double *b, int n)
{
 memcpy(a, b, n*sizeof(double));
}



LOCAL void trim_comments(char *line)
{		/* # comments. */

 if(line[0] == '#')
	line[0] = '\0';

 char *m;
 for(m=line; m[0] && m[1]; m++)
	if(m[1]=='#' && isspace(m[0]))
		m[0] = '\0';
}



LOCAL void print_help(void)
{
 C char **h;
 for(h=help; *h; h++)
	fprintf(stderr, "%s\n", *h);
 fprintf(stderr, "version= %s, source code in %s/%s\n", sccs, dir, __FILE__);
}



main(int argc, char **argv)
{
 if(argc>2 || (argc==2 && strcmp(argv[1], "^")==0))	{
	print_help();
	exit(1);
	}

 BOOL more;
 do	{
	printf("minimize2d.gpk.1\n");
	fflush(stdout);
	more = minimize();
	fflush(stdout);
	} while(more);

 exit(0);
}


struct xyz	{ double x, y, z; struct xyz *next;};

xyz_push(struct xyz **p, C char *x, C char *y, C char *z)
{
 struct xyz *tmp = new xyz;
 assert(tmp);
 double t;
 if(sscanf(x, "%lf", &tmp->x)!=1 || sscanf(x, "%lf", &tmp->y)!=1
	|| sscanf(x, "%lf", &tmp->x)!=1)	{
	gpk_err("Bad xyz parse", 0);
	}
 new->next = *list;
 *list = new;
}



struct xyz xyz_average(struct xyz *list)
{
 struct xyz tmp;
 tmp.x = tmp.y = 0.0;
 tmp.z = infinity();
 int n = 0;
 for(; list; list = list->next)	{
	tmp.x += list->x;
	tmp.y += list->y;
	if(list->z < tmp.z && finite(list->z))
		tmp.z = list->z;
	n++;
	}
 tmp.x /= n;
 tmp.y /= n;
 return tmp;
}



void xyz_subtract(struct xyz *list, C struct xyz& m)
{
 for(; list; list = list->next)	{
	list->x -= m.x;
	list->y -= m.y;
	list->z -= m.z;
	}
}


xyz_free(struct xyz *list)
{
 struct xyz *n;
 while(list)	{
	n = list->next;
	delete list;
	list = n;
	}
}



BOOL minimize()
{
 char line[2000];
 BOOL more = FALSE;
 xyz *list = 0;
 proplist info;
 while(fgets(line, sizeof(line), stdin))	{
	if(chomp_newline(line) < 0)
		gpk_err("Line too long - no newline", 0);

	if(is_blank_line(line))
		continue;
	if(strcmp(line, "END") == 0)	{
		more = TRUE;
		break;
		}

	D fprintf(stderr, "#L: %s\n", line);
	if(is_error_line(line))
		gpk_err(line, 0);

	proplist tmp = avparse(line);
	if(tmp.get(""))
		gpk_err("bad parse", tmp.gets("", "unknown error"));

	if(tmp.get("x") && tmp.get("y") && tmp.get("z"))
		xyz_push(&list, tmp.gets("x", "NaN"),
				tmp.gets("y", "NaN"),
				tmp.gets("z", "NaN"));

	info += tmp;
	}


 xyz avg = xyz_average(list);
 xyz_subtract(list, xyz);
 xyz goto = do_minimize(info, list);
 xyz goto = xyz_plus(avg, goto);

 xyz_free(list);
 return more;
}


xyz xyz_plus(C xyz& a, C xyz& b)
{
 xyz rv;
 rv.x = a.x + b.x;
 rv.y = a.y + b.y;
 rv.z = a.z + b.z;
 rv.next = 0;
 return rv;
}


xyz do_minimize(info, list)
{
}


C double MAX_CONDITION = 100.0;


LOCAL vecpair fit_dir_parab(C xyz *s,
							double dely, double T,
							maybe_s *maybe)
{
 double **m = double2alloc(6, 6);
 double beta[6];
 int i;
 {
 int j;
 for(j=0; j<6; j++)	{
	int k;
	for(k=0; k<6; k++)
		m[j][k] = 0;
	beta[j] = 0;
	}
 }
						/* Add default curvature, do svd_invert, subtract def curv. */
						/* Since SVD sets singular values to zero, it will approach the
						default paraboloid as the data becomes more and more singular. */
 double big = -infinity();
 int used = 0;
 for(; s; s=s->next)	{
	if(!finite(s->z))
		continue;
	if(used>6 && !(s->z < big+T))
		continue;
	if(s->z > big)
		big = s->z;

	double x[6];
	x[0] = xx( s->x );
	x[1] = 2* s->x * s->y;
	x[2] = xx( s->y );
	x[3] = s->x;
	x[4] = s->y;
	x[5] = 1/dely;
	int j;
	for(j=0; j<6; j++)	{
		int k;
		for(k=0; k<6; k++)
			m[j][k] += x[j]*x[k];
		beta[j] += x[j] * s->z;
		}
	used++;
	}

 if(!svd_invert(m, 6, MAX_CONDITION))	{
	double2free(m);
	fprintf(stderr, "\tInvert fails!.   Points follow:\n");
	return vecpair();
	}

 double abcdef[6];
 for(i=0; i<6; i++)	{
	int k;
	abcdef[i] = 0;
	for(k=0; k<6; k++)
		abcdef[i] += m[i][k] * beta[k];
	}
 double2free(m);
 D fprintf(stderr, "\tsolution (%g, %g, %g)^2 + (%g,%g) + %g + %g\n",
					abcdef[0], abcdef[1], abcdef[2],
					abcdef[3], abcdef[4], abcdef[5],
					s[0].y);

 matrix22 curv(abcdef[0], abcdef[1], abcdef[1], abcdef[2]);


 vector2 evec1, evec2;
 jacobi_step(curv, &evec1, &evec2);
 C double eval1 = sandwitch(evec1, curv);
 C double eval2 = sandwitch(evec2, curv);

 vector2 downhill(-abcdef[3], -abcdef[4]);

 if(eval1+2*eval2<0 || eval2+2*eval1<0)	{
	/* The eigenvalues are far from positive definite.  We cannot
		trust any parabolic approximation. */
	return vecpair();
	}

 if(dot(downhill, evec1) < 0)
	evec1 *= -1;

 if(dot(downhill, evec2) < 0)
	evec2 *= -1;

 evec1 *= sqrt(fabs(dely/eval1));	/* vectors have length to change chi-squared by about dely. */
 evec2 *= sqrt(fabs(dely/eval2));


 /* Down here, there is some reason to believe that the
 eigenvalues mean *something*... */
 matrix22 invc;
 {
 C double min_eval = min(eval1, eval2);
 if(min_eval < 0)	{		/* Force the matrix to be positive definite, in an arbitrary manner. */
	curv += 2*min_eval*matrix22::identity();
	/* The minimum is probably wrong, so beware... */
	maybe->trustworthy = FALSE;
	}
 else	maybe->trustworthy = TRUE;

 double det = quiet_nan(0);
 invc = inverse(curv, &det);

	/* Strong cancellations - minimum is doubtful: */
 if(det < 0.1*curv(0,0)*curv(1,1))
	maybe->trustworthy = FALSE;
 }

 C vector2 randvec = evec1 * normalrand() * sqrt(T/dely)
				+ evec2 * normalrand() * sqrt(T/dely);
 /* Calculate where the minimum ought to be, if the surface were
		truly parabolic. Add in a random jiggle (r1&r2) so
	that we explore the region with temperature T. */
 maybe->x = invc*downhill*0.5 + s[0].v + randvec;
 maybe->slope = vector2(abcdef[3], abcdef[4]);
 maybe->y0 = abcdef[5] + s[0].y;
 maybe->setcurv(curv);
 maybe->x0 = s[0].v;


 return vecpair(evec1, evec2);
}


