/*--------------------------------------------------------------------------
 * maximize.ox - code for numerical differentiation and function maximization
 *
 *       (C) Jurgen Doornik 1994-2002
 *
 * 16-05-02 (JAD):- Minor change to NumJacobian, added NumJacobianEx,
 *                  NumJacobianMul
 * 23-01-00 (JAD):- small change in convergence criteria, also using actual
 *				    last delta in BFGS/Newton, not theoretical next.
 *				  - also linesearch when step is big
 *				  - testing for strong conv. if line search fails
 *--------------------------------------------------------------------------*/

#include <oxstd.h>
#include <oxfloat.h>
#include <maximize.h>

/*========================= numerical derivatives ==========================*/

const decl SQRT_EPS =1E-8;        /* appr. square root of machine precision */
const decl DIFF_EPS =1E-8; /* Rice's formula: log(DIFF_EPS)=log(MACH_EPS)/2 */
const decl DIFF_EPS1=5E-6; /* Rice's formula: log(DIFF_EPS)=log(MACH_EPS)/3 */
const decl DIFF_EPS2=1E-4; /* Rice's formula: log(DIFF_EPS)=log(MACH_EPS)/4 */

static dFiniteDiff0(const x)
{
    return max( (fabs(x) + SQRT_EPS) * SQRT_EPS, DIFF_EPS);
}
static dFiniteDiff1(const x)
{
    return max( (fabs(x) + SQRT_EPS) * SQRT_EPS, DIFF_EPS1);
}
static dFiniteDiff2(const x)
{
    return max( (fabs(x) + SQRT_EPS) * SQRT_EPS, DIFF_EPS2);
}

/*----------------------------- Num1Derivative -----------------------------*/
Num1Derivative(const func, vP, const avScore)
{
    decl i, cp = sizer(vP), left, right, fknowf = FALSE, p, h, f, fm, fp, v;
	decl d, hr, hl;

    v = new matrix[cp][1];

    for (i = 0; i < cp; i++)    /* get 1st derivative by central difference */
    {
        p = double(vP[i][0]);
        h = dFiniteDiff1(p);

        vP[i][0] = d = p + h;
		hr = d - p;
        right = func(vP, &fp, 0, 0);

		vP[i][0] = d = p - h;
		hl = p - d;
        left = func(vP, &fm, 0, 0);

		vP[i][0] = p;                         /* restore original parameter */

        if (left && right)
            v[i][0] = (fp - fm) / (hr + hl);     /* take central difference */
        else if (left)
        {
            if (!fknowf)					    /* see if we already know f */
			{	fknowf = func(vP, &f, 0, 0);	      /* not: try to get it */
				if (!fknowf)
                	return FALSE;
			}
            v[i][0] = (f - fm) / hl;                 /* take left difference */
        }
        else if (right)
        {
            if (!fknowf)
			{	fknowf = func(vP, &f, 0, 0);
				if (!fknowf)
                	return FALSE;
			}
            v[i][0] = (fp - f) / hr;               /* take right difference */
        }
        else
            return FALSE;
    }
    avScore[0] = v;

return TRUE;
}
/*--------------------------- END Num1Derivative ---------------------------*/

/*----------------------------- Num2Derivative -----------------------------*/
Num2Derivative(const func, vP, const amHessian)
{
    decl i, j, ret_val, pi, pj, hi, hj, f, fpp, fmm, fpm, fmp, cp = sizer(vP), m;
	decl hia, hja, di, dj;
	
    m = new matrix[cp][cp];
    ret_val = FALSE;

    func(vP, &f, 0, 0);/* get function at vP, around which to differentiate */

    for (i = 0; i < cp; i++)/* approximate 2nd deriv. by central difference */
    {
        pi = double(vP[i][0]);
        hi = dFiniteDiff2(pi);
        ret_val = TRUE;

        for (j = 0; j < i; j++)                    /* off-diagonal elements */
        {     
            pj = double(vP[j][0]);
            hj = dFiniteDiff2(pj);
			
            vP[i][0] = di = pi + hi;  vP[j][0] = dj = pj + hj;     /* +1 +1 */
			hia = di - pi;   hja = dj - pj;
			ret_val = ret_val && func(vP, &fpp, 0, 0);

			vP[i][0] = di = pi - hi;  vP[j][0] = dj = pj - hj;     /* -1 -1 */
			hia += pi - di;   hja += pj - dj;
            ret_val = ret_val && func(vP, &fmm, 0, 0);

            vP[i][0] = pi + hi;  vP[j][0] = pj - hj;               /* +1 -1 */
            ret_val = ret_val && func(vP, &fpm, 0, 0);

            vP[i][0] = pi - hi;  vP[j][0] = pj + hj;               /* -1 +1 */
            ret_val = ret_val && func(vP, &fmp, 0, 0);

            if (!ret_val)
                return FALSE;

            m[i][j] = m[j][i] = ((fpp - fpm) + (fmm - fmp)) / (hia * hja);

            vP[j][0] = pj;
        }
                                                       /* diagonal elements */
        vP[i][0] = di = pi + 2 * hi;           	                   /* +1 +1 */
        ret_val = ret_val && func(vP, &fpp, 0, 0);
		hia = (di - pi) / 2;

        vP[i][0] = di = pi - 2 * hi;           	                   /* -1 -1 */
        ret_val = ret_val && func(vP, &fmm, 0, 0);
		hia += (pi - di) / 2;

        if (!ret_val)
            return FALSE;

        m[i][i] = ((fpp - f) + (fmm - f)) / (hia * hia);

        vP[i][0] = pi;
    }
    amHessian[0] = m;

return ret_val;
}
/*---------------------------- END Num2Derivative --------------------------*/

/*----------------------- numerical Jacobian functions ---------------------*/
NumJacobianEx(const func, vU, const vF, const fCentral, const amJacobian)
{
    decl i, p, h, hi, fp, fm, f0 = <>, cu = sizerc(vU), mj = <>;
	decl fleft, fright;

    // approximate first derivative by taking central or one-sided difference
    for (i = 0; i < cu; i++)
    {
        p = vU[i];
        h = dFiniteDiff0(p);
		hi = 0;

        vU[i] = p + h;
		fright = func(&fp, vU);		// take right difference
		if (fright)					// yes: succeeded
			hi = vU[i] - p;	    	// see, e.g., Dennis&Schnabel Section 5.4

		if (fCentral || !fright)	// central requested or right failed
		{
	        vU[i] = p - h;
			fleft = func(&fm, vU);
			if (fleft)				// yes: left succeeded
				hi += p - vU[i];	// update actual step length
		}
		else
			fleft = FALSE;

		if (!fleft && !fright)
		{
		    amJacobian[0] = mj;
			return 0;
		}
		vU[i] = p;                 	// restore original parameter

		// check step lengths for zeros/negative values
		if (hi <= 0) hi = ((fright != 0) + (fleft != 0)) * h;
		// allocate Jacobian matrix in first iteration
		if (i == 0)
			mj = zeros(fright ? sizer(vec(fp)) : sizer(vec(fm)), cu);

		if (fleft && fright)
	        mj[][i] = (vec(fp) - vec(fm)) / hi;	// central difference
		else
		{
			if (!sizerc(f0)) f0 = vF;		    // first time only
			if (!sizerc(f0)) func(&f0, vU);		// assume always possible
			if (fright)
				mj[][i] = (vec(fp) - f0) / hi; 	// forward difference
			else
		        mj[][i] = (f0 - vec(fm)) / hi; 	// forward difference
		}
    }
    amJacobian[0] = mj;

return 1;
}
NumJacobian(const func, vU, const amJacobian)
{
	return NumJacobianEx(func, vU, <>, TRUE, amJacobian);
}
NumJacobianMul(const func, const vU, const vF, const mD, const fCentral,
	const amJacobian)
{
    decl i, h, hh, hi, x, fp, fm, cd = sizec(mD), mjd = zeros(sizerc(vF), cd);
	decl vd, nd, nu = norm(vU), fleft, fright, f0 = <>;

    /* approximate directional first derivative by taking forward difference*/
    for (i = 0; i < cd; i++)      /* compute the directional gradient delta */
    {
		vd = mD[][i];
		nd = norm(vd);
		if (nd == 0)
			continue;
        h = dFiniteDiff0(nu / nd);
		hh = hi = 0;

		x = vU + h * vd;
		fright = func(&fp, x);			// take right difference
		if (fright)						// yes: succeeded
		{	hi = (x - vU) ./ vd;		// recompute actual step length
			hh = h;
		}
		if (fCentral || !fright)		// central requested or right failed
		{
			x = vU - h * vd;
			fleft = func(&fm, x);
			if (fleft)					// yes: left succeeded
			{	hi += (vU - x) ./ vd;	// update actual step length
				hh += h;
			}
		}
		else
			fleft = FALSE;
		// check step lengths for zeros/negative values
		hi = vd .== 0 .|| hi .<= 0 .? hh .: hi;
		
		if (!fleft && !fright)
		{
		    amJacobian[0] = mjd;
			return 0;
		}
		if (fleft && fright)
	        mjd[][i] = (vec(fp) - vec(fm)) ./ hi;// central difference
		else
		{
			if (fright)
				mjd[][i] = (vec(fp) - vF) ./ hi;// forward difference
			else
		        mjd[][i] = (vF - vec(fm)) ./ hi;// backward difference
		}
    }
    amJacobian[0] = mjd;

return 1;
}
/*--------------------- END numerical Jacobian functions -------------------*/


/*========================= function maximization ==========================*/

static decl s_mxIter = 1000;
static decl s_dEps1 = 1e-4, s_dEps2 = 5e-3;         /* convergence criteria */
static decl s_iPrint = 0;      /* print results every s_iPrint'th iteration */
static decl s_bCompact = 0;     /* TRUE: print iterations in compact format */
static decl s_dStepLinearGap = 1e-14;		/* smallest gap for line search */

/*------------------- fIsConvergence/iConvergenceCode ----------------------*/
static fIsStrongConvergence(const vP, const vScore, const vDelta, const dEps1)
{
	decl vpp = vP .? 1.0 / sqr(sizerc(vP)) + fabs(vP) .: 1;

    if (fabs(vScore .* vpp) < dEps1 && fabs(vDelta ./ vpp) < 10 * dEps1)
        return TRUE;

return FALSE;
}
static fIsWeakConvergence(const vP, const vScore, const dEps2)
{
	decl vpp = vP .? 1.0 / sqr(sizerc(vP)) + fabs(vP) .: 1;

    if (fabs(vScore .* vpp) < dEps2)
        return TRUE;

return FALSE;
}
static iConvergenceCode(const fConv, const fStepOK, const fItMax)
{
    if (!fStepOK)
        return (fConv) ? MAX_WEAK_CONV : MAX_LINE_FAIL;
    else if (fConv)
        return MAX_CONV;
    else
        return MAX_MAXIT;
}
MaxConvergenceMsg(const iCode)
{
    if (iCode == MAX_CONV)
        return "Strong convergence";
    else if (iCode == MAX_WEAK_CONV)
        return "Weak convergence (no improvement in line search)";
    else if (iCode == MAX_MAXIT)
        return "No convergence (maximum no of iterations reached)";
    else if (iCode == MAX_LINE_FAIL)
        return "No convergence (no improvement in line search)";
    else if (iCode == MAX_FUNC_FAIL)
        return "No convergence (function evaluation failed)";
    else
        return "No convergence";
}
MaxControlEps(const dEps1, const dEps2)
{
    if (dEps1 > 0) ::s_dEps1 = dEps1;
    if (dEps2 > 0) ::s_dEps2 = dEps2;
}
MaxControl(const mxIter, const iPrint, ...)
{
    if (mxIter >= 0) ::s_mxIter = mxIter;
    if (iPrint >= 0) ::s_iPrint = iPrint;
	decl va = va_arglist();
	if (sizeof(va) > 0 && va[0] >= 0)
		s_bCompact = va[0];
}
GetMaxControl()
{
	return { s_mxIter, s_iPrint, s_bCompact };
}
GetMaxControlEps()
{
	return { s_dEps1, s_dEps2 };
}
/*----------------- END fIsConvergence/iConvergenceCode --------------------*/

/*----------------------------- MaxMonitor ---------------------------------*/
MaxMonitor(const sMethod, const cIter, const vP, const vScore, const vDelta,
    const dFuncInit, const dFunc, const steplen, const condhes, const ret_val,
	const bCompact)
{
	if (bCompact)
	{
		if (sizer(vScore))
		{	decl dgmax = max(fabs(vScore));
			decl vpp = vP .? 1.0 / sqr(sizerc(vP)) + fabs(vP) .: 1;
    		decl tol1 = norm(vScore .* vpp);
			decl tol2 = norm(vDelta ./ (10 * vpp));
			println("it", "%-4d", cIter, " f=", "%#14.7g", dFunc,
				" df=", "%#10.4g", dgmax, " e1=", "%#10.4g", tol1,
				" e2=", "%#10.4g",tol2, " step=", "%.4g", steplen);
		}
		else if (sizer(vDelta))
		{	decl vpp = vP .? 1.0 / sqr(sizerc(vP)) + fabs(vP) .: 1;
			decl tol2 = norm(vDelta ./ (10 * vpp));
			println("it", "%-4d", cIter, " f=", "%#14.7g", dFunc,
				" e2=", "%#10.4g",tol2, " step=", "%.4g", steplen);
		}
		else
			println("it", "%-4d", cIter, " f=", "%#14.7g", dFunc,
				" step=", "%.4g", steplen);
        if (ret_val != MAX_NOCONV)
            println(MaxConvergenceMsg(ret_val));
		return;
	}
    if (cIter <= 0)
        println("\nStarting values");
    else
    {   println("\nPosition after ", cIter, " ", sMethod, " iterations");
        if (ret_val != MAX_NOCONV)
            println("Status: ", MaxConvergenceMsg(ret_val));
    }
    print("parameters", vP');
    if (columns(vScore))
        print("gradients", vScore');

    if (cIter <= 0)
    	print("Initial function = ", "%19.12g", dFuncInit);
    else
    {   print("function value = ", "%19.12g", dFunc);
        if (condhes != 0)
            print("  condhes = ", condhes);
        if (steplen != 1)
            print("  steplen = ", steplen);
    }
    print("\n");
}
/*----------------------------- END MaxMonitor -----------------------------*/

/*----------------------------- fLineSearch --------------------------------*/
static fLineSearch(const func, const avP, const adFunc, const vDelta,
    const adStep, const dFuncBase, const vScoreBase)
{
    decl dstep = adStep[0], f;

    do                         /* move towards zero with linear line search */
    {   dstep /= 2;
        avP[0] -= dstep * vDelta;

        if (func(avP[0], adFunc, 0, 0) && !isnan(adFunc[0]) && adFunc[0] > dFuncBase)
        {
			/* function & better function value; continue while improvement */
        	while (func(avP[0] - (dstep / 2) * vDelta, &f, 0, 0) && f > adFunc[0])
		    {   dstep /= 2;
        		avP[0] -= dstep * vDelta;
				adFunc[0] = f;
			}
			adStep[0] = dstep;
            return 1;
        }
    } while (dstep >= s_dStepLinearGap);
                                   /* if we're here, the line search failed */
             /* reset function value and parameters to that at steplength 0 */
    avP[0] -= dstep * vDelta;     /* this removes the remaining step length */
    adFunc[0] = dFuncBase; /* dFuncBase was function value of previous iter */
    adStep[0] = dstep;

return 0;                               /* failed to improve in line search */
}
/*------------------------- END fLineSearch --------------------------------*/

/*------------------------------ MaxBFGS -----------------------------------*/
MaxGetParams(const args)
{
	// first process default arguments settings
	decl deps1 = s_dEps1, deps2 = s_dEps2, mxiter = s_mxIter,
		iprint = s_iPrint, bcompact = s_bCompact;
	decl cargs = sizeof(args);
	if (cargs >= 1 && args[0] > 0)  deps1 = args[0];
	if (cargs >= 2 && args[1] > 0)  deps2 = args[1];
	if (cargs >= 3 && args[2] >= 0) mxiter = args[2];
	if (cargs >= 4 && args[3] >= 0) iprint = args[3];
	if (cargs >= 5 && args[4] >= 0) bcompact = args[4];
	return {deps1, deps2, mxiter, iprint, bcompact};
}
MaxBFGS(const func, const avP, const adFunc, const amHessian, const fNumDer, ...)
{
	// first process default arguments settings
	decl deps1, deps2, mxiter, iprint, bcompact;
	[deps1, deps2, mxiter, iprint, bcompact] = MaxGetParams(va_arglist());
	
	// start of MaxBFGS
	decl itno = -1, cp = sizer(avP[0]);
    decl fstepok, fconv, fok, ret_val = MAX_NOCONV, fhesreset = FALSE;
    decl dsteplen, dmxstep, dfprev, dfinit, doldstep;
    decl d_2 = 0, d_2i, d_3;
    decl vscore, vdelta, vpprev, vgamma, vhgamm, vdeltaprev, minvhess;

    vscore = vdelta = vgamma = vhgamm = vpprev = zeros(cp, 1);
    dsteplen = dmxstep = 1.0;

	if (isarray(amHessian))			          /* process amHessian argument */
		minvhess = amHessian[0];
	else if (ismatrix(amHessian))
		minvhess = amHessian;
	else
        minvhess = unit(cp);                        /* default initial Hessian */
	
	if (!isarray(avP) || !isarray(adFunc))
    {   println("MaxBFGS(): arguments 2 and 3 must be an address");
        return MAX_FUNC_FAIL;
    }
	if (columns(avP[0]) > 1)
    {   println("MaxBFGS(): expecting parameters in column vector");
        return MAX_FUNC_FAIL;
    }
	if (sizer(minvhess) != cp || columns(minvhess) != cp)
    {   println("MaxBFGS(): initial Hessian dimension error, using I");
        minvhess = unit(cp);                        /* default initial Hessian */
    }

    if (!func(avP[0], adFunc, 0, 0))
        adFunc[0] = -DBL_MAX;
    dfinit = adFunc[0];
	if (cp == 0)
		return MAX_CONV;

    do                     /* start iterating, first loop is initialization */
    {
		itno++;  fstepok = TRUE;  fconv = FALSE;

		doldstep = dsteplen;
        dsteplen = dmxstep;
        dfprev = adFunc[0];       /* keep old dfunc; compute new parameters */
		vpprev = avP[0];
		vdeltaprev = dsteplen * vdelta;
        avP[0] += vdeltaprev;		  /* new trial value; first time: add 0 */
        vgamma = vscore;                                  /* save old score */

                                         /* evaluate function: 1 is success */
        fok = func(avP[0], adFunc, fNumDer ? 0 : &vscore, 0);
             /* now have analytical score if used, else get numerical later */
		if (isnan(adFunc[0]) || (!fNumDer && isnan(vscore)))
			fok = FALSE;

        if (!fok)                        /* yes: function evaluation failed */
        {   if (itno == 0)         
            {   println("MaxBFGS(): function evaluation failed at starting values");
                return MAX_FUNC_FAIL;
            }
            adFunc[0] = -DBL_MAX;             /* very large negative number */
        }
        if (itno > 0 && (adFunc[0] < dfprev || !fok ||
			dsteplen * norm(vdelta) > 10 * (0.1 + norm(vgamma))) )
        {  /* failure to evaluate or improve: determine steplength in (0,1] */
								   				 /* now also for large step */
	        fstepok = fLineSearch(func, avP, adFunc, vdelta, &dsteplen,	dfprev, vgamma);

            if (!fstepok)                  /* no improvement in line search */
            {       /* loglik/param have been reset to that at steplength 0 */
                vscore = vgamma;                    /* also reset old score */

                if (!fhesreset)                  /* don't do twice in a row */
                {
                    fhesreset = TRUE; /* try once more, with a unit Hessian */
                    vdelta = vscore;                          /* vdelta = q */
                    continue;                                  /* try again */
                }
                else
                {
        			fconv = fIsStrongConvergence(avP[0], vscore, avP[0] - vpprev, deps1);
					if (fconv)
						fstepok = TRUE;
					else
						fconv = fIsWeakConvergence(avP[0], vscore, deps2);
                    break;                                /* stop iterating */
                }
            }

            if (!fok)            /* yes: failed to evaluate at steplength 1 */
                dmxstep = max(dsteplen, 0.1);/*switch to restr. step method */

                       /* success in line search: now need analytical score */
            if (!fNumDer && !func(avP[0], adFunc, &vscore, 0))
                return MAX_FUNC_FAIL;	 /* continue linesearch if failure? */

            vdelta *= dsteplen;           /* actual delta after line search */
        }
        fhesreset = FALSE;                     /* was a succesful iteration */

        if (itno > 0)                      /* adjust restricted step method */
        {   if (dsteplen >= dmxstep)             /* grow maximum steplength */
            {   dmxstep *= 2;
                if (dmxstep > 1) dmxstep = 1.0;
            }
            else if (dmxstep < 1 && dsteplen < dmxstep / 2)       /* shrink */
            {   dmxstep /= 2;
                if (dmxstep < 1e-6) dmxstep = 1e-6;
            }
        }
            /* yes: get numerical score; else already have analytical score */
        if (fNumDer && !Num1Derivative(func, avP[0], &vscore))
        {
            println("MaxBFGS(): numerical score failed");
            return MAX_FUNC_FAIL;
        }

        vgamma -= vscore;                            /* g = change in score */

		if (itno > 0)
		{	   /* Hnew = Hold + [(1 + g'Hg/d'g)/d'g]dd' - (dg'H + Hgd')/d'g */
                  /* first time: d_2 == d_3 == 0, so Hessian doesn't change */
	        vhgamm = minvhess * vgamma;
    	    d_2 = double(vdelta'vgamma);							 /* d'g */
        	d_3 = double(vgamma'vhgamm);						    /* g'Hg */

			if (d_2 <= 0)
    	    {   d_2 = 0;
        	    minvhess = unit(cp);                       /* reset Hessian */
	        }
    	    else
        	{   d_2i = 1 / d_2;                             /* d_2i = 1/d'g */
		        d_3 = (1 + d_3 * d_2i) * d_2i;        /* (1 + g'Hg/d'g)/d'g */

				decl mup = (d_3 / 2 * vdelta) * vdelta' - (d_2i * vhgamm) * vdelta';
				mup += mup';
				minvhess += mup;
			}
		}
        vdelta = minvhess * vscore;                          /* vdelta = Hq */
		vdeltaprev = itno <= 1 ? vdelta : avP[0] - vpprev;
        fconv = fIsStrongConvergence(avP[0], vscore, vdeltaprev, deps1);

        if (!fconv && iprint > 0 && imod(itno, iprint) == 0)
            MaxMonitor("BFGS", itno, avP[0], vscore, vdeltaprev, dfinit, adFunc[0],
				dsteplen, 0, ret_val, bcompact);

    } while (!(fconv || itno >= mxiter));

    ret_val = iConvergenceCode(fconv, fstepok, itno >= mxiter);

    if (iprint > 0)
        MaxMonitor("BFGS", itno, avP[0], vscore, vdeltaprev, dfinit, adFunc[0],
			dsteplen, 0, ret_val, bcompact);

	if (isarray(amHessian))
		amHessian[0] = minvhess;
		
return ret_val;
}
/*----------------------------- END MaxBFGS --------------------------------*/


/*------------------------------ MaxSimplex --------------------------------*/
static mInitSimplex(const func, const vP, const dSteplen, const vDelta)
{
    decl i, cp = sizer(vP), msimplex, df;

    msimplex = new matrix[cp+1][cp+1];         /* construct initial simplex */

           /* each column has parameter point; function value in bottom row */
    msimplex[][] = unit(cp) * vP + diag(dSteplen * vDelta);
    msimplex[][cp] = vP;

    for (i = 0; i <= cp; i++)               /* evaluate function at simplex */
    {
        if (!func(msimplex[0:cp-1][i], &df, 0, 0))
            df = -DBL_MAX;
        msimplex[cp][i] = df;
    }
return msimplex;
}
static fCheckSimplex(const func, vP, const dFunc, const avDelta, const avScore,
    const dEps)
{
    decl i, ret_val = TRUE, p, h, fm, fp, cp = sizer(vP), v;

    avDelta[0] = new matrix[cp][1];
    v = ones(cp,1);

    for (i = 0; i < cp; i++)
    {
        p = vP[i][0];
        avDelta[0][i][0] = h = dFiniteDiff1(p);

        vP[i][0] = p + h;
        ret_val = ret_val && func(vP, &fp, 0, 0);
        vP[i][0] = p - h;
        ret_val = ret_val && func(vP, &fm, 0, 0);

        ret_val = ret_val && (fp <= dFunc || fp - dFunc < dEps)
            && (fm <= dFunc || fm - dFunc < dEps);

        v[i][0] = (fp - fm) / (2 * h);           /* take central difference */

        vP[i][0] = p;                         /* restore original parameter */
    }
    avScore[0] = v;

return ret_val;
}
MaxSimplex(const func, const avP, const adFunc, vDelta)
{
    decl i, itno = -1, cp = sizer(avP[0]), cp1, iconv;
    decl fcontract, fconv, ret_val = MAX_NOCONV, fweak;
    decl dsteplen, dfinit, dfmin, dfmax, dfnew, dfext;
    decl vpmin, vpmax, vpnew, vpext, msimplex, vpcentroid, vscore = <>;
    decl alpha = 1.0, beta = 0.5, gamma = 2.0, algam = alpha * gamma;

    dsteplen = 1.0;
    cp1 = cp - 1;
    iconv = 6;
	vDelta = matrix(vDelta);
	
	if (!isarray(avP) || !isarray(adFunc))
    {   println("MaxSimplex(): avP and adFunc must be an address");
        return MAX_FUNC_FAIL;
    }
    if (vDelta == 0 && !Num1Derivative(func, avP[0], &vDelta))
    {   println("MaxSimplex(): automatic initial simplex: numerical derivatives failed");
        return MAX_FUNC_FAIL;
    }
    if (rows(vDelta) != cp)
    {   println("MaxSimplex(): initial simplex dimension error, vDelta must have ", cp, " rows");
        return MAX_FUNC_FAIL;
    }
    if (vDelta == 0)
    {   println("MaxSimplex(): initial simplex is zero, no progress can be made");
        return MAX_FUNC_FAIL;
    }

	msimplex = mInitSimplex(func, avP[0], dsteplen, vDelta);
    dfinit = dfmax = msimplex[cp][cp];
    vpmax = msimplex[:cp1][cp];

    do                                                   /* start iterating */
    {   itno++;  iconv--;  fcontract = fconv = fweak = FALSE;

        msimplex = sortbyr(msimplex, cp);

        if (s_iPrint > 0 && imod(itno, s_iPrint) == 0)
            MaxMonitor("Simplex", itno, vpmax, <>, <>, dfinit, dfmax, dsteplen, 0, ret_val, s_bCompact);

        dfmin = msimplex[cp][0];   vpmin = msimplex[:cp1][0];
        dfmax = msimplex[cp][cp];  vpmax = msimplex[:cp1][cp];

                                                    /* check for convergence */
        if (iconv < 0 && (dfmax - dfmin) < s_dEps1 * 1e-8)
        {                                      /* yes, now check for minimum */
            if (!fCheckSimplex(func, vpmax, dfmax, &vDelta, &vscore, s_dEps1 * 1e-8) )
            {
                msimplex = mInitSimplex(func, vpmax, 1.0, vDelta);

                if (itno < s_mxIter)
                {   iconv = 6;
                    continue;
                }
                else
                {   break;
                }
            }
            fconv = fIsStrongConvergence(vpmax, vscore, 0, s_dEps1);
            if (fconv)
                break;
        }
        vpcentroid = sumr(msimplex[:cp1][1:]) / cp;    /* omit lowest point */
                                                              /* reflection */
        vpnew = (1 + alpha) * vpcentroid - alpha * vpmin;
        if (!func(vpnew, &dfnew, 0, 0))
            dfnew = -DBL_MAX;

        if (dfnew > dfmax)            /* is an improvement on previous best */
        {                                          /* try an extension step */
            vpext = (1 + algam) * vpcentroid - algam * vpmin;
            if (!func(vpext, &dfext, 0, 0))
                dfext = -DBL_MAX;
            if (dfext > dfnew)       /* is an improvement on the reflection */
                vpnew = vpext, dfnew = dfext;      /* so keep the extension */
        }
        else if (dfnew < msimplex[cp][1]) /* not even better than 2nd worst */
        {                                           /* contract the simplex */
            if (dfnew < dfmin)            /* yes: worse than previous worst */
                vpnew = vpmin, dfnew = dfmin;

            vpext = (1 - beta) * vpcentroid + beta * vpnew;     /* contract */
            if (!func(vpext, &dfext, 0, 0))
                dfext = -DBL_MAX;

            if (dfext > dfnew)             /* contraction is an improvement */
                vpnew = vpext, dfnew = dfext;    /* so keep the contraction */
            else
            {           /* do a general contraction around the current best */
                for (i = 0; i < cp; i++)
                {
                    vpnew = msimplex[:cp1][i] = (msimplex[:cp1][i] + vpmax) * beta;
                    if (!func(vpnew, &dfnew, 0, 0))
                        dfnew = -DBL_MAX;
                    msimplex[cp][i] = dfnew;
                }
                fcontract = TRUE;
            }
        }

        if (!fcontract)
            msimplex[][0] = vpnew, msimplex[cp][0] = dfnew;

    } while (!(fconv || itno >= s_mxIter));

    ret_val = iConvergenceCode(fconv, TRUE, itno >= s_mxIter);
    avP[0] = vpmax;  adFunc[0] = dfmax;

    if (s_iPrint > 0)
        MaxMonitor("Simplex", itno, vpmax, vscore, <>, dfinit, dfmax, 1.0, 0, ret_val, s_bCompact);

return ret_val;
}
/*------------------------------- END MaxSimplex ---------------------------*/

/*-------------------------------- MaxNewton -------------------------------*/
MaxNewton(const func, const avP, const adFunc, const amHessian, const fNumDer, ...)
{
	// first process default arguments settings
	decl deps1, deps2, mxiter, iprint, bcompact;
	[deps1, deps2, mxiter, iprint, bcompact] = MaxGetParams(va_arglist());
	

	// start of MaxNewton
	decl i, itno = -1, cp = sizer(avP[0]);
    decl fstepok, fconv, fsingular = FALSE, fok;
    decl p, h, dfprev, dfinit, dsteplen, ret_val = MAX_NOCONV;
    decl vdelta, vscale, vdiag, vscore, vscoreprev, vdeltaprev, ml, mhess, vpprev;

    vscore = vdelta = zeros(cp, 1);
	mhess = unit(cp);

	if (!isarray(avP) || !isarray(adFunc))
    {   println("MaxNewton(): arguments 2 and 3 must be an address");
        return MAX_FUNC_FAIL;
    }
	if (columns(avP[0]) > 1)
    {   println("MaxNewton(): expecting parameters in column vector");
        return MAX_FUNC_FAIL;
    }
	adFunc[0] = -DBL_MAX;						   /* to avoid (null) value */

                        /* first time (itno == 0): evaluate starting values */
    do                                                   /* start iterating */
    {   itno++;  dsteplen = 1;	fstepok = TRUE;  fconv = FALSE;

		dfprev = adFunc[0];       /* keep old dfunc; compute new parameters */
		vscoreprev = vscore;
		vpprev = avP[0];
        avP[0] += vdelta;			                   /* first time: add 0 */
                                         /* evaluate function: 1 is success */
        fok = func(avP[0], adFunc, &vscore, (fNumDer) ? 0 : &mhess);
		if (isnan(adFunc[0]) || isnan(vscore))
			fok = FALSE;
		if (!fok)
		{
            if (itno == 0)         
            {   println("MaxNewton(): function evaluation failed at starting values");
                return MAX_FUNC_FAIL;
            }
            adFunc[0] = -DBL_MAX;             /* very large negative number */
		}
           /* now have analytical Hessian if used, else get numerical later */

        if (itno == 0)         
			dfinit = adFunc[0];
        else if (adFunc[0] < dfprev || fsingular)
        {  /* failure to evaluate or improve: determine steplength in (0,1] */
            fstepok = fLineSearch(func, avP, adFunc, vdelta, &dsteplen, dfprev, vscoreprev);

			if (!fstepok && !fsingular)  /* not better, try steepest decent */
            {       /* loglik/param have been reset to that at steplength 0 */
        		avP[0] += vscoreprev;
				vdelta = vscoreprev;
	        	if (!func(avP[0], adFunc, 0, 0))
            		adFunc[0] = -DBL_MAX;
				dsteplen = 1;
    	        fstepok = fLineSearch(func, avP, adFunc, vdelta, &dsteplen,	dfprev, vscoreprev);
			}

            if (!fstepok)                  /* no improvement in line search */
            {       /* loglik/param have been reset to that at steplength 0 */
				vscore = vscoreprev;/* ml, vscale are still of steplength 0 */
       			fconv = fIsStrongConvergence(avP[0], vscore, avP[0] - vpprev, deps1);
				if (fconv)
					fstepok = TRUE;
				else
	                fconv = fIsWeakConvergence(avP[0], vscore, deps2);
                break;		                              /* stop iterating */
            }
											/* get derivatives at new point */
			if (!func(avP[0], adFunc, &vscore, (fNumDer) ? 0 : &mhess))
                return MAX_FUNC_FAIL;

        }
        if (fNumDer)
        {
			vdiag = zeros(cp, 1);
            for (i = 0; i < cp; i++)    /* numerical approx.to minus hessian */
            {
                p = avP[0][i];
				h = dFiniteDiff1(p);
                avP[0][i] = p + h;
				if (!func(avP[0], adFunc, &vdiag, 0))
	                return MAX_FUNC_FAIL;
                mhess[][i] = (vscore - vdiag) / h;    /* forward difference */
                avP[0][i] = p;                /* restore original parameter */
            }
			mhess = (mhess + mhess') / 2;        		  /* make symmetric */
        }
		else
			mhess = -mhess;					  /* use minus 2nd der., Q = -H */

		vscale = diagonal(mhess)';			 /* determine new scale factors */
        vscale = vscale .> 0 .? 1 ./ sqrt(vscale) .: 1;
						          /* solve the system q=Qb as Sq=SQSa, Sa=b */
		if (decldl(vscale' .* mhess .* vscale, &ml, &vdiag) == 1)
        {		/* solve system for scaled score; solution stored in vdelta */
            vdelta = solveldl(ml, vdiag, vscore .* vscale) .* vscale;
			fsingular = FALSE;
		}
		else
		{	ml = mhess;						   /* remember for return value */
        	vdelta = vscore;							 /* steepest decent */
			fsingular = TRUE;
        }

		vdeltaprev = itno <= 1 ? vdelta : avP[0] - vpprev;
        fconv = fIsStrongConvergence(avP[0], vscore, vdeltaprev, deps1);

        if (!fconv && iprint > 0 && imod(itno, iprint) == 0)
            MaxMonitor("Newton", itno, avP[0], vscore, vdeltaprev, dfinit, adFunc[0],
				dsteplen, 0, ret_val, bcompact);

    } while (!(fconv || itno >= mxiter));

    ret_val = iConvergenceCode(fconv, fstepok, itno >= mxiter);

	if (iprint > 0)
        MaxMonitor("Newton", itno, avP[0], vscore, vdelta, dfinit, adFunc[0],
			dsteplen, 0, ret_val, bcompact);

	if (isarray(amHessian))
	{	if (fsingular)
			amHessian[0] = -invertgen(ml, 1);    /* use generalized inverse */
		else
			amHessian[0] = -vscale' .* solveldl(ml, vdiag, unit(cp)) .* vscale;
	}
		
return ret_val;
}
/*----------------------------- END MaxNewton ------------------------------*/

