using System;

namespace Legalsoft.Truffer
{
    /// <summary>
    /// 多维下坡单纯形法
    /// Downhill Simplex Method in Multidimensions
    /// </summary>
    public class Amoeba
    {
        private int nfunc { get; set; }
        private int mpts { get; set; }
        private int ndim { get; set; }
        public double fmin { get; set; }
        private double ftol { get; }
        private double[] y { get; set; }
        private double[,] p { get; set; }

        public Amoeba(double ftoll)
        {
            this.ftol = ftoll;
        }

        public double[] minimize(double[] point, double del, RealValueFun func)
        {
            double[] dels = new double[point.Length];
            for (int i = 0; i < dels.Length; i++)
            {
                dels[i] = del;
            }
            return minimize(point, dels, func);
        }

        public double[] minimize(double[] point, double[] dels, RealValueFun func)
        {
            int ndim = point.Length;
            double[,] pp = new double[ndim + 1, ndim];
            for (int i = 0; i < ndim + 1; i++)
            {
                for (int j = 0; j < ndim; j++)
                {
                    pp[i, j] = point[j];
                }
                if (i != 0)
                {
                    pp[i, i - 1] += dels[i - 1];
                }
            }
            return (minimize(pp, func));
        }

        public double[] minimize(double[,] pp, RealValueFun func)
        {
            const int NMAX = 5000;
            const double TINY = 1.0e-10;

            mpts = pp.GetLength(0);
            ndim = pp.GetLength(1);
            double[] psum = new double[ndim];
            double[] pmin = new double[ndim];
            double[] x = new double[ndim];
            p = pp;
            //y.resize(mpts);
            y = new double[mpts];
            for (int i = 0; i < mpts; i++)
            {
                for (int j = 0; j < ndim; j++)
                {
                    x[j] = p[i, j];
                }
                y[i] = func.funk(x);
            }
            nfunc = 0;
            get_psum(p, psum);
            for (; ; )
            {
                int ilo = 0;
                int ihi = y[0] > y[1] ? (0) : (1);
                int inhi = y[0] > y[1] ? (1) : (0);
                for (int 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;
                    }
                }
                double rtol = 2.0 * Math.Abs(y[ihi] - y[ilo]) / (Math.Abs(y[ihi]) + Math.Abs(y[ilo]) + TINY);
                if (rtol < ftol)
                {
                    Globals.SWAP(ref y[0], ref y[ilo]);
                    for (int i = 0; i < ndim; i++)
                    {
                        Globals.SWAP(ref p[0, i], ref p[ilo, i]);
                        pmin[i] = p[0, i];
                    }
                    fmin = y[0];
                    return (pmin);
                }
                if (nfunc >= NMAX)
                {
                    throw new Exception("NMAX exceeded");
                }
                nfunc += 2;
                double ytry = amotry(p, y, psum, ihi, -1.0, func);
                if (ytry <= y[ilo])
                {
                    ytry = amotry(p, y, psum, ihi, 2.0, func);
                }
                else if (ytry >= y[inhi])
                {
                    double ysave = y[ihi];
                    ytry = amotry(p, y, psum, ihi, 0.5, func);
                    if (ytry >= ysave)
                    {
                        for (int i = 0; i < mpts; i++)
                        {
                            if (i != ilo)
                            {
                                for (int j = 0; j < ndim; j++)
                                {
                                    p[i, j] = psum[j] = 0.5 * (p[i, j] + p[ilo, j]);
                                }
                                y[i] = func.funk(psum);
                            }
                        }
                        nfunc += ndim;
                        get_psum(p, psum);
                    }
                }
                else
                {
                    --nfunc;
                }
            }
        }

        public void get_psum(double[,] p, double[] psum)
        {
            for (int j = 0; j < ndim; j++)
            {
                double sum = 0.0;
                for (int i = 0; i < mpts; i++)
                {
                    sum += p[i, j];
                }
                psum[j] = sum;
            }
        }

        public double amotry(double[,] p, double[] y, double[] psum, int ihi, double fac, RealValueFun func)
        {
            double[] ptry = new double[ndim];
            double fac1 = (1.0 - fac) / ndim;
            double fac2 = fac1 - fac;
            for (int j = 0; j < ndim; j++)
            {
                ptry[j] = psum[j] * fac1 - p[ihi, j] * fac2;
            }
            double ytry = func.funk(ptry);
            if (ytry < y[ihi])
            {
                y[ihi] = ytry;
                for (int j = 0; j < ndim; j++)
                {
                    psum[j] += ptry[j] - p[ihi, j];
                    p[ihi, j] = ptry[j];
                }
            }
            return ytry;
        }
    }
}