/************************************************************************
EnergyMinimizer.cpp
Minimize energy by Conjugate Gradient method
("Numerical Recipes in C" implimentation)
************************************************************************/

#include "stdafx.h"
#include "MeshEditor.h"
#include "EnergyMinimizer.h"
#include "Energy.h"
#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// �\�z/����
//////////////////////////////////////////////////////////////////////

EnergyMinimizer::EnergyMinimizer()
{

}

EnergyMinimizer::~EnergyMinimizer()
{

}

#define ITMAX 200
#define EPS 1.0e-10

void EnergyMinimizer::frprmn(float p[], int n, float ftol, int *iter, float *fret)
{
	int j,its;
	float gg,gam,fp,dgg;
	float *g,*h,*xi;

	g=vector(n);
	h=vector(n);
	xi=vector(n);
	fp=func(p);
	dfunc(p, xi);
	for (j=0;j<n;j++) {
		g[j] = -xi[j];
		xi[j]=h[j]=g[j];
	}
	for (its=1;its<=ITMAX;its++) {
		*iter=its;
		dlinmin(p,xi,n,fret);
		if (2.0*fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+EPS)) {
			free_vector(xi);
			free_vector(h);
			free_vector(g);
			return;
		}
		fp=func(p);
		dfunc(p, xi);

		dgg=gg=0.0;
		for (j=0;j<n;j++) {
			gg += g[j]*g[j];
			dgg += (xi[j]+g[j])*xi[j];
		}
		if (gg == 0.0) {
			free_vector(xi);
			free_vector(h);
			free_vector(g);
			return;
		}
		gam=dgg/gg;
		for (j=0;j<n;j++) {
			g[j] = -xi[j];
			xi[j]=h[j]=g[j]+gam*h[j];
		}
	}
	nrerror("Too many iterations in frprmn");
}

#undef ITMAX
#undef EPS

#define TOL 2.0e-4

void EnergyMinimizer::dlinmin(float p[], float xi[], int n, float *fret)
{
	int j;
	float xx,xmin,fx,fb,fa,bx,ax;

	ncom=n;
	pcom=vector(n);
	xicom=vector(n);

	for (j=0;j<n;j++) {
		pcom[j]=p[j];
		xicom[j]=xi[j];
	}
	ax=0.0;
	xx=1.0;
	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb);
	*fret=dbrent(ax,xx,bx,(float)TOL,&xmin);
	for (j=0;j<n;j++) {
		xi[j] *= xmin;
		p[j] += xi[j];
	}
	free_vector(xicom);
	free_vector(pcom);
}
#undef TOL

void EnergyMinimizer::nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	//exit(1);
}

float *EnergyMinimizer::vector(long n)
/* allocate a float vector with subscript range v[nl..nh] */
{
	float *v;

	v = new float[n];
	return v;

	/*
	v=(float *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float)));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl+NR_END;
	*/
}

void EnergyMinimizer::free_vector(float *v)
/* free a float vector allocated with vector() */
{
	delete[] v;
}

float EnergyMinimizer::func(float p[])
{
	return energy->func(p);
}

void EnergyMinimizer::dfunc(float p[], float d[])
{
	energy->dfunc(p,d);
}

#define ITMAX 100
#define ZEPS 1.0e-10
#define MOV3(a,b,c, d,e,f) (a)=(d);(b)=(e);(c)=(f);

float EnergyMinimizer::dbrent(float ax, float bx, float cx, float tol, float *xmin)
{
	int iter,ok1,ok2;
	float a,b,d,d1,d2,du,dv,dw,dx,e=0.0;
	float fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;

	a=(ax < cx ? ax : cx);
	b=(ax > cx ? ax : cx);
	x=w=v=bx;
	fw=fv=fx=f1dim(x);
	dw=dv=dx=df1dim(x);
	for (iter=1;iter<=ITMAX;iter++) {
		xm=0.5f*(a+b);
		tol1=tol*(float)fabs(x)+ZEPS;
		tol2=2.0f*tol1;
		if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
			*xmin=x;
			return fx;
		}
		if (fabs(e) > tol1) {
			d1=2.0f*(b-a);
			d2=d1;
			if (dw != dx) d1=(w-x)*dx/(dx-dw);
			if (dv != dx) d2=(v-x)*dx/(dx-dv);
			u1=x+d1;
			u2=x+d2;
			ok1 = (a-u1)*(u1-b) > 0.0 && dx*d1 <= 0.0;
			ok2 = (a-u2)*(u2-b) > 0.0 && dx*d2 <= 0.0;
			olde=e;
			e=d;
			if (ok1 || ok2) {
				if (ok1 && ok2)
					d=(fabs(d1) < fabs(d2) ? d1 : d2);
				else if (ok1)
					d=d1;
				else
					d=d2;
				if (fabs(d) <= fabs(0.5*olde)) {
					u=x+d;
					if (u-a < tol2 || b-u < tol2)
						d=SIGN(tol1,xm-x);
				} else {
					d=0.5f*(e=(dx >= 0.0 ? a-x : b-x));
				}
			} else {
				d=0.5f*(e=(dx >= 0.0 ? a-x : b-x));
			}
		} else {
			d=0.5f*(e=(dx >= 0.0 ? a-x : b-x));
		}
		if (fabs(d) >= tol1) {
			u=x+d;
			fu= f1dim(u);
		} else {
			u=x+SIGN(tol1,d);
			fu=f1dim(u);
			if (fu > fx) {
				*xmin=x;
				return fx;
			}
		}
		du=df1dim(u);
		if (fu <= fx) {
			if (u >= x) a=x; else b=x;
			MOV3(v,fv,dv, w,fw,dw)
			MOV3(w,fw,dw, x,fx,dx)
			MOV3(x,fx,dx, u,fu,du)
		} else {
			if (u < x) a=u; else b=u;
			if (fu <= fw || w == x) {
				MOV3(v,fv,dv, w,fw,dw)
				MOV3(w,fw,dw, u,fu,du)
			} else if (fu < fv || v == x || v == w) {
				MOV3(v,fv,dv, u,fu,du)
			}
		}
	}
	nrerror("Too many iterations in routine dbrent");
	return 0.0;
}

#undef ITMAX
#undef ZEPS
#undef MOV3

float EnergyMinimizer::f1dim(float x)
{
	int j;
	float f,*xt;

	xt=vector(ncom);
	for (j=0;j<ncom;j++) xt[j]=pcom[j]+x*xicom[j];
	f = func(xt);
	free_vector(xt);
	return f;	
}

float EnergyMinimizer::df1dim(float x)
{
	int j;
	float df1=0.0;
	float *xt,*df;

	xt=vector(ncom);
	df=vector(ncom);
	for (j=0;j<ncom;j++) xt[j]=pcom[j]+x*xicom[j];
	dfunc(xt,df);
	for (j=0;j<ncom;j++) df1 += df[j]*xicom[j];
	free_vector(df);
	free_vector(xt);
	return df1;
}

#define GOLD 1.618034f
#define GLIMIT 100.0f
#define TINY 1.0e-20
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);

void EnergyMinimizer::mnbrak(float *ax, float *bx, float *cx, float *fa, float *fb, float *fc)
{
	float ulim,u,r,q,fu,dum;

	*fa=f1dim(*ax);
	*fb=f1dim(*bx);
	if (*fb > *fa) {
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx=(*bx)+GOLD*(*bx-*ax);
	*fc=f1dim(*cx);
	while (*fb > *fc) {
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u=(*bx)-((*bx-*cx)*q-(*bx-*ax)*r)/(2.0f*SIGN(FMAX(fabs(q-r),TINY),q-r));
		ulim=(*bx)+GLIMIT*(*cx-*bx);
		if ((*bx-u)*(u-*cx) > 0.0) {
			fu=f1dim(u);
			if (fu < *fc) {
				*ax=(*bx);
				*bx=u;
				*fa=(*fb);
				*fb=fu;
				return;
			} else if (fu > *fb) {
				*cx=u;
				*fc=fu;
				return;
			}
			u=(*cx)+GOLD*(*cx-*bx);
			fu=f1dim(u);
		} else if ((*cx-u)*(u-ulim) > 0.0) {
			fu=f1dim(u);
			if (fu < *fc) {
				SHFT(*bx,*cx,u,*cx+GOLD*(*cx-*bx))
				SHFT(*fb,*fc,fu,f1dim(u))
			}
		} else if ((u-ulim)*(ulim-*cx) >= 0.0) {
			u=ulim;
			fu=f1dim(u);
		} else {
			u=(*cx)+GOLD*(*cx-*bx);
			fu=f1dim(u);
		}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
	}
}

#undef GOLD
#undef GLIMIT
#undef TINY
#undef SHFT