/* Amoeba routine for optimizing model parameters */
#include <float.h>
#include "cddefines.h"
#define	NMAX	20
#include "varypar.h"
#include "amoeba.h"
/*helper routine for Amoeba */
double AmoebaHelper(float *p, 
  float y[], 
  float SumOfP[], 
  long int mp, 
  long int ndim, 
  double (*func)(float[]), 
  long int ihi, 
  double fac);

void Amoeba(float *p, 
  float y[], 
  long int mp, 
  long int ndim, 
  double toler, 
  double (*func)(float[]), 
  long int *iteram)
{
#define P(I_,J_)	(*(p+(I_)*(mp)+(J_)))
	long int i, 
	  ihi, 
	  ilo, 
	  inhi, 
	  j, 
	  m, 
	  n;
	float SumOfP[NMAX]; 
	double  RTolerance, 
	  sum, 
	  swap, 
	  xmin,
	  xmax,
	  ysave, 
	  ytry;

#	ifdef DEBUG_FUN
	fputs( "<+>Amoeba()\n", debug_fp );
#	endif

	/*uses AmoebaHelper,func */
	*iteram = 0;

L_1:
	for( n=0; n < ndim; n++ )
	{
		sum = 0.;
		for( m=0; m < (ndim + 1); m++ )
		{
			sum += P(n,m);
		}
		SumOfP[n] = (float)sum;
	}

L_2:
	ilo = 1;
	if( y[0] > y[1] )
	{
		ihi = 1;
		inhi = 2;
	}
	else
	{
		ihi = 2;
		inhi = 1;
	}
	for( i=0; i < (ndim + 1); i++ )
	{
		if( y[i] <= y[ilo-1] )
			ilo = i+1;
		if( y[i] > y[ihi-1] )
		{
			inhi = ihi;
			ihi = i+1;
		}
		else if( y[i] > y[inhi-1] )
		{
			if( (i+1) == ihi )
				inhi = i+1;
		}
	}

	/* determine largest dimension of simplex */
	RTolerance = 0.;
	for( i=0; i < ndim; i++ )
	{
		xmin = DBL_MAX;
		xmax = -DBL_MAX;
		for( j=0; j <= ndim; j++ )
		{
			xmax = MAX2(xmax,P(i,j));
			xmin = MIN2(xmin,P(i,j));
		}
		RTolerance = MAX2(RTolerance,xmax-xmin);
	}

	if( RTolerance < toler )
	{
		swap = y[0];
		y[0] = y[ilo-1];
		y[ilo-1] = (float)swap;
		for( n=0; n < ndim; n++ )
		{
			swap = P(n,0);
			P(n,0) = P(n,ilo-1);
			P(n,ilo-1) = (float)swap;
		}
		
#		ifdef DEBUG_FUN
		fputs( " <->Amoeba()\n", debug_fp );
#		endif
		return;
	}

	if( VaryPar.nOptimiz >= VaryPar.nIterOptim )
	{
		fprintf( ioQQQ, " Optimizer exceeding maximum iterations.\n This can be reset with the OPTIMZE ITERATIONS command.\n" );
		
#		ifdef DEBUG_FUN
		fputs( " <->Amoeba()\n", debug_fp );
#		endif
		return;
	}

	*iteram += 2;
	ysave = 1.;/* this needed to get through lint */
	ysave = -ysave;
	ytry = AmoebaHelper(p,y,SumOfP,mp,ndim,func,ihi,ysave);
	if( ytry <= y[ilo-1] )
	{
		ytry = AmoebaHelper(p,y,SumOfP,mp,ndim,func,ihi,2.0);
	}
	else if( ytry >= y[inhi-1] )
	{
		ysave = y[ihi-1];
		ytry = AmoebaHelper(p,y,SumOfP,mp,ndim,func,ihi,0.5);
		if( ytry >= ysave )
		{
			for( i=0; i < (ndim + 1); i++ )
			{
				if( (i+1) != ilo )
				{
					for( j=0; j < ndim; j++ )
					{
						SumOfP[j] = (float)(0.5*(P(j,i) + P(j,ilo-1)));

						VaryPar.vparm[0][j] = (float)SumOfP[j];

						P(j,i) = (float)SumOfP[j];
					}
					y[i] = (float)(*func)(SumOfP);
				}
			}
			*iteram += ndim;
			goto L_1;
		}
	}
	else
	{
		*iteram -= 1;
	}
	goto L_2;
#undef	P
}

/*helper routine for Amoeba */
double AmoebaHelper(float *p, 
  float y[], 
  float SumOfP[], 
  long int mp, 
  long int ndim, 
  double (*func)(float[]), 
  long int ihi, 
  double fac)
{
#define P(I_,J_)	(*(p+(I_)*(mp)+(J_)))
	long int j;
	double amotry_v, 
	  fac1, 
	  fac2, 
	  ytry;
	float ptry[NMAX];

#	ifdef DEBUG_FUN
	fputs( "<+>AmoebaHelper()\n", debug_fp );
#	endif
	/*U    USES func */
	fac1 = (1. - fac)/(double)ndim;
	fac2 = fac1 - fac;
	for( j=0; j < ndim; j++ )
	{
		ptry[j] = (float)(SumOfP[j]*fac1 - P(j,ihi-1)*fac2);
	}
	ytry = (*func)(ptry);

	if( ytry < y[ihi-1] )
	{
		y[ihi-1] = (float)ytry;
		for( j=0; j < ndim; j++ )
		{
			SumOfP[j] += (float)(-P(j,ihi-1) + ptry[j]);
			P(j,ihi-1) = (float)ptry[j];
		}
	}
	amotry_v = ytry;
	
#	ifdef DEBUG_FUN
	fputs( " <->AmoebaHelper()\n", debug_fp );
#	endif
	return( amotry_v );
#	undef	P
}