/*
 * Created on Sep 30, 2004
 *
 */

/**
 * @author DM Auslander
 * Implements the Nelder-Mead nonlinear simplex method
 * Reference: Numerical Recipes in C, section 10.4
 * (www.nr.com)
 * Optimizer
 */
public class SimplexSearch
{
    private OptFunctions funcs = null;  // Reference to a class that implements
	// OptFunctions
    private int maxIt;  // Maximum number of iterations
    private int itCount = 0;  // Iteration counter
    private double sign = 1.0;  // Use 1.0 to minimize, -1.0 to maximize
    private double [][] simplex = null;
    private double [] jSimplex = null;  // Performance values
    private int isBest = -1, isNextBest = -1, isWorst = -1, isNextWorst = -1;
    private int nParam = 0;

    /**
     * Constructor for simplex optimizer
     * @param funcs		a class that implements the optimization functions
     * 					and provides the performance measure and success condition
     * @param maxIt		maximum number of iterations before giving up
     */
    public SimplexSearch(OptFunctions funcs, int maxIt)
    {
        this.funcs = funcs;
        this.maxIt = maxIt;
    }
    
    /**
     * After a call to minimize or maximize, the best score found.  Only
     * valid if minimize or maximize returned true.
     * @return		best score found by optimizer
     */
    public double getBestScore() {
        return (jSimplex == null) ? -1 : jSimplex[isBest];
    }
    
    /**
     * After a call to minimize or maximize, the parameters that
     * correspond to the best score.  Only valid if minimize or maximize
     * returned ture.
     * @return		parameters that caused the best score
     */
    public double[] getBestParams() {
        return (simplex == null) ? null : simplex[isBest];
    }

    /**
     * Perform the minimization by Monte Carlo (random) search.  Basically,
     * find a set of inputs within the xMin, xMax range that provides the
     * minimum performance measure and passes the success condition
     * specified by the OptFunctions passed into the constructor.
     * @param xMin	array of min values for the parameters
     * @param xMax	array of max values for the parameters
     * @return		true if successful, false otherwise
     */
    public boolean minimize(double [] xMin, double [] xMax)
    {
        boolean success = false;
        nParam = xMin.length;  // Number of parameters
        simplex = new double[nParam + 1][nParam]; // Each row is a parameter
        		// vector
        jSimplex = new double[nParam + 1];  // Performance index values

        // Constuct the initial simplex
        // Use the center of the search space to start the simplex
        double [] xCenter = new double[nParam];
        double [] dxs = new double[nParam];  // Offset for simplex construction

        for(int i = 0; i < nParam; i++)
        {
            xCenter[i] = (xMin[i] + xMax[i]) / 2.0;
            dxs[i] = (xMax[i] - xMin[i]) * 0.2;
            simplex[0][i] = xCenter[i];  // First simplex point is at
            	// the center of the parameter space
        }
        jSimplex[0] = sign * funcs.performanceFunc(simplex[0]);

        for(int i = 1; i < (nParam + 1); i++)
        {
            for(int j = 0; j < nParam; j++)
            {
                // i is the simplex number; j is the parameter number
                if((i - 1) == j)
                    simplex[i][j] = xCenter[j] + dxs[j];
                else
                    simplex[i][j] = xCenter[j];
            }
            jSimplex[i] = sign * funcs.performanceFunc(simplex[i]);
            itCount++;
        }

        while(itCount < maxIt)
        {
            sortSimplex();  // Make sure the simplex is sorted
	        //***printSimplex();
	        double [] xTry = new double[nParam];
            double [] xTry2 = new double[nParam];
            double jTry2 = Double.MAX_VALUE;
	        reflectFromWorst(1.0,xTry);
	        double jTry = sign * funcs.performanceFunc(xTry);
	        itCount++;
	        //***System.out.println("itCount, jTry: " + itCount + " " + jTry);
	        if(jTry < jSimplex[isBest])
	        {
	            // This try was better than the best
	            // Try again with a larger step
	            reflectFromWorst(2.0,xTry2);
	            jTry2 = sign * funcs.performanceFunc(xTry2);
	            itCount++;
	            //***System.out.println("jTry2: " + jTry2);
	            if(jTry2 < jTry)
	            {
	                // Still better, replace worst with this and re-sort
	                for(int i = 0; i < nParam; i++)
	                {
	                    simplex[isWorst][i] = xTry2[i];
	                    jSimplex[isWorst] = jTry2;
	                }
	            }
	            else
	            {
	                // Not better, replace worst with xTry and re-sort
	                for(int i = 0; i < nParam; i++)
	                {
	                    simplex[isWorst][i] = xTry[i];
	                    jSimplex[isWorst] = jTry;
	                }
	            }
	        }
	        else if((jTry < jSimplex[isWorst]) ||
	                (jTry2 < jSimplex[isWorst]))
	        {
	            // We can replace the current worst point with either
	            // try or try2
	            if(jTry < jTry2)
	            {
		            // jTry is better than the worst point, replace the worst with
		            // this one and re-sort for another trial
	                for(int i = 0; i < nParam; i++)
	                {
	                    simplex[isWorst][i] = xTry[i];
	                    jSimplex[isWorst] = jTry;
	                }
	            }
	            else
	            {
		            // jTry2 is better than the worst point, replace the worst with
		            // this one and re-sort for another trial
	                for(int i = 0; i < nParam; i++)
	                {
	                    simplex[isWorst][i] = xTry2[i];
	                    jSimplex[isWorst] = jTry2;
	                }
	            }
	        }
	        else
	        {
	            // No good news yet! Try a reduced reflection
	            //  Re-use jTry, xTry -- they are known bad at this
	            // point.
		        reflectFromWorst(0.5, xTry);
		        jTry = sign * funcs.performanceFunc(xTry);
		        if(jTry < jSimplex[isWorst])
		        {
	                for(int i = 0; i < nParam; i++)
	                {
	                    simplex[isWorst][i] = xTry[i];
	                    jSimplex[isWorst] = jTry;
	                }
		        }
		        else
		        {
		            // Nothing worked! Contract about the best point
		            // and try again
		            contractOnBest(0.5);
		        }
	        }
            sortSimplex();  // Make sure the simplex is sorted

            // Call optProgress to see whether to continue searching
	        if(!funcs.optProgress(sign * jSimplex[isBest],
	        	sign * jSimplex[isNextBest],
                simplex[isBest],simplex[isNextBest]))
	        {
	            success = true;
	            isBest = isNextBest;
	            break;
	        }
        } // End of main iteration loop

        return success;
    }

    /**
     * Similar to minimize, but instead of finding the set of inputs with the
     * minimum performance measure, it finds the inputs with the maximum 
     * performance measure.
     */
    public boolean maximize(double [] xMin, double [] xMax)
    {
        sign = -1.0;
        return minimize(xMin,xMax);
    }

    private void printSimplex()
    {
        System.out.println("Simplex:");
        for(int i = 0; i < (nParam + 1); i++)
        {
            for(int j = 0; j < nParam; j++)
            {
                // i is the simplex number; j is the parameter number
                System.out.print(simplex[i][j] + " ");
            }
            System.out.println(" : " + jSimplex[i]);
        }
        System.out.println("isBest isNextBest isWorst isNextWorst: "
                + isBest + " " +
                isNextBest + " " + isWorst + " " + isNextWorst);
        System.out.println("jBest jNextBest jWorst jNextWorst: " +
                sign * jSimplex[isBest] + " " +
                sign * jSimplex[isNextBest] + " " + sign * jSimplex[isWorst]
                + " " + sign * jSimplex[isNextWorst]);
    }

    private void sortSimplex()
    {
        // Get best, next best, and worst indices
        double jBest = jSimplex[0];
        isBest = 0;
        double jWorst = jSimplex[0];
        isWorst = 0;

        for(int i = 1; i < (nParam + 1); i++)
        {
            if(jSimplex[i] < jBest)
            {
                jBest = jSimplex[i];
                isBest = i;
            }
            if(jSimplex[i] > jWorst)
            {
                jWorst = jSimplex[i];
                isWorst = i;
            }
        }
        // Get next best and next worst by searching over all but the best
        double jNextBest = jSimplex[isWorst];
        isNextBest = isWorst;
        double jNextWorst = jSimplex[isBest];
        isNextWorst = isBest;
        for(int i = 0; i < (nParam + 1); i++)
        {
            // Don't consider the best or worst vertices
            if((i == isBest) || (i == isWorst))continue;

            if(jSimplex[i] < jNextBest)
            {
                jNextBest = jSimplex[i];
                isNextBest = i;
            }
            if(jSimplex[i] > jNextWorst)
            {
                jNextWorst = jSimplex[i];
                isNextWorst = i;
            }
        }
    }

    private void reflectFromWorst(double factor, double [] xTry)
    {
        // Reflect from the worst point through the centroid
        // of all points except the worst
        // factor = 1.0 reflects at the same size
        double [] xCentroid = new double[nParam];  // Centroid for reflection
        for(int i = 0; i < (nParam + 1); i++)
        {
            if(i == isWorst)continue;  // Don't include worst point
            for(int j = 0; j < nParam; j++)
                xCentroid[j] += simplex[i][j];
        }
        for(int j = 0; j < nParam; j++)
            xCentroid[j] /= nParam;  // Average the values
        //printParameterVector("Centroid",xCentroid);

        //double [] xDif = new double[nParam];
        for(int i = 0; i < nParam; i++)
        {
            double xDif = xCentroid[i] - simplex[isWorst][i];
            xTry[i] = xCentroid[i] + factor * xDif;
        }
        //printParameterVector("<reflect> xTry", xTry);
    }

    private void contractOnBest(double factor)
    {
        // Contract keeping only the best point
        for(int i = 0; i < (nParam + 1); i++)
        {
            if(i == isBest)continue;  // Don't touch the best point
            for(int j = 0; j < nParam; j++)
            {
                double xNew = simplex[isBest][j] + factor *
            		(simplex[i][j] - simplex[isBest][j]);
                simplex[i][j] = xNew;
           }
           jSimplex[i] = sign * funcs.performanceFunc(simplex[i]);
           itCount++;
        }
    }

    private void printParameterVector(String s,double [] pv)
    {
        System.out.print(s + " ");
        for(int i = 0; i < nParam; i++)
            System.out.print(pv[i] + " ");
        System.out.println();
    }
}