/********************************

	Multiplier method

********************************/

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

#define	Debug		1
#define	Static		static

static int	nvar, nineq, ncond;
static dbl	*lm, *t;
static dbl	*mu, *r;
static dbl	(*objective)();
static dbl	*(*gradient)();
static dbl	*(*inequalities)();
static dbl	**(*gradineqs)();
static dbl	*(*conditions)();
static dbl	**(*gradconds)();

static dbl	*u, **ug, *umat;
static dbl	*v, **vg, *vmat;
static dbl	*gvalue, *ga;
static dbl	*hvalue, *ha;

static void initialize()
{
	int	i, j;
	
	lm = alloc(dbl, nineq);
	t  = alloc(dbl, nineq);
	
	for (i=0; i<nineq; i++) {
		lm[i] = 0.00;
		t[i] = 10.00;
	}
	
	mu = alloc(dbl, ncond);
	r  = alloc(dbl, ncond);
	
	for (j=0; j<ncond; j++) {
		mu[j] = 0.00;
		r[j] = 10.00;
	}
	
	u = alloc(dbl, nineq);
	v = alloc(dbl, ncond);
	
	ug = alloc(dbl *, nineq);
	vg = alloc(dbl *, ncond);
	
	umat = alloc(dbl, nineq*nvar);
	vmat = alloc(dbl, ncond*nvar);
	
	for (i=0; i<nineq; i++) {
		ug[i] = umat + i*nvar;
	}
	
	for (j=0; j<ncond; j++) {
		vg[j] = vmat + j*nvar;
	}
	
	gvalue = alloc(dbl, nineq);
	ga = alloc(dbl, nineq);
	
	hvalue = alloc(dbl, ncond);
	ha = alloc(dbl, ncond);
}

static void terminate()
{
	free(lm);
	free(t);
	free(mu);
	free(r);
	free(u);
	free(v);
	free(ug);
	free(vg);
	free(umat);
	free(vmat);
	free(gvalue);
	free(ga);
	free(hvalue);
	free(ha);
}

Static dbl function_l(int n, dbl *x)
{
	int	i, j;
	dbl	sum;
	
	sum = (*objective)(n, x);
	
	(*inequalities)(n, x, nineq, u);
	for (i=0; i<nineq; i++) {
		if (lm[i] + t[i]*u[i] >= 0.00) {
			sum += (lm[i] + 0.5*t[i]*u[i])*u[i];
		} else {
			sum += (-0.5)*lm[i]*lm[i]/t[i];
		}
	}
	
	(*conditions)(n, x, ncond, v);
	for (j=0; j<ncond; j++) {
		sum += (mu[j] + 0.5*r[j]*v[j])*v[j];
	}
	
	return (sum);
}

Static dbl *grad_function_l(int n, dbl *x, dbl *d)
{
	int	i, j;
	dbl	scale;
	
	(*gradient)(n, x, d);
	(*inequalities)(n, x, nineq, u);
	(*gradineqs)   (n, x, nineq, ug);
	(*conditions)  (n, x, ncond, v);
	(*gradconds)   (n, x, ncond, vg);
	
	for (i=0; i<nineq; i++) {
		if (( scale = lm[i] + t[i]*u[i] ) >= 0.00) {
			scalarvector(n, ug[i], scale, ug[i]);
			vectoradd(n, d, d, ug[i]);
		}
	}
	
	for (j=0; j<ncond; j++) {
		scale = mu[j] + r[j]*v[j];
		scalarvector(n, vg[j], scale, vg[j]);
		vectoradd(n, d, d, vg[j]);
	}
	
#if Debug >= 2
	fprintf(stderr, "grad of function-l\n");
	vectorfprint(stderr, n, d);
#endif
	return (d);
}

/*	the following values are used for
	the minimization process by quasi-Newton method
	of extended function L(x,mu)				*/

static int	timeoutn = 2000;
static dbl	epsn = 1.0e-20;

extern void MultiplierMethodSetConsts(int tm, dbl ep)
{
	timeoutn = tm;
	epsn = ep;
}

/*
	minimize function f(x) under conditions
	g[o](x)<=0 thought g[m-1]<=0 and h[0](x)=0 through h[l-1](x)=0
	using multiplier method
	
	inputs: n --- the number of variables (dimension)
	
		f --- objective function
			dbl f(int n, dbl x[])
		gradf --- gradient of function f
			dbl *gradf(int n, dbl x[], dbl d[])
			d[i]: differential coefficient of f
			      with respect to x_{i}
		
		m --- the number of inequality conditions
		g --- inequality conditions
			dbl *g(int n, dbl x[], int m, dbl ge[])
			ge[i]: value of condition g_{i}
		gradg --- gradients of individual conditions
			dbl **gradg(int n, dbl x[], int m, dbl *g[])
			(g[i])[k]: differential coefficient of g_{i}
				   with respect to x_{k}
		
		l --- the number of equational conditions
		h --- equational conditions
			dbl *h(int n, dbl x[], int l, dbl he[])
			he[j]: value of condition h_{j}
		gradh --- gradients of individual conditions
			dbl **gradh(int n, dbl x[], int l, dbl *g[])
			(g[i])[k]: differential coefficient of h_{i}
				   with respect to x_{k}
		x --- initial value
		timeout --- the maximum number of iterations
		eps --- small real number to test convergence
		
	outputs: x --- solution
	return value: --- suceess, failure, or timeout
									*/

static dbl fmax(dbl a, dbl b)
{
	if (a >= b) {
		return (a);
	} else {
		return (b);
	}
}

static dbl vectormax(int size, dbl *d)		/* max d[k] */
{
	int	k;
	dbl	max;
	
	max = d[0];
	for (k=0; k<size; k++) {
		if (d[k] >= max) {
			max = d[k];
		}
	}
	return max;
}

extern status MultiplierMethod(n, f, gradf, m, g, gradg, l, h, gradh, x, timeout, eps)
int	n, m, l;
dbl	(*f)(), *(*gradf)();
dbl	*(*g)(), **(*gradg)(), *(*h)(), **(*gradh)();
dbl	x[];
int	timeout;
dbl	eps;
{
	int	count, i, j;
	dbl	alpha = 10, beta = 0.25, c = MAXFLOAT;
	dbl	lopt, gmax, hmax, betac;
	status	s, stat;
	
	nvar = n;
	nineq = m;
	ncond = l;
	objective = f;
	gradient = gradf;
	inequalities = g;
	gradineqs = gradg;
	conditions = h;
	gradconds = gradh;
	
	initialize();
	
	stat = failure;
	
	for (count=0; count<timeout; count++) {
#if Debug
		fprintf(stderr, "%d-th iteration\n", count);
#endif
		s = QuasiNewtonMethod(nvar, function_l, grad_function_l, x,
					 &lopt, timeoutn, epsn);
		if (s != success) {
			stat = s;
			break;
			/* return(stat); */
		}
		
		inequalities(nvar, x, nineq, gvalue);
		conditions(nvar, x, ncond, hvalue);
#if Debug
		fprintf(stderr, "  xopt\n");
		vectorfprint(stderr, nvar, x);
#endif
		for (i=0; i<nineq; i++) {
			ga[i] = fabs( fmax( gvalue[i], -lm[i]/t[i] ) );
		}
		for (j=0; j<ncond; j++) {
			ha[j] = fabs( hvalue[j] );
		}
		
		gmax = vectormax(nineq, ga);
		hmax = vectormax(ncond, ha);
#if Debug > 1
		fprintf(stderr, "gmax = %lf	hmax = %lf\n", gmax, hmax);
		fprintf(stderr, "------------------------------------\n");
#endif
		if (gmax <= c && hmax <= c) {
			/*	Updating c	*/
			if ((c = fmax( gmax, hmax )) < eps) {
				stat = success;
				break;
				/* return(success); */
			}
			/*	Updating lambda and mu	*/
			for (i=0; i<nineq; i++) {
				lm[i] = fmax( 0.00, lm[i]+t[i]*gvalue[i] );
			}
			for (j=0; j<ncond; j++) {
				mu[j] += r[j]*hvalue[j];
			}
		}
#if Debug
		fprintf(stderr, "c = %lf\n", c);
#endif
		
		/*	Updating t and r	*/
		betac = beta*c;
		for (i=0; i<nineq; i++) {
			if (ga[i] > betac) {
				t[i] *= alpha;
			}
		}
		for (j=0; j<ncond; j++) {
			if (ha[j] > betac) {
				r[j] *= alpha;
			}
		}
	}
	
	/* fprintf(stderr, "***** end *****\n"); */
	terminate();
	return (stat);
	/* return(failure); */
}

/*
	for the case of no inequaltional conditions
*/

static void *nullfunction(){}

extern status MultiplierMethodEqConds(n, f, gradf, l, h, gradh, x, timeout, eps)
int	n, l;
dbl	(*f)(), *(*gradf)();
dbl	*(*h)(), **(*gradh)();
dbl	x[];
int	timeout;
dbl	eps;
{
	return MultiplierMethod(n, f, gradf, 0, nullfunction, nullfunction, l, h, gradh, x, timeout, eps);
}