#include <stdio.h>
#include <math.h>

#include "nrutil.h"

#define NMAX 5000   // 関数評価の回数の上限
#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

void amoeba(double **p, double y[], int ndim, double ftol,double (*funk)(double []), int *nfunk)
/*
Nelder-Mead の滑降シンプレックス法による関数 funk(x) の多次元の最小化。
x[ ] は ndim 次元のベクトル。入力する行列 p[ndim+1:p297の多角形(シンプレックス)の頂点数，
ndimより一つ増える][ndim:変数の個数] の ndim+1 個の行は、出発点
でのシンプレックスの頂点の座標を表す ndim 次元のベクトル。入力するベクトル
ftol は関数値（極小点の座標ではない！）の収束を判定するための相対許容値。
戻り時の p と y は、最終位置での ndim+1 個の頂点の座標と関数値（どの関数値も
最小の関数値との相対的な外れが ftol 以内）。nfunk は関数の評価回数。
*/
{
	int i, ihi, ilo, inhi, j, mpts = ndim + 1;
	double rtol, sum, swap, ysave, ytry, *psum;


	for(j = 0; j < mpts; j++){
		y[j] = (*funk)(p[j]); // p[j]は0
		//printf("%lf\n",p[j]);
		//printf("%lf\n",y[j]);
	}

	psum = vector(ndim); // 1次元メモリ確保
	*nfunk = 0;			 // nfunkに値0を代入
	for(j = 0; j < ndim; j++)
	{
		sum = 0.0;
		for(i = 0; i < mpts; i++)	sum += p[i][j]; // 列データをすべて足してる
		psum[j] = sum;
	}
	for(;;)
	{
		ilo = 0;
		/* まずシンプレックスの頂点についてループし、最悪、最悪の次、最良の点を調べる。*/
		if(y[0] > y[1])
		{
			inhi = 1;
			ihi = 0;
		}
		else
		{
			inhi = 0;
			ihi = 1;
		}
		for(i = 0; i < mpts; i++)
		{
			if(y[i] <= y[ilo])	ilo = i;
			if(y[i] > y[ihi])
			{
				inhi = ihi;
				ihi = i;
			}
			else if(y[i] > y[inhi] && i != ihi)	inhi = i;
		}
		rtol = 2.0 * fabs(y[ihi] - y[ilo]) / (fabs(y[ihi]) + fabs(y[ilo]));

		/* 関数値の最大の比を求め、これが十分小さいなら終了。*/
		if(rtol < ftol)				/* 戻る際には最良の点を頭に持って行く */
		{
			SWAP(y[0], y[ilo]);
			for(i = 0; i < ndim; i++)	SWAP(p[0][i], p[ilo][i]);
			break;
		}
		if(*nfunk >= NMAX)
		{
			printf("Process Stop : NMAX(=%d) exceeded.\n", NMAX);
			SWAP(y[0], y[ilo]);
			for(i = 0; i < ndim; i++)	SWAP(p[0][i], p[ilo][i]);
			break;
		}
		*nfunk += 2;
		/* 新しい反復を始める。まず最悪の点を残りの点の重心の反対側に対称移動する。*/
		ytry = amotry(p, y, psum, ndim, funk, ihi, -1.0);
		if(ytry <= y[ilo])
		{
			/* 現在の最良の点より良くなったので、さらに２倍だけ進んでみる。*/
			ytry = amotry(p, y, psum, ndim, funk, ihi, 2.0);
		}
		else if(ytry >= y[inhi])
		{
			/* 対称移動した点は２番目に悪い点よりも悪いので１次元の収縮を試みる。*/
			ysave = y[ihi];
			ytry = amotry(p, y, psum, ndim, funk, ihi, 0.5);
			if(ytry >= ysave)
			{
				/* それでも良くないなら全体を最良点に向かって収縮。*/
				for(i = 0; i < mpts; i++)
				{
					if(i != ilo)
					{
						for(j = 0; j < ndim; j++)
							p[i][j] = psum[j] = 0.5 * (p[i][j] + p[ilo][j]);
						y[i] = (*funk)(psum);
					}
				}
				*nfunk += ndim;			/* 関数の評価回数を数える。 */
				for(j = 0; j < ndim; j++)		/* psum を再計算。 */
				{
					sum = 0.0;
					for(i = 0; i < mpts; i++)	sum += p[i][j];
					psum[j] = sum;
				}
			}
		}
		else	--(*nfunk);				/* 評価回数を補正。 */
	}
	free_vector(psum);
}

#undef SWAP
#undef NMAX