#pragma rtGlobals=1		// Use modern global access method.

//|**************************************************
//|
//| Downhill Simplex Optimizer
//|
//| How to use:
//|
//| (1) Rewrite function to be optimized.
//| The argument should be n-dimensional vector wave, 
//| to solve n-variable problem.
//|	Function fToBeOptimized(w)
//|		Wave w;
//|		Variable x, y;
//|		Variable ans;
//|		x = w[0];
//|		y = w[1];
//|		ans = (x - 1)^2 + 3 * (y - 4)^6;
//|		Return ans;
//|	End Function;	
//|	// for example.
//|
//| (2) Make matrix wave wVertex[n+1][n] for n-variable 
//| (n-dimensional) problem, and input initial values.
//|	Variable nPoint=3, nDim=2;
//|	Make/D/O/N=(nPoint,nDim) wVertex = {{0,10,0},{0,0,10}};
//| or
//|	Variable nPoint=3, nDim=2;
//|	Make/D/O/N=(nPoint,nDim) wVertex;
//|	wVertex[0][0] = 0;
//|	wVertex[0][1] = 0;
//|	wVertex[1][0] = 10;
//|	wVertex[1][1] = 0;
//|	wVertex[2][0] = 0;
//|	wVertex[2][1] = 10;
//|	// for example.
//|
//| (3) Make (n+1)-dimensional vector wave wValue[n+1] 
//| for value of each vertex, and evaluate the function.
//|	Variable nPoint=3, nDim=2;
//|	Variable iPoint, iDim;
//|	Make/D/O/N=(nPoint) wValue;
//|	Make/D/O/N=(nDim) pTry;
//|	iPoint = 0;
//|	Do
//|		iDim = 0;
//|		Do
//|			pTry[iDim] = wVertex[iPoint][iDim];
//|			iDim += 1;
//|		While (iDim < nDim);
//|		wValue[iPoint] = fToBeOptimized(pTry);
//|		iPoint += 1;
//|	While (iPoint < nPoint);
//|	// for example
//|
//|**************************************************

//----------------
//| Test function
//----------------
function fToBeOptimized(w)
	wave w;
	variable x, y, ans;
	x = w[0];
	y = w[1];
	ans = (x-3.0)^2 + 3.0*(y-2.0)^2;
	return ans;
end function;

//----------------
//| Demonstration
//----------------
function DemoAmoeba()
	variable nPoint=3;	// Number of Vertex (n+1)
	variable nDim=2;	// Number of Variables (n)
	variable iPoint, iDim;
	variable iResult;
	variable fTolerance = 1.0E-6;	// Relative tolerance
	make/d/o/n=(nPoint,nDim) wVertex;	// Declare wVertex
	make/d/o/n=(nPoint) wValue;	// Declare wValue

	make/d/o/n=(nDim) pTry;	// Temporary vector

	wVertex={{0,10,0},{0,0,10}};	// Initialize vertex

	make/d/o/n=(100,100) wZ;	// matrix for displaying contour map
	setscale/p x -10,0.3,"",wZ;	// scale along x axis
	setscale/p y -10,0.3,"",wZ;	// scale along y axis
	wZ = fToBeOptimized({x,y});	// calculate matrix for contour map
	dowindow/k Graph0;	// delete "Graph0"
	Display;
	AppendMatrixContour wZ;	// display contour map
	modifygraph width=300,height=300;
	make/d/o/n=4 wx, wy;	// triangle
	appendtograph wy vs wx;	// display triangle
	modifygraph mode(wy)=4,rgb(wy)=(0,0,0);
	doupdate;

	variable waitTime;
	Prompt waitTime,"Wait time in [s] : ";
	DoPrompt/HELP="waitTime is in [s]" "Demo of Amoeba" waitTime;	
	if (V_flag == 1)
		return -1;
	endif;
	variable/g gWaitTime;
	gWaitTime = waitTime;
	iPoint = 0;
	do
		iDim = 0;
		do
			pTry[iDim] = wVertex[iPoint][iDim];
			iDim += 1;
		while (iDim < nDim);
		wValue[iPoint] = fToBeOptimized(pTry);
		iPoint += 1;
	while (iPoint < nPoint);

//	variable V_Flag;
	string sTitle = "Alert";
	string sMessage = "";
	V_flag = 1;
	do
		iResult = Amoeba(wVertex, wValue, fTolerance);
		sMessage = "";
		if (iResult == 0)
			sMessage = sMessage + "Normal termination.\r";
			sMessage = sMessage + " x = "+num2str(wVertex[0][0])+"\r"
			sMessage = sMessage + " y = "+num2str(wVertex[0][1]);
			DoAlert/T=sTitle 0,sMessage;
			V_flag = 2;
		elseif (iResult == -1)
			sMessage = sMessage + "NMAX exceeded !\r";
			sMessage = sMessage + "Continue ?";
			DoAlert/T=sTitle 1, sMessage;
		endif;
	while (V_flag == 1);
end function;

//---------------------------------
//| Show triangle for demostration
//---------------------------------
function ShowAmoeba()
	nvar gWaitTime;	// wait time in [s]
	wave wx, wy;
	wave wVertex;
	wx[0] = wVertex[0][0];
	wy[0] = wVertex[0][1];
	wx[1] = wVertex[1][0];
	wy[1] = wVertex[1][1];
	wx[2] = wVertex[2][0];
	wy[2] = wVertex[2][1];
	wx[3] = wx[0];
	wy[3] = wy[0];
	variable tick0;
	tick0 = ticks;
	do
		;
	while (ticks < tick0 + gWaitTime * 60);
	doupdate;
end function;


//---------------------------------------------------
//| Interface function for downhill simplex algorithm
//---------------------------------------------------
static function funk(wVariable)
	wave wVariable;	// n-dimensional vector
	return fToBeOptimized(wVariable);	// <- replace this function
end;

//-------------------------------------------------------------------------
//| Main routine for downhill simplex method
//|
//| uses static function funk(wVariable)
//| uses static function AmoebaTry(wVertex, wValue, pSum, iHighest, factor)
//|-------------------------------------------------------------------------
function Amoeba(wVertex, wValue, fTolerance)
// multidimensional minimization of a function funk(x)
// for x[0..nDim-1], p[0..nPoint][0..nDim-1]
	wave wVertex;
	wave wValue;
	variable fTolerance;	// factor of tolerance

	variable TINY = 1E-10;
	variable NMAX = 100;

	variable iShowAmoeba = 1;
	
	wave wCoef;	// ?
	variable nDim, iDim;	// number of unknown variables = dimension
	variable nPoint, iPoint;	// number of vertices
	nPoint = numpnts(wValue);
	nDim = nPoint - 1;
	make/d/o/n=(nDim) pSum;
		// sum of vertices (nPoint-times gravity center)
	redimension/n=(nDim) wCoef;	// ?
	variable nFunk = 0;	// total number of times function evaluated
	variable iHighest;	// index of highest (worst) point
	variable iNextHighest;	// index of next heigenst (next worst) point
	variable iLowest;	// index of lowest (best) point
	variable vTemp;	// temporary variable

//	StartProgress("Executing Downhill Simplex ...");	//->"ShowProgressProc"
	//-----------------------------------------------------------
	//| Calculate sum of vertices ( nPoint-times gravity center )
	//-----------------------------------------------------------
	iDim = 0;
	do
		pSum[iDim] = 0;
//		pSum[iDim] = 1;
		iPoint = 0;
		do
			pSum[iDim] += wVertex[iPoint][iDim];
//			pSum[iDim] *= wVertex[iPoint][iDim];
			iPoint += 1;
		while (iPoint < nPoint);
		iDim += 1;
	while (iDim < nDim);

	do
		//---------------------------------------
		//| Find best, worst, second worst points
		//---------------------------------------
		iLowest = 0;
		if (wValue[1] < wValue[0])
			iNextHighest = 1;
			iHighest = 0;
		else
			iNextHighest = 0;
			iHighest = 1;
		endif;
		iPoint = 0;
		do
			if (wValue[iPoint] < wValue[iLowest])
				iLowest = iPoint;
			endif;
			if (wValue[iHighest] < wValue[iPoint])
				iNextHighest = iHighest;
				iHighest = iPoint;
			elseif ((wValue[iNextHighest] < wValue[iPoint]) %& (iPoint != iHighest))
				iNextHighest = iPoint;
			endif;
			iPoint += 1;
		while (iPoint < nPoint);
		
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				if (iShowAmoeba == 1)
					ShowAmoeba();
				endif
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
		
		//------------------------------
		//| Calculate relative tolerance
		//------------------------------
		variable rTol;
		rTol = 2.0 * abs(wValue[iHighest] - wValue[iLowest]);
		rTol = rtol / (abs(wValue[iHighest])+abs(wValue[iLowest]) + TINY);

		if (rTol < fTolerance)
			//--------------------
			//| Normal termination
			//--------------------
			//------------------------------------
			//| Swap wValue[0] and wValue[iLowest]
			//------------------------------------
			vTemp = wValue[0];
			wValue[0] = wValue[iLowest];
			wValue[iLowest] = vTemp;
			//--------------------------------------------------
			//| Swap wVertex[0][iDim] and wVertex[iLowest][iDim]
			//--------------------------------------------------
			iDim = 0;
			do
				vTemp = wVertex[0][iDim];
				wVertex[0][iDim] = wVertex[iLowest][iDim];
				wVertex[iLowest][iDim] = vTemp;
				wCoef[iDim] = wVertex[0][iDim];	// ?
				iDim += 1;
			while (iDim < nDim);
//			print "Reached tolerance"
//			UpdateProgress(100);	//->"ShowProgressProc"
			return 0;
		endif;

		if (NMAX < nFunk)
			//-------------------------------
			//| Too many evaluation of funk()
			//-------------------------------
			//|-----------------------------------
			//| swap wValue[0] and wValue[iLowest]
			//|-----------------------------------
			vTemp = wValue[0];
			wValue[0] = wValue[iLowest];
			wValue[iLowest] = vTemp;
			//--------------------------------------------------
			//| Swap wVertex[0][iDim] and wVertex[iLowest][iDim]
			//--------------------------------------------------
			iDim = 0;
			do
				vTemp = wVertex[0][iDim];
				wVertex[0][iDim] = wVertex[iLowest][iDim];
				wVertex[iLowest][iDim] = vTemp;
				wCoef[iDim] = wVertex[0][iDim];	// ?
				iDim += 1;
			while (iDim < nDim);
//			print "NMAX exceeded";
//			UpdateProgress(100);	//->"ShowProgressProc"
			return -1;
		endif;

		variable vTry;

		//|--------------------------------
		//| "Reflection" of worst point ...
		//|--------------------------------
		vTry = AmoebaTry(wVertex, wValue, pSum, iHighest, -1.0);
		nFunk += 1;
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				if (iShowAmoeba == 1)
					ShowAmoeba();
				endif
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!

		if (vTry <= wValue[iLowest])
			// If reflected point is the best, 
			// try "extrapolation" ...
			//|----------------
			//| "Extrapolation"
			//|----------------
			vTry = AmoebaTry(wVertex, wValue, pSum, iHighest, 2.0);
			nFunk += 1;
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				if (iShowAmoeba == 1)
					ShowAmoeba();
				endif
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
		elseif (wValue[iNextHighest] <= vTry)
			// If reflected point is worse than the second-highest, 
			// try "contraction" ...
			variable vSave;
			vSave = wValue[iHighest];
			//|--------------
			//| "Contraction"
			//|--------------
			vTry = AmoebaTry(wVertex, wValue, pSum, iHighest, 0.5);
			nFunk += 1;
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				if (iShowAmoeba == 1)
					ShowAmoeba();
				endif
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
			if (vSave <= vTry)
				// if contracted point is the worst, try "reduction"
				//-------------
				//| "Reduction"
				//-------------
				iPoint = 0;
				do
					if (iPoint != iLowest)
						iDim = 0;
						do
							pSum[iDim] = 0.5 * (wVertex[iPoint][iDim] + wVertex[iLowest][iDim]);
//							pSum[iDim] = (wVertex[iPoint][iDim] * wVertex[iLowest][iDim])^0.5;
							wVertex[iPoint][iDim] = pSum[iDim];
							iDim += 1;
						while (iDim < nDim);
						wValue[iPoint] = funk(pSum);
						nFunk += 1;
					endif;
					iPoint += 1;
				while (iPoint < nPoint);
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				if (iShowAmoeba == 1)
					ShowAmoeba();
				endif
//		ShowAmoeba();	//!!!!!!!!!!!!!!!!
				//---------------------------------------------------------
				//| Calculate sum of vertices (nPoint-times gravity center)
				//---------------------------------------------------------
				iDim = 0;
				do
					pSum[iDim] = 0;
//					pSum[iDim] = 1;
					iPoint = 0;
					do
						pSum[iDim] += wVertex[iPoint][iDim];
//						pSum[iDim] *= wVertex[iPoint][iDim];
						iPoint += 1;
					while (iPoint < nPoint);
					iDim += 1;
				while (iDim < nDim);

			endif;
		endif;

		//------------------------------------------------
		//| Following block can be used for user abort ...
		//------------------------------------------------
//		if (2 == UpdateProgress(100 * nFunk / NMAX))
//			print "UserAbort: Amoeba2() [DownhillSimplex2Proc]";
//			UpdateProgress() -> "ShowProgressProc"
//			return 2;
//		endif;
	while (1);
// UpdateProgress(100);	//->"ShowProgressProc"
end function;

static function AmoebaTry(wVertex, wValue, pSum, iHighest, factor)
	wave wVertex;
	wave wValue;
	wave pSum;
	variable iHighest;
	variable factor;
		// = -1.0: reflection
		// = 2.0: extrapolation
		// = 0.5: contraction
	variable nPoint, iPoint, nDim, iDim;
	nPoint = numpnts(wValue);
	nDim = nPoint - 1;
	make/d/o/n=(nDim) pTry;	// trial point
	variable factor1, factor2;
	//------------------------------------------
	//| Reflection, Expansion or Contraction ...
	//------------------------------------------
	factor1 = (1.0 - factor) / nDim;
	factor2 = factor1 - factor;
	iDim = 0;
	do
		pTry[iDim] = pSum[iDim] * factor1 - wVertex[iHighest][iDim] * factor2;
//		pTry[iDim] = pSum[iDim] ^ factor1 / wVertex[iHighest][iDim] ^ factor2;
		iDim += 1;
	while (iDim < nDim);

	variable vTry;
	vTry = funk(pTry);	// evaluate the function at the trial point
	if (vTry < wValue[iHighest])
		// If the trial point is better than the worst, apply it
		//----------------------------
		//| Application of trial point
		//----------------------------
		wValue[iHighest] = vTry;
		iDim = 0;
		do
			pSum[iDim] += pTry[iDim] - wVertex[iHighest][iDim];
//			pSum[iDim] *= pTry[iDim] / wVertex[iHighest][iDim];
			wVertex[iHighest][iDim] = pTry[iDim];
			iDim += 1;
		while (iDim < nDim);
	endif;
	return vTry;
end function;