#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "define.h"

static int	nvar, nineq, ncond;
static double	*lm, *t;
static double	*mu, *r;
static double	(*objective)();
static double	*(*inequalities)();
static double	*(*conditions)();
static double	*u, **ug, *umat;
static double	*v, **vg, *vmat;
static double	*gvalue, *ga;
static double	*hvalue, *ha;
static int	timeoutn = 1000;
static double	epsn = 1.0e-20;

static void initialize()
{
	int	i, j;
	
	lm = alloc(double, nineq);
	t  = alloc(double, nineq);
	
	for (i=0; i<nineq; i++) {
		lm[i] = 0.00;
		t[i] = 10.00;
	}
	
	mu = alloc(double, ncond);
	r  = alloc(double, ncond);
	
	for (j=0; j<ncond; j++) {
		mu[j] = 0.00;
		r[j] = 10.00;
	}
	
	u = alloc(double, nineq);
	v = alloc(double, ncond);
	
	ug = alloc(double *, nineq);
	vg = alloc(double *, ncond);
	
	umat = alloc(double, nineq*nvar);
	vmat = alloc(double, 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(double, nineq);
	ga = alloc(double, nineq);
	
	hvalue = alloc(double, ncond);
	ha = alloc(double, ncond);
}

double function_l(int n, double *x)
{
	int	i, j;
	double	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);
}

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

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

MultiplierMethodSimplex(int n, double (*f)(), int m, double *(*g)(), int l, double *(*h)(), double x[], 
									  double length, int timeout, double eps)
{
	int	count, i, j;
	double	alpha = 10, beta = 0.25, c = MAXFLOAT;
	double	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++) {
		s = NelderMeadSimplexMethod(nvar, function_l, x,length, &lopt, timeoutn, epsn);
		if (s != success) {
			stat = s;
			break;
		}
		
		inequalities(nvar, x, nineq, gvalue);
		conditions(nvar, x, ncond, hvalue);

		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 (gmax <= c && hmax <= c) {
			if ((c = fmax( gmax, hmax )) < eps) {
				stat = success;
				break;
			}
			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];
			}
		}
		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;
			}
		}
	}
	return (stat);
}

double *nullfunction(){
	return 0;
}

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