#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <assert.h>
#include "gpk_std.h"
#include "gpklib.h"
#include "numerics.h"

/* #define PRINT */

#define GAMMA	2.02241
#define ALPHA	1.0
#define BETA	0.478993

LOCAL void copy_into(double *to, C double *from, int n)
{
 C double *end = from+n;
 while(from<end)
	*to++ = *from++;
}

double **amoeba_allfree(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc)
{
 int i, ihi, inhi, ilo, iter, j;
 double *y, *pbar, *pr, ypr, yprr, *prr, yhi;

 assert(n > 0);
 iter = 0;
 y = (double *)malloc(sizeof(*y) * (n+1));
 assert(y);
 pbar = (double *)malloc(sizeof(*pbar) * n);
 assert(pbar);
 pr = (double *)malloc(sizeof(*pr) * n);
 assert(pr);
 prr = (double *)malloc(sizeof(*prr) * n);
 assert(prr);
 for(i=0;i<n+1;i++)
 	y[i] = (*funk)(p[i], n, c, nc);

 for(;;)	{
#ifdef PRINT
	 printf("amoeba loop\n");
#endif
	 for(i=ilo=inhi=ihi=0;i<n+1;i++)	{
	 	if(y[i] < y[ilo])	ilo = i;
		if(y[i] > y[ihi])	{inhi = ihi;	ihi = i;}
		}
	 if(inhi == ihi)
	 	for(inhi=ilo,i=0;i<n+1;i++)
			if(i!=ihi && y[i]>y[inhi])	inhi = i;

	 if(2*fabs(y[ihi]-y[ilo]) < (fabs(y[ihi]) + fabs(y[ilo]))*ftol)
		break;
	 if(iter > itmax)	{
#ifdef PRINT
	 	printf("Amoeba has too many iterations");
#endif
		p = 0;
		break;
		}
#ifdef PRINT
	printf("iter++ ");
#endif
	iter++;
	/* evaluate the test vector. This is a vector through the center of
	the face which is opposite the highest point. */
	for(i=0;i<n;i++)
		pbar[i] = 0.0;
	for(j=0;j<n+1;j++)
		if(j != ihi)
			for(i=0;i<n;i++)
				pbar[i] += p[j][i];
	for(i=0;i<n;i++)	{
		pbar[i] /= n;
		pr[i] = (1+ALPHA)*pbar[i] - ALPHA * p[ihi][i];
		}
	/* evaluate the function at the reflected point */
#ifdef PRINT
	printf("funk?");
#endif
	ypr = (*funk)(pr, n, c, nc);
	/* if it is better than the previous best, extrapolate further. */
	if(ypr < y[ilo])	{
#ifdef PRINT
		printf("great!");
#endif
		for(i=0;i<n;i++)
			prr[i] = GAMMA*pr[i] + (1-GAMMA)*pbar[i];
		yprr = (*funk)(prr, n, c, nc);
		if(yprr < ypr)	{
			copy_into(p[ihi], prr, n);
			y[ihi] = yprr;
			}
		else	{
			copy_into(p[ihi], pr, n);
			y[ihi] = ypr;
			}
		}
	/* mediocre result. Just replace the high point. */
	else if(ypr < y[inhi])	{
#ifdef PRINT
		printf("ok");
#endif
		copy_into(p[ihi], pr, n);
		y[ihi] = ypr;
		}
	/* worse than the second-highest point. */
	else	{
			yhi = y[ihi];
			if(ypr < y[ihi])	{
				copy_into(p[ihi], pr, n);
				y[ihi] = ypr;
				}
			/* check to see if an intermediate lower point can be found. */
			for(i=0;i<n;i++)
				prr[i] = BETA*p[ihi][i] + (1-BETA)*pbar[i];
			yprr = (*funk)(prr, n, c, nc);
			/* if the intermediate point is better, accept it... */
			if(yprr < ypr)	{
				copy_into(p[ihi], prr, n);
				y[ihi] = yprr;
				}
			/* if nothing has helped, contract in all directions toward
			the lowest point. */
			if(ypr>=yhi && yprr>=yhi)	{
				for(j=0;j<n+1;j++)	{
					if(j != ilo)
						for(i=0;i<n;i++)
							p[j][i] = 0.5*(p[j][i] + p[ilo][i]);
					y[j] = (*funk)(p[j], n, c, nc);
					}
				}
			}
	}
 free((char *)y);
 free((char *)pbar);
 free((char *)pr);
 free((char *)prr);
 return p;
}

