#include <stdcxx.hxx>	/* min */
#include <matrix.h>
#include <assert.h>
#include <libc.h>	/* for exit() */
#include <math.h>	/* for sqrt() */
#include <mnewt.h>
#include <assert.h>
#include <svdcmp.h>

struct twomat   {
	matrix one, two;
	twomat(C matrix& a, C matrix& b) : one(a), two(b) {;}
	twomat() : one(), two() {;}
	operator void *() C
		{return (one.badmat()||two.badmat()) ? 0 : (void *)this;}
	BOOL operator!() C {return !(void *)(*this);}
	};

#define NGOOD   2       /* number of repeats to assure completion */
#define SMALLMAG        1e-10
C double EPS = 1e-10;

LOCAL matrix svd_invert(C matrix& a, double condition)
{
 ENTER_FCN("svd_invert");
 assert(!a.badmat());
 assert(condition<1.0  && condition>0.0);
 svdcmp x(a);
 MARK_LINE;
 C double typ = sqrt(x.w.norm_2()/x.w.size().x);
 diag_matrix wi(x.w.size().x);
 int i;
 assert(wi.size() == x.w.size());
 MARK_LINE;
 cout << "svd_invert-- typ:" << typ << "condition:" <<condition << endl;
 for(i=0;i<x.w.size().x;i++)	{
	C double tmpw = x.w(i,i);
	cout << "w"<<x.w(i,i)<<"tmpw"<<tmpw << endl;
	if((tmpw<0.0?-tmpw:tmpw) < typ*condition)	{
		cout << "tmpw:" << tmpw << endl;
		wi(i,i) = 0.0;
		if(mnewt_trace)
			cout << "near singular("<<i<<")" << endl;
		}
	else	{
		cout << "ww"<<x.w(i,i) << endl;
		wi(i,i) = 1.0/tmpw;
		}
	}
 return x.v*wi*transpose(x.u);
}

LOCAL matrix Broyden(C matrix& b, C matrix& p, C matrix& q);

matrix fejacobian(C matrix& x, C double *c, int nc, mnewt_function fun,
						C abstract_matrix& del);
LOCAL BOOL done(double normdir, double normf, double howsmall, int *picount);
LOCAL struct twomat mmize(C matrix& dir, double *psmall, mnewt_function fun,
							C double *c, int nc, C matrix& x,
							C abstract_matrix& del, C matrix& invscale,
							double *plast);



LOCAL BOOL gBroyden(double howsmall, double lastsmall)
{ return howsmall>lastsmall || howsmall>=0.8; }

LOCAL matrix jscale(C matrix& j, C abstract_matrix& del, C matrix& invscale)
{
 ENTER_FCN("jscale");
 if(j.badmat())
	abstract_matrix::badarg.raise("mnewt:jscale:j");
 assert(!del.badmat());
 assert(!invscale.badmat());

 // cerr << "invscale:" << invscale;
 // cerr << "j:" << j;
 // cerr << "del:" << del;
 matrix rv = invscale * j * del;
 // cout << "scaled jacobian:" << rv;
 return rv;
}

LOCAL int errquit(C char *s)
{
 ENTER_FCN("errquit");
 assert_tmp_rnmn.fail(s, 0, "mnewt_fail");
 return 0;
}

LOCAL matrix bumpdiag(C matrix& in, double fac)
{
 ENTER_FCN("bumpdiag");
 assert(in.is_square());
 assert(!in.badmat());
 matrix a = in;
 if(fabs(fac-1.0) > 1e-6)	{
 	C int is = a.size().x;
 	int i;
 	for(i=0;i<is;i++)
		a(i,i) *= fac;
	}
 return a;
}

 class wrapper	{
	wrapper();	// unused
	public:
	C int nc;
	C double *C p;
	wrapper(C double *pp, int nn) : p(pp), nc(nn) {;}
	virtual ~wrapper() {;}
	virtual matrix operator()(C matrix& x) C = 0;
	};

class numeric_wrapper : public wrapper	{
	C mnewt_function f;
	C abstract_matrix *C delp;
	public:
	numeric_wrapper(C double *pp, int nc, mnewt_function ff,
											C abstract_matrix& dp);
	matrix operator()(C matrix& x) C;
	};

class known_wrapper : public wrapper	{
	matrix (*C j)(C matrix&, C double *, int);
	public:
	known_wrapper(C double *pp, int nc,
						matrix (*j)(C matrix&, C double *, int));
	matrix operator()(C matrix& x) C;
	};

known_wrapper::known_wrapper(C double *pp, int nn,
								matrix (*jj)(C matrix&, C double *, int))
: wrapper(pp, nn), j(jj)
{;}

numeric_wrapper::numeric_wrapper(C double *pp, int nn, mnewt_function ff,
									C abstract_matrix& dp)
: wrapper(pp, nn), f(ff), delp(&dp)
{;}

matrix known_wrapper::operator()(C matrix& x) C
{
 ENTER_FCN("known_jacobian");
 return (*j)(x, p, nc);
}

matrix numeric_wrapper::operator()(C matrix& x) C
{
 return fejacobian(x, p, nc, f, *delp);
}

#include <string.h>
#define ERRBUFSIZE 200
LOCAL char errbuf[ERRBUFSIZE];

LOCAL int errstore(C char *s)
{
 strncpy(errbuf, s, ERRBUFSIZE);
 return 1;
}


#define BAD_ARG(s, ign) { mnewt_fail.raise(strncat(errbuf, s, ERRBUFSIZE));\
							errbuf[0] = '\0';\
							abstract_matrix::badarg.appoint(ign);\
							return matrix();}


matrix mnewt(mnewt_function fun, C double *c, int nc,
				C matrix& x0, C abstract_matrix& del, C abstract_matrix& scale,
				matrix (*jcb)(C matrix&, C double *, int))
{
 ENTER_FCN("mnewt");
 double howsmall = 1.0;
 int icount = 0;
 BOOL stop;

 assert(!del.badmat());
 assert(!scale.badmat());
 assert(!x0.badmat());
 assert(fun);
 errbuf[0] = '\0';

 struct wrapper *jacobian = jcb
							? (wrapper *)new known_wrapper(c, nc, jcb)
							: (wrapper *)new numeric_wrapper(c, nc, fun, del);

 MARK_LINE;
 // cout << "scale=" << scale << endl;
 Objection_action *mat_ign = abstract_matrix::badarg.appoint(errstore);
 matrix invscale = invert(scale);
 if(invscale.badmat())
	BAD_ARG("can't invert scale", mat_ign);
 MARK_LINE;
 // cout << "del=" << del << endl;
 matrix invdel = invert(del);
 if(invdel.badmat())
	BAD_ARG("can't invert del", mat_ign);
 abstract_matrix::badarg.appoint(mat_ign);
 matrix xm = x0;
 matrix oldf = (*fun)(xm, c, nc);
 if(oldf.badmat())
	BAD_ARG("mnewt:bad matrix from (*fun)() at start", mat_ign);

 double last = (invscale*oldf).norm_2();

 do     {
	ENTER_FCN("doloop");
	BOOL rejacob = FALSE;
	BOOL jrj = TRUE;
	matrix j = jscale((*jacobian)(xm), del, invscale);
	if(j.badmat())   {
		mnewt_fail.raise("can't calculate jacobian\n");
		delete jacobian;
		return matrix();
		}
	do      {
		ENTER_FCN("doloop2");
		BOOL badij;
		unsigned ct = 0;
		matrix dir;
		C double lastsmall = howsmall;
		twomat xy;
		double diagbump = 1.0;

		diagbump *= 1-log(howsmall)*0.01;
		do	{
			ENTER_FCN("doloop3");
			assert(diagbump > 0.99);
			if(ct==3 && !jrj)
				j = jscale((*jacobian)(xm), del, invscale);
			matrix invj = svd_invert(bumpdiag(j, diagbump), 0.1);
			if(!(badij=invj.badmat()))	{
				dir = invj * invscale * oldf;
				xy = mmize(dir, &howsmall, fun, c, nc, xm, del, invscale,
								      &last);
				}
			if(badij || !xy)	{
				diagbump *= mnewt_diagbump;
				if(mnewt_trace)	cout <<  "increase bumpdiag" << endl;
				howsmall = sqrt(howsmall);
				}
			else if(howsmall>0.5)
				diagbump = sqrt(diagbump);
			} while((badij || !xy) && ct++<5);

		if(badij || !xy)	{
			mnewt_fail.raise(badij ? "Can't invert jacobian."
								: (xy.two.badmat() ? "Trajectory out of bounds."
													: "Can't mmize."));
			delete jacobian;
			return matrix();
			}

		MARK_LINE;
		if(!(stop = done(dir.norm_2(), (invscale*xy.two).norm_2(),
							howsmall, &icount)))      {
			ENTER_FCN("notstop");
			rejacob = !gBroyden(howsmall, lastsmall);
			if(!rejacob)    {
				// cout << "j="<<j<<" before update" << endl;
				j = Broyden(j, invdel* (xy.one- xm),
					      invscale* (xy.two- oldf));
				jrj = FALSE;
				// cout << "j(adjusted jacobian)="<<j<<endl;
				}
			}
		// cout << "stop=" << stop << endl;
		xm = xy.one;
		oldf = xy.two;
		}while(!stop && !rejacob);
	}while(!stop);
 delete jacobian;
 return xm;
}


LOCAL matrix Broyden(C matrix& b, C matrix& p, C matrix& q)
{
 ENTER_FCN("broyden");
 DOUBLE divisor, cancel;
 matrix change;

 assert(!p.badmat() && !q.badmat());

 if((divisor=p.norm_2()) < SMALLMAG) {
	return b;
	}
 change = q-b*p;
 cancel = change.max() / q.max();
 change = (change* transpose(p)) * (cancel/(divisor*(cancel+SMALLMAG)));
 // cout << "change to j is " << change << endl;
 return b + change;
}

C double BIG_DECR = 0.4;
C double SMALL_DECR = 0.7;

LOCAL struct twomat mmize(C matrix& dir, double *psmall,
							mnewt_function fun,
							C double *c, int nc, C matrix& x,
							C abstract_matrix& del, C matrix& invscale,
							double *plast)
{
 ENTER_FCN("mmize");
 assert(plast && *plast>0.0);
 assert(psmall && *psmall>0.0);

 if(dir.badmat() || del.badmat() || !psmall || invscale.badmat() || x.badmat()){
	mnewt_fail.raise("bad args to mmize");
	return twomat();
	}

	// Make *psmall approach unity:
 double nextr = min(1.0, *psmall>0.05 ? 2**psmall : 4**psmall);
 double thisr = nextr;
 double lastr = thisr;
 // cout << "nextr=" << nextr << " del=" << del << " dir=" << dir << endl;
 double last = -1.0;
 double lastlast = last;
 matrix xt, y, xlast, ylast;
 BOOL repeat = TRUE;
 do     {
	ENTER_FCN("loop");
	xlast = xt;
	lastr = thisr;
	xt = x - (thisr=nextr)*del*dir;
	if(mnewt_trace)	{
		cout << "thisr=" <<thisr << endl;
		cout << "testing for minimum at" << xt << endl;
		}

	ylast = y;
	y = (*fun)(xt, c, nc);
	if(y.badmat())
		nextr *= SMALL_DECR;
	else 	{
		lastlast = last;
		last = (invscale*y).norm_2();
		repeat = (last>*plast*(1+EPS) && *plast>=0.0)	/* worse than no move*/
					|| (last<lastlast*(1-EPS) && last>=1.0 /* so,so */
							&& last>*plast/3);
		if(last<lastlast*0.99 && lastlast>0.0 && thisr<0.99*lastr)	{
			C double rmin = thisr				/* parabolic approximation */
							+ 0.5*(xx(thisr-0)*(last-lastlast)
									-xx(thisr-lastr)*(last-*plast))
								/ ((thisr-0)*(last-lastlast)
									-(thisr-lastr)*(last-*plast));
			if(rmin>thisr*BIG_DECR && rmin<thisr*SMALL_DECR)
				nextr = rmin;
			nextr = thisr* (nextr>0.05||last>6**plast ? BIG_DECR : SMALL_DECR);
			}
		else
			nextr = thisr*(nextr>0.05||last>6**plast ? BIG_DECR : SMALL_DECR);
		}
	
	} while(repeat && nextr>mnewt_smallest);

 if(last<0.0 || last>=*plast)	// last<0 if (*fun)() never worked.
	return twomat(matrix(), y);

 if(last<lastlast || lastlast<0.0)	{	// lastlast<0 if only one pass thru loop
 	*plast = last;
 	*psmall = thisr;
	}
 else	{
	*plast = lastlast;
	*psmall = lastr;
	return twomat(xlast, ylast);
	}
 // cout << "best value of function is" << y << endl;
 // cout << "at x=" << xt << endl;
 return twomat(xt, y);
}

LOCAL BOOL done(double normdir, double normf, double howsmall, int *picount)
{
 BOOL nowgood;

 if(mnewt_trace)
 	cerr << "norms:" << normdir << ", " << normf <<" howsmall:" << howsmall
				<< " icount:" << *picount << endl;
 /*
 cout << "howsmall="<<howsmall << "mag(dir)="<<dir.norm_2() << endl;
 */

 nowgood = normdir<sqrt(howsmall) || normf<1.0;

 if((nowgood && ++*picount>=NGOOD) || normf<0.01)
	return TRUE;
 else if(nowgood)       {
	++*picount;
	// cout << "done: *picount=" << *picount << endl;
	}
 else   {
	*picount = 0;
	// cout << "done: reset *picount" << endl;
	}
 return FALSE;
}

GLOBAL Objection mnewt_fail(errquit);

GLOBAL BOOL mnewt_trace = FALSE;
GLOBAL double mnewt_smallest = 0.001;
GLOBAL double mnewt_diagbump = 9.0;

