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

static int	nvar;
static double	(*objective)();
static double	**simp, *fvalue;
static int	ih, is, il;
static double	*xcentroid;
static double	fmean, fvar;
static double	*xreflect,  *xcontract, *xexpand;
static double	freflect, fcontract, fexpand;
static double	al = 1, bt = 0.5, gm = 2;

void initialize()
{
	int	i;
	
	simp = alloc(double *, nvar+1);
	for (i=0; i<=nvar; i++) {
		simp[i] = alloc(double, nvar);
	}
	fvalue = alloc(double, nvar+1);
	
	xcentroid = alloc(double, nvar);
	
	xreflect  = alloc(double, nvar);
	xcontract = alloc(double, nvar);
	xexpand   = alloc(double, nvar);
}

void initial_simplex(double *xinit, double length)
{
	int	i, j;
	double	a, d1, d2, *v;
	
	a = nvar + 1;
	d1 = (sqrt(a) + nvar - 1)/sqrt(2.00)/nvar;
	d2 = (sqrt(a) - 1)/sqrt(2.00)/nvar;
	
	v = simp[0];
	for (j=0; j<nvar; j++) v[j] = 0.00;
	for (i=1; i<=nvar; i++) {
		v = simp[i];
		for (j=0; j<nvar; j++) {
			v[j] = d2;
		}
		v[i-1] = d1;
	}
	
	for (i=0; i<=nvar; i++) {
		v = simp[i];
		scalarvector(nvar, v, length, v);
		vectoradd(nvar, v, xinit, v);
	}
}

void search_simplex()
{
	int	i;
	
	if (fvalue[0] > fvalue[1]) {
		ih = 0;
		is = il = 1;
	} else {
		ih = 1;
		is = il = 0;
	}
	
	for (i=2; i<=nvar; i++) {
		if (fvalue[i] > fvalue[ih]) {
			is = ih;
			ih = i;
		} else if (fvalue[i] > fvalue[is]) {
			is = i;
		} else if (fvalue[i] < fvalue[il]) {
			il = i;
		}
	}
}

void compute_xcentroid()
{
	int	i, j;
	double	*x;
	
	for (j=0; j<nvar; j++) xcentroid[j] = 0.00;
	for (i=0; i<=nvar; i++) {
		if (i == ih) continue;
		x = simp[i];
		for (j=0; j<nvar; j++) xcentroid[j] += x[j];
	}
	for (j=0; j<nvar; j++) xcentroid[j] /= nvar;
}

void compute_fmean_fvar()
{
	int	i;
	double	d;
	
	fmean = 0.00;
	for (i=0; i<=nvar; i++) fmean += fvalue[i];
	fmean /= (nvar+1);
	
	fvar = 0.00;
	for (i=0; i<=nvar; i++) {
		d = fvalue[i] - fmean;
		fvar += d*d;
	}
	fvar /= (nvar+1);
}

void reflection()
{
	int	j;
	double	*xh;
	
	xh = simp[ih];
	for (j=0; j<nvar; j++) {
		xreflect[j] = (1+al)*xcentroid[j] - al*xh[j];
	}
	freflect = (*objective)(nvar, xreflect);
}

void contraction()
{
	int	j;
	double	*xh;
	
	xh = simp[ih];
	for (j=0; j<nvar; j++) {
		xcontract[j] = (1-bt)*xcentroid[j] + bt*xh[j];
	}
	fcontract = (*objective)(nvar, xcontract);
}

void expansion()
{
	int	j;
	for (j=0; j<nvar; j++) {
		xexpand[j] = gm*xreflect[j] + (1-gm)*xcentroid[j];
	}
	fexpand = (*objective)(nvar, xexpand);
}

status NelderMeadSimplexMethod
(int n, double (*f)(), double *xinit, double length, double *fopt, int timeout, double eps)
{
	status	stat = failure;
	int	count, i;
	
	nvar = n;
	objective = f;
	
	initialize();
	initial_simplex(xinit, length);
	for (i=0; i<=nvar; i++) {
		fvalue[i] = (*objective)(nvar, simp[i]);
	}
	
	for (count=0; count<timeout; count++) {
		search_simplex();
		compute_xcentroid();
		compute_fmean_fvar();

		if (fvar <= eps) {
			stat = success;
			break;
		}
		reflection();
		if (freflect <= fvalue[is]) {
			if (freflect >= fvalue[il]) {
				(nvar, simp[ih], xreflect);
				fvalue[ih] = freflect;
			} else {
				expansion();
				if (fexpand < fvalue[il]) {
					vectorcopy(nvar, simp[ih], xexpand);
					fvalue[ih] = fexpand;
				} else {
					vectorcopy(nvar, simp[ih], xreflect);
					fvalue[ih] = freflect;
				}
			}
		} else {
			if (freflect < fvalue[ih]) {
				vectorcopy(nvar, simp[ih], xreflect);
				fvalue[ih] = freflect;
			}
			contraction();
			if (fcontract < fvalue[ih]) {
				vectorcopy(nvar, simp[ih], xcontract);
				fvalue[ih] = fcontract;
			} else {
				for (i=0; i<=nvar; i++) {
					if (i == il) continue;
					vectoradd(nvar, simp[i], simp[i], simp[il]);
					scalarvector(nvar, simp[i], 0.50, simp[i]);
					fvalue[i] = (*objective)(nvar, simp[i]);
				}
			}
		}
	}
	vectorcopy(nvar, xinit, simp[il]);
	*fopt = fvalue[il];
	
	return stat;
}

void vectorcopy(int n, double x[], double y[])
{
	int	i;
	for (i=0; i<n; i++) {
		x[i] = y[i];
	}
}

void scalarvector(int n, double y[], double k, double x[])	/* y = kx   k = scalar */
{
	int	i;
	for (i=0; i<n; i++) {
		y[i] = k * x[i];
	}
}

void vectoradd(int n, double x[], double y[], double z[])	/* x = y + z */
{
	int	i;
	for (i=0; i<n; i++) {
		x[i] = y[i] + z[i];
	}
}