#include <cmath>
#include <limits>
#include "nr.h"
using namespace std;

Vec_DP *fvec_p;
void (*nrfuncv)(Vec_I_DP &v, Vec_O_DP &f);

void NR::broydn(Vec_IO_DP &x, bool &check, void vecfunc(Vec_I_DP &, Vec_O_DP &))
{
	const int MAXITS=200;
	const DP EPS=numeric_limits<DP>::epsilon();
	const DP TOLF=1.0e-8, TOLX=EPS, STPMX=100.0, TOLMIN=1.0e-12;
	bool restrt,sing,skip;
	int i,its,j,k;
	DP den,f,fold,stpmax,sum,temp,test;

	int n=x.size();
	Mat_DP qt(n,n),r(n,n);
	Vec_DP c(n),d(n),fvcold(n),g(n),p(n),s(n),t(n),w(n),xold(n);
	fvec_p=new Vec_DP(n);
	nrfuncv=vecfunc;
	Vec_DP &fvec=*fvec_p;
	f=fmin(x);
	test=0.0;
	for (i=0;i<n;i++)
		if (fabs(fvec[i]) > test) test=fabs(fvec[i]);
	if (test < 0.01*TOLF) {
		check=false;
		delete fvec_p;
		return;
	}
	for (sum=0.0,i=0;i<n;i++) sum += SQR(x[i]);
	stpmax=STPMX*MAX(sqrt(sum),DP(n));
	restrt=true;
	for (its=1;its<=MAXITS;its++) {
		if (restrt) {
			fdjac(x,fvec,r,vecfunc);
			qrdcmp(r,c,d,sing);
			if (sing) nrerror("singular Jacobian in broydn");
			for (i=0;i<n;i++) {
				for (j=0;j<n;j++) qt[i][j]=0.0;
				qt[i][i]=1.0;
			}
			for (k=0;k<n-1;k++) {
				if (c[k] != 0.0) {
					for (j=0;j<n;j++) {
						sum=0.0;
						for (i=k;i<n;i++)
							sum += r[i][k]*qt[i][j];
						sum /= c[k];
						for (i=k;i<n;i++)
							qt[i][j] -= sum*r[i][k];
					}
				}
			}
			for (i=0;i<n;i++) {
				r[i][i]=d[i];
				for (j=0;j<i;j++) r[i][j]=0.0;
			}
		} else {
			for (i=0;i<n;i++) s[i]=x[i]-xold[i];
			for (i=0;i<n;i++) {
				for (sum=0.0,j=i;j<n;j++) sum += r[i][j]*s[j];
				t[i]=sum;
			}
			skip=true;
			for (i=0;i<n;i++) {
				for (sum=0.0,j=0;j<n;j++) sum += qt[j][i]*t[j];
				w[i]=fvec[i]-fvcold[i]-sum;
				if (fabs(w[i]) >= EPS*(fabs(fvec[i])+fabs(fvcold[i]))) skip=false;
				else w[i]=0.0;
			}
			if (!skip) {
				for (i=0;i<n;i++) {
					for (sum=0.0,j=0;j<n;j++) sum += qt[i][j]*w[j];
					t[i]=sum;
				}
				for (den=0.0,i=0;i<n;i++) den += SQR(s[i]);
				for (i=0;i<n;i++) s[i] /= den;
				qrupdt(r,qt,t,s);
				for (i=0;i<n;i++) {
					if (r[i][i] == 0.0) nrerror("r singular in broydn");
					d[i]=r[i][i];
				}
			}
		}
		for (i=0;i<n;i++) {
			for (sum=0.0,j=0;j<n;j++) sum += qt[i][j]*fvec[j];
			p[i] = -sum;
		}
		for (i=n-1;i>=0;i--) {
			for (sum=0.0,j=0;j<=i;j++) sum -= r[j][i]*p[j];
			g[i]=sum;
		}
		for (i=0;i<n;i++) {
			xold[i]=x[i];
			fvcold[i]=fvec[i];
		}
		fold=f;
		rsolv(r,d,p);
		lnsrch(xold,fold,g,p,x,f,stpmax,check,fmin);
		test=0.0;
		for (i=0;i<n;i++)
			if (fabs(fvec[i]) > test) test=fabs(fvec[i]);
		if (test < TOLF) {
			check=false;
			delete fvec_p;
			return;
		}
		if (check) {
			if (restrt) {
				delete fvec_p;
				return;
			} else {
				test=0.0;
				den=MAX(f,0.5*n);
				for (i=0;i<n;i++) {
					temp=fabs(g[i])*MAX(fabs(x[i]),1.0)/den;
					if (temp > test) test=temp;
				}
				if (test < TOLMIN) {
					delete fvec_p;
					return;
				}
				else restrt=true;
			}
		} else {
			restrt=false;
			test=0.0;
			for (i=0;i<n;i++) {
				temp=(fabs(x[i]-xold[i]))/MAX(fabs(x[i]),1.0);
				if (temp > test) test=temp;
			}
			if (test < TOLX) {
				delete fvec_p;
				return;
			}
		}
	}
	nrerror("MAXITS exceeded in broydn");
	return;
}