#include <stdio.h>
#include <math.h>
#include <values.h>
#include "opt.h"

#define	Debug		1
#define	Static		static

#define SKIPTIME	100	/* print interval for debugging */

static int	nvar;
static dbl	(*objective)();
static dbl	*(*gradient)();

static dbl	*xtmp;
static dbl	*gtmp;

static dbl	*h, *dl, *gm;
static dbl	*dg, *gd;
static dbl	*matq;
static dbl	*hnew;

static dbl	*xnew, *grad, *gradnew, *d;

static void initialize()
{
	xtmp = alloc(dbl, nvar);
	gtmp = alloc(dbl, nvar);
	
	h = alloc(dbl, nvar*nvar);
	dl = alloc(dbl, nvar);
	gm = alloc(dbl, nvar);
	dg = alloc(dbl, nvar);
	gd = alloc(dbl, nvar);
	matq = alloc(dbl, nvar*nvar);
	hnew = alloc(dbl, nvar*nvar);
	
	xnew = alloc(dbl, nvar);
	grad = alloc(dbl, nvar);
	gradnew = alloc(dbl, nvar);
	d = alloc(dbl, nvar);
}

static void terminate()
{
	free(xtmp);
	free(gtmp);
	
	free(h);
	free(dl);
	free(gm);
	free(dg);
	free(gd);
	free(matq);
	free(hnew);
	
	free(xnew);
	free(grad);
	free(gradnew);
	free(d);
}

Static dbl theta_1(xinit, d, finit, p1, ro)
dbl	*xinit;
dbl	*d;
dbl	finit, p1, ro;
{
	int	i;
	
	for (i=0; i<nvar; i++) xtmp[i] = xinit[i] + ro*d[i];
	return ((*objective)(nvar, xtmp) - finit - p1*ro);
}

Static dbl theta_2(xinit, d, p2, ro)
dbl	*xinit;
dbl	*d;
dbl	p2, ro;
{
	int	i;
	
	for (i=0; i<nvar; i++) xtmp[i] = xinit[i] + ro*d[i];
	(*gradient)(nvar, xtmp, gtmp);
	return (vectorvector(nvar, gtmp, d) - p2);
}

static int	search_times = 10000;
static dbl	search_eps = 1.0e-10;
static dbl	mu1 = 0.0002;
static dbl	mu2 = 0.9;

Static dbl Wolfe_line_search(xinit, d)
dbl	*xinit;
dbl	*d;
{
	int	k;
	dbl	t1, t2, ro, dro, a, b;
	dbl	finit, p1, p2;
	
	ro = 1;
	dro = 1;
	
	finit = (*objective)(nvar, xinit);
	(*gradient)(nvar, xinit, gtmp);
	p1 = mu1 * vectorvector(nvar, gtmp, d);
	p2 = mu2 * vectorvector(nvar, gtmp, d);
	
	for (k=0; k<search_times; k++) {
		t1 = theta_1(xinit, d, finit, p1, ro);
		t2 = theta_2(xinit, d, p2, ro);
#if Debug > 1
		if (k % 1 == 0) {
			fprintf(stderr, "%d: t1=%lg t2=%lg\n", k, t1, t2);
		}
#endif
		if (t1 <= 0) {
			if (t2 >= 0) {
				return(ro);
			} else {
				ro += dro;
			}
		} else {
			a = ro - dro;
			b = ro;
			break;
		}
	}
	if (k == search_times) {
		return(0.00);
	}
	
	for (;;) {
#if Debug > 1
		fprintf(stderr, "a=%lg b=%lg\n", a, b);
#endif
		ro = (a + b) / 2.00;
		if (b - a < search_eps) {
			return(ro);
		}
		t1 = theta_1(xinit, d, finit, p1, ro);
		t2 = theta_2(xinit, d, p2, ro);
#if Debug > 1
		fprintf(stderr, "t1=%lg t2=%lg\n", t1, t2);
#endif
		if (t1 <=  0 && t2 >= 0) {
			return(ro);
		}
		if (t1 > 0) {
			b = ro;
		} else {
			a = ro;
		}
	}
}

#if 0

Static void update(dbl *h, dbl *dl, dbl *gm)
{
	int	i, j, k, l;
	dbl	sum;
	
	if ((sum = vectorvector(nvar, dl, gm)) == 0.00) {
		return;
	}
	
	for (i=0; i<nvar; i++) {
		for (k=0; k<nvar; k++) dg[k] = - dl[i]*gm[k]/sum;
		dg[i] += 1;
		for (j=0; j<nvar; j++) {
			for (l=0; l<nvar; l++) gd[l] = - gm[l]*dl[j]/sum;
			gd[j] += 1;
			hnew[i*nvar+j] = 0.00;
			for (k=0; k<nvar; k++) {
				for (l=0; l<nvar; l++) {
					hnew[i*nvar+j] += dg[k]*h[k*nvar+l]*gd[l];
				}
			}
		}
	}
	
	for (i=0; i<nvar; i++) {
		for (j=0; j<nvar; j++) {
			h[i*nvar+j] = hnew[i*nvar+j] + dl[i]*dl[j]/sum;
		}
	}
}

#else

Static void update(dbl *h, dbl *dl, dbl *gm)
{
	int	i, j, k, l;
	dbl	sum;
	dbl	a, p, q;
	
	if ((sum = vectorvector(nvar, dl, gm)) == 0.00) {
		return;
	}
	
	for (i=0; i<nvar; i++) {
		for (j=0; j<nvar; j++) {
			matq[i*nvar+j] = - dl[i]*gm[j]/sum;
		}
		matq[i*nvar+i] += 1.00;
	}
	
	for (i=0; i<nvar; i++) {
		for (j=0; j<nvar; j++) {
			a = 0;
			for (k=0; k<nvar; k++) {
				p = matq[i*nvar+k];
				for (l=0; l<nvar; l++) {
					a += p*h[k*nvar+l]*matq[j*nvar+l];
				}
			}
			hnew[i*nvar+j] = a;
		}
	}
	
	for (i=0; i<nvar; i++) {
		for (j=0; j<nvar; j++) {
			h[i*nvar+j] = hnew[i*nvar+j] + dl[i]*dl[j]/sum;
		}
	}
}

#endif

/*
	minimize function f(x) using BFGS formula
	
	inputs: n --- the number of variables (dimension)
		f --- objective function
			dbl f(int n, dbl x[])
		drv --- gradient of function f
			dbl *drv(int n, dbl x[], dbl d[])
			d[i]: differential coeeficient of f
			      with respect to x_{i}
		xinit --- initial value
		timeout --- the maximum number of iterations
		eps --- small real number to test convergence
		
	outputs: xinit --- solution
		 *fopt --- optimal value
	return value: --- suceess, failure, or timeout
								*/


extern status QuasiNewtonMethod(n, f, drv, xinit, fopt, timeout, eps)
int	n;
dbl	(*f)();
dbl	*(*drv)();
dbl	*xinit;
dbl	*fopt;
int	timeout;
dbl	eps;
{
	int	k;
	dbl	sum, al;
	status	stat;
	static dbl	finit, fnew;
	
	nvar = n;
	objective = f;
	gradient = drv;
	
	/* fprintf(stderr, "number of vars. %d\n", nvar); */
	
	initialize();
	
	matrixunit(nvar, h);
	finit = MAXFLOAT;
	
	stat = failure;
	
	for (k=0; k<timeout; k++) {
		fnew = finit;
		finit = (*f)(nvar, xinit);
		if (finit <= -MAXFLOAT || finit >= MAXFLOAT) {
			fprintf(stderr, "overflow\n");
			stat = failure;
			break;
			/* return(failure); */
		}
#if Debug
		if (k % SKIPTIME == 0) {
			fprintf(stderr, "k = %d   f = %lg\n", k, finit);
			/* vectorfprint(stderr, nvar, xinit); */
		}
#endif
		if (fnew - finit < eps) {
			stat = success;
			break;
			/* *fopt = finit; */
			/* return(success); */
		}
		
		(*drv)(nvar, xinit, grad);
		
		matrixvector(nvar, nvar, d, h, grad);
		scalarvector(nvar, d, -1.00, d);
		
		sum = vectorvector(nvar, grad, d);
		if (sum >= 0) {
			matrixunit(nvar, h);
			matrixvector(nvar, nvar, d, h, grad);
			scalarvector(nvar, d, -1.00, d);
		}
		
		al = Wolfe_line_search(xinit, d);
		if (al == 0.00) {
			fprintf(stderr, "bfgs.c: alpha = %lf\n", al);
			break;
		}
		
		scalarvector(nvar, dl, al, d);
		vectoradd(nvar, xnew, xinit, dl);
		
		(*drv)(nvar, xnew, gradnew);
		vectorsub(nvar, gm, gradnew, grad);
		
		update(h, dl, gm);
		
		vectorcopy(nvar, xinit, xnew);
	}
	
	if (k >= timeout) fprintf(stderr, "timeout\n");
	
	/* fprintf(stderr, "***** end *****\n"); */
	*fopt = finit;
	terminate();
	return (stat);
	
	/* *fopt = finit; */
	/* return(failure); */
}