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

	Multiplier method using
	Nelder and Mead's simplex 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	*(*inequalities)();
static dbl	*(*conditions)();

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);
	/* vectorfprint(stderr, 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);
	/* vectorfprint(stderr, ncond, v); */
	for (j=0; j<ncond; j++) {
		sum += (mu[j] + 0.5*r[j]*v[j])*v[j];
	}
	
	return (sum);
}

/*
	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[])
		
		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}
		
		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}
		
		x --- initial value
		length --- initial length of simplex
		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;
}

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

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

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

extern status MultiplierMethodSimplex(n, f, m, g, l, h, x, length, timeout, eps)
int	n, m, l;
dbl	(*f)(), *(*g)(), *(*h)();
dbl	x[];
dbl	length;
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;
	inequalities = g;
	conditions = h;
	
	initialize();
	
	stat = failure;
	
	for (count=0; count<timeout; count++) {
#if Debug
		fprintf(stderr, "%d-th iteration (multi-simp)\n", count);
#endif
		s = NelderMeadSimplexMethod(nvar, function_l, x,
					    length, &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);
		fprintf(stderr, "  ineqs\n");
		vectorfprint(stderr, nineq, gvalue);
		fprintf(stderr, "  conds\n");
		vectorfprint(stderr, ncond, hvalue);
#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, "***** multi-simp: end *****\n");
	/* terminate(); */
	return (stat);
	/* return(failure); */
}

/*
	for the case of no inequaltional conditions
*/

static void *nullfunction(){}

extern status MultiplierMethodSimplexEqConds(n, f, l, h, x, length, timeout, eps)
int	n, l;
dbl	(*f)(),	*(*h)();
dbl	x[];
dbl	length;
int	timeout;
dbl	eps;
{
	return MultiplierMethodSimplex(n, f, 0, nullfunction, l, h,
				       x, length, timeout, eps);
}