/*--------------------------------------------------------------------------
 * maxsqp.ox - definitions/declarations for quadratic programming
 *
 *       (C) Jurgen Doornik 2001-2002
 *
 * It is not permitted to translate this source code into any other
 * computing language. This code may only be used as part of the
 * Ox system.
 *--------------------------------------------------------------------------*/

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

static maxSQP_getparams(const args)
{
	decl fnjaci = 0, fnjace = 0, deps1, deps2, mxiter, iprint, bcompact,
		cargs = sizeof(args);

	if (cargs > 0)  			   fnjaci 	= 	args[0];
	if (cargs > 1)  			   fnjace 	= 	args[1];

	[deps1, deps2, mxiter, iprint, bcompact] = MaxGetParams(cargs > 2 ? args[2:] : {});

	return {fnjaci, fnjace, deps1, deps2, mxiter, iprint, bcompact};
}
static maxSQP_checkargs(const sFunc, const func, const avP, const adFunc, 
	const amHessian, const cfunc_gt0, const cfunc_eq0, vLo, vHi)
{
	decl cp = sizer(avP[0]), vp, fbounds, iret = 1;
	decl mhess, mchol, ddiag;

	if (!isarray(avP) || !isarray(adFunc))
    {   println(sFunc, "(): arguments 2 and 3 must be an address");
        iret = 0;
    }
	if (columns(avP[0]) > 1)
    {   println(sFunc, "(): expecting parameters in column vector");
        iret = 0;
    }
	if (!isfunction(func))
    {   println(sFunc, "(): argument 1 must be a function");
        iret = 0;
    }
	if ((sizerc(vLo) && sizer(vLo) != cp) || (sizerc(vHi) && sizer(vHi) != cp))
    {   println(sFunc, "(): need bounds for all parameters (may use -.Inf or +.Inf as values)");
        iret = 0;
    }
	if (iret == 0)
		return {<>, 0, vLo, vHi, <>, <>};
		
	fbounds = FALSE;
	if (sizer(vLo))
	{	fbounds = TRUE;
		if (!sizer(vHi))
			vHi = constant(+.Inf, cp, 1);
	}
	if (sizer(vHi))
	{	fbounds = TRUE;
		if (!sizer(vLo))
			vLo = constant(-.Inf, cp, 1);
	}
	vp = avP[0];
	
	if (fbounds)
    {
		vp = vp .< vLo .? vLo .: vp;
		vp = vp .> vHi .? vHi .: vp;
    }

	if (isarray(amHessian))			           /* process amHessian argument */
	{	mhess = amHessian[0];
		mchol = SolveQP_chol(mhess, &ddiag);
	}
	else if (ismatrix(amHessian))
	{	mhess = amHessian;
		mchol = SolveQP_chol(mhess, &ddiag);
	}
	else
        mhess = mchol = unit(sizer(vp));         /* default initial Hessian */

	return {vp, fbounds, vLo, vHi, mhess, mchol};
}
static maxSQP_chol(const vScore, const vScorePrev, const vDelta, const amHess,
	const amChol, const iItno, iReset)
{	
	decl ddiag, d0, d1, vy, vg;
	
	vy = -vScore + vScorePrev;
	vg = amHess[0] * vDelta;
	d0 = vDelta' vg;
	d1 = vDelta' vy;
	if (d1 < 0.2 * d0)
	{
		d1 = 0.8 * d0 / (d0 - d1);
		vy = d1 * vy + (1 - d1) * vg;
		d1 = vDelta'vy;
	}
	if (d0 > 1e-14 && d1 > 1e-14)
	{	amHess[0] += vy * (vy / d1)' - vg * (vg / d0)';
		amChol[0] = SolveQP_chol(amHess[0], &ddiag);
	}
	else if (iItno - iReset > 10)
	{
		vg = diagonal(amHess[0]);
		vg = vg .< 1e-2 .? 1e-2 .: vg .> 1e2 .? 1e2 .: vg;
		amHess[0] = diag(vg);
		amChol[0] = diag(sqrt(vg));
		iReset = iItno;
	}
	// else skip update
	return iReset;
}
static maxSQP_c(const vP, const cfunc_gt0, const cfunc_eq0)
{
	decl vce, vci, dpen_i, dpen_e = 0;
	
	vce = vci = <>;
	dpen_i = dpen_e = 0;
	
	if (isfunction(cfunc_gt0))
	{	cfunc_gt0(&vci, vP);
		if (sizer(vci))
			dpen_i = double(sumc(vci .>= 0 .? 0 .: vci));
	}
	if (isfunction(cfunc_eq0))
	{	cfunc_eq0(&vce, vP);
		if (sizer(vce))
			dpen_e -= double(sumc(fabs(vce)));
	}
	return {vci, vce, dpen_i, dpen_e};
}

static maxSQP_jac(const vP, const cfunc_gt0, const cfunc_eq0,
	const fnJacI, const fnJacE)
{
	decl mjaci, mjace;
	
	if (isfunction(cfunc_gt0))
	{	if (isfunction(fnJacI))
			fnJacI(&mjaci, vP);
		else
			NumJacobian(cfunc_gt0, vP, &mjaci);
	}
	else
		mjaci = <>;

	if (isfunction(cfunc_eq0))
	{	if (isfunction(fnJacE))
			fnJacE(&mjace, vP);
		else
			NumJacobian(cfunc_eq0, vP, &mjace);
	}
	else
		mjace = <>;

	return {mjaci, mjace};
}
MaxFuncEval_01(const func, const vP, const adFunc, const avScore, const fNumDer)
{
	decl fok, f;

	if (isarray(avScore))
	{
		if (!fNumDer)
		{
			fok = func(vP, &f, avScore, 0);	// 0 is failure
			if (isarray(adFunc))
				adFunc[0] = f;
			if (!fok || ismissing(f) || ismissing(avScore[0]))
	        {
	            println("MaxSQP(F): function evaluation failed");
	            return 0;
	        }
            return 1;
		}
        else if (!Num1Derivative(func, vP, avScore) || ismissing(avScore[0]))
        {
            println("MaxSQP(F): numerical score failed");
            return 0;
        }
	}
	if (isarray(adFunc))
	{
		fok = func(vP, adFunc, 0, 0);
	    if (!fok || ismissing(adFunc[0]))
	    {   println("MaxSQP(F): function evaluation failed");
	        return 0;
	    }
	}
    return 1;
}

/*-------------------------------- MaxSQPF ---------------------------------*/
static decl s_MaxSQPF_cfunc;
static MaxSQPF_cfunc_sumsqr(const vP, const adFunc, const avScore, const amHessian)
{
	decl f;
	s_MaxSQPF_cfunc(&f, vP);
	adFunc[0] = - sumc(fabs(f .>= 0 .? 0 .: f - 1e-5));
//	adFunc[0] = - sumsqrc(f - 1e-10);
	return 1;
}

MaxSQPF(const func, const avP, const adFunc, const amHessian, const fNumDer,
	const cfunc_gt0, const cfunc_eq0, vLo, vHi, ...)
{
	// first process default arguments settings
	decl fnjaci, fnjace, deps1, deps2, mxiter, iprint, bcompact;
	[fnjaci, fnjace, deps1, deps2, mxiter, iprint, bcompact] = maxSQP_getparams(va_arglist());

	decl itno, ifail, ireset = -100, ret_val = MAX_NOCONV, retqp, fbounds;
	decl cp, ci, ce, cc, cactn, cactbn;
	decl isactn, isactbn, isactln, isacthn;
	decl dsteplen, dmxstep, dfinit, dfprev, normd, df,
		gamma, mu, d0, d1, ddiag, tol2, eta;
	decl vscore, vdelta, vlambda, vci, vce, vdc, vp, vdprev, vscorelag,
		vscorelagprev, vy, vg, veqlo, veqhi, vcapc, vcinfeas;
	decl mhess, mchol, mjaci, mjace;
	// the following are constants
	decl sqrtepsm = sqrt(DBL_EPSILON), dsteptol = 100 * DBL_EPSILON,
		alpha = 0.1, beta = 0.5, tau = 2.5, delta = 10, etaeps = 0.01,
		capc_lo = 1e-3, capc_hi = 1 / capc_lo, choleps = sqrt(100 * inverteps(0));

	[vp, fbounds, vLo, vHi, mhess, mchol] = maxSQP_checkargs("MaxSQPF",
		func, avP, adFunc, amHessian, cfunc_gt0, cfunc_eq0, vLo, vHi);
	if (vp == <>)
        return MAX_FUNC_FAIL;

	// evaluate constraints
	[vci, vce] = maxSQP_c(vp, cfunc_gt0, cfunc_eq0);
	ci = sizer(vci);
	ce = sizer(vce);
	cc = ce + ci;
	// check for feasibility
	if (ci && any(vci .< 0))
	{
		s_MaxSQPF_cfunc = cfunc_gt0;
		MaxSQPF(MaxSQPF_cfunc_sumsqr, &vp, &d0, 0, TRUE, 0, 0, vLo, vHi);
		cfunc_gt0(&vci, vp);
		if (any(vci .< 0))
		{
			println("MaxSQPF(): failed to obtain feasible starting values");
	        return MAX_FUNC_FAIL;
		}
	}
	if (isfunction(cfunc_eq0))
	{	println("MaxSQPF(): equality constraints not supported");
        return MAX_FUNC_FAIL;
	}
	
	// initialize variables
	cp = sizer(vp);
	avP[0] = vp;
	vscorelag = vscore = vdelta = vdprev = vdc = zeros(cp, 1);
	isactn = isactbn = isactln = isacthn = <>;
	veqlo = veqhi = <>;
	normd = dsteplen = dmxstep = tol2 = 1;
	vcapc = ones(cc, 1);
	
	// evaluate at starting values, with score
	if (!MaxFuncEval_01(func, vp, &df, &vscore, fNumDer))
		return MAX_FUNC_FAIL;

	dfinit = dfprev = adFunc[0] = df = double(df);

    for (itno = ifail = 0; itno <= mxiter; )
    {   
		// already have constraints; get Jacobian function at vp
		[mjaci, mjace] = maxSQP_jac(vp, cfunc_gt0, cfunc_eq0, fnjaci, fnjace);

		// determine eta
		if (tol2 == 0)  	// finish with pure SQP step(s)
			eta = 0;
		else if (ifail == 1)
			eta = 1;
		else if (itno == 0)
			eta = etaeps;
		else if (normd <= etaeps)
		{
			eta = sqr(normd);
			cactbn = sizerc(isactbn);
			if (cactbn)
			{
				[retqp, vdc, vg] = SolveQP_run(mchol, -vscore, <>, <>,
					mjaci[isactbn][], -vci[isactbn], veqlo, veqhi, <>, vdelta,
					range(0, cactbn - 1) ~ cactbn + vecindex(veqlo .!= -.Inf)',
					1e-10, 100, TRUE);
				vg = cactbn ? vg[ : cactbn - 1]	: <>;
					
				d0 = norm(vdc, 2);
				if (retqp == 0 && d0 <= etaeps && !any(vg .< 0))
					eta = sqr(d0);
				else
					eta = sqr(normd);
			}
		}
		else
			eta = sqr(etaeps);

		// step 1a: solve the perturbed QP problem QP(x,H,eta) (sign of q and J changed because maximizing)
		d1 = norm(vscore, 2);		// this scaling makes a big difference!
		if (d1 < 1) d1 = 1;

		[retqp, vdelta, vlambda] = SolveQP_run(
			diagcat(mchol, choleps * norm(mchol)), zeros(cp,1) | 1,
			(mjaci ~ eta * vcapc) | (vscore'/d1 ~ 1/d1), -vci | 0, <>, <>,
			fbounds ? (vLo - vp | -.Inf) : <>,
			fbounds ? (vHi - vp | +.Inf) : <>,
			<>, zeros(cp + 1, 1), isactbn', 1e-10, 100 * cp, TRUE);

		gamma = vdelta[cp];
		mu = vlambda[cc];
		vdelta = vdelta[ : cp - 1];

		if (fbounds)
		{	isactln = vecindex(vlambda[cc + 1 : cc + cp] .> 0);
			isacthn = vecindex(vlambda[cc + cp + 2 : cc + 2 * cp + 1] .> 0);
			veqlo = constant(-.Inf, cp, 1);
			veqhi = constant(+.Inf, cp, 1);
			veqhi[isactln] = veqlo[isactln] = (vLo - vp)[isactln];
			veqhi[isacthn] = veqlo[isacthn] = (vHi - vp)[isacthn];
		}

		if (cc)
		{	vlambda = vlambda[ : cc - 1];
			isactbn = vecindex(vlambda .> 0);
		}
		else
			vlambda = <>;

		// check for convergence
		normd = norm(vdelta, 2);
		vg = vp .? 1.0 / sqr(cp) + fabs(vp) .: 1;
		tol2 = norm(vdelta ./ (10 * vg));
		tol2 = max(tol2, normd);

		if (tol2 < deps1)
		{
			if (eta <= 1e-20)
			{	ret_val = MAX_CONV;
				break;
			}
			tol2 = 0;
			continue;	   			// continue with pure SQP step(s)
		}

		if (iprint > 0 && imod(itno, iprint) == 0)
		{   MaxMonitor("SQPF", itno, vp, <>, vdelta, dfinit,
				df, dsteplen, 0, ret_val, bcompact);
		}
		
		// step 1b: determine the Maratos correction, solving LS^C(x,d,H,I)
		vg = mjaci * vdelta;
		isactn = vecindex(vci + vg + gamma * eta * vcapc .<= sqrtepsm);
		cactn = sizerc(isactn);
		if (cactn)
		{
			if (ci)
				cfunc_gt0(&vci, vp + vdelta);

			vy = vg + min(1e-2 * normd, normd ^ tau) - vci;
			[retqp, vdc, vg] = SolveQP_run(mchol, -vscore, <>, <>,
				mjaci[isactn][], vy[isactn], veqlo, veqhi, <>, vdelta,
				range(0, cactn - 1) ~ cactn + vecindex(veqlo .!= -.Inf)',
				1e-10, 100, TRUE);

			if (retqp == 0 && norm(vdc, 2) <= min(normd, capc_hi))
			{	if (fbounds)
				{	vdc = vdc .< vLo - vp .? vLo - vp .: vdc;
					vdc = vdc .> vHi - vp .? vHi - vp .: vdc;
				}
				vdc -= vdelta;
			}
			else
				vdc[] = 0;
		}
		else
			vdc = zeros(cp, 1);
			
		vp = avP[0];
		// step 2: move into the feasible area
		vcinfeas = <>;
		dfprev = df;
		d0 = double(vscore'vdelta);

		for (dsteplen = dmxstep; dsteplen >= dsteptol; dsteplen *= beta)
		{
			vp = avP[0] + dsteplen * vdelta + sqr(dsteplen) * vdc;

			if (ci)
			{	cfunc_gt0(&vci, vp);
				if (any(vci .< 0) || isnan(vcinfeas))
				{
					if (vcinfeas == <>)
						vcinfeas = vci;
					continue;
				}
			}
			if (!func(vp, &df, 0, 0))/* yes: failed to evaluate at steplength 1 */
            {   dmxstep = max(dsteplen, 0.1);/*switch to restr. step method */
				continue;
			}
			if (df >= dfprev + alpha * dsteplen * d0)
				break;			
		}

		// if no improvement in arc search, try some drastic measures
		if (dsteplen < dsteptol)
	    {
			++ifail;
			if (ifail == 4)
	        {	ret_val = normd < deps2 ? MAX_WEAK_CONV : MAX_LINE_FAIL;
	            break;    			// stop iterating
	        }

			if (ifail == 2)			// ifail=1 uses eta=1
			{
				vcapc[] = 1;		// reset vcapc
			}
			else if (ifail == 3)	// try once more, with a diagonal Hessian
	        {
				ireset = itno;
				vg = diagonal(mhess);
				vg = vg .< 1e-2 .? 1e-2 .: vg .> 1e2 .? 1e2 .: vg;
				mhess = diag(vg);
				mchol = diag(sqrt(vg));
	        }
            continue;
	    }
		
		// update parameters
		vdelta = vp - avP[0];
		avP[0] = vp;
		adFunc[0] = df = double(df);
		vdprev = vdelta;
		++itno;
        // get new score, already have function value and constraints
		if (!MaxFuncEval_01(func, vp, 0, &vscore, fNumDer))
			return MAX_FUNC_FAIL;

		// update quasi-Hessian
		vscorelagprev = vscorelag;
		vscorelag = vscore;
		if (cc)
		{	if (mu > sqrtepsm)
				vlambda /= mu;
			vscorelag += mjaci'vlambda;
		}
		ireset = maxSQP_chol(vscorelag, vscorelagprev, vdelta, &mhess, &mchol, itno, ireset);
		ifail = 0;
		
		// update C
		if (dsteplen < dmxstep)
		{
			if (sizer(vcinfeas))
				vcapc[vecindex(vcinfeas .< 0 .|| isdotnan(vcinfeas))] *= delta;
			else
				vcapc /= delta;
			vcapc = setbounds(vcapc, capc_lo, capc_hi);
		}
		// update `restricted' steplength
        if (dsteplen > dmxstep)
        {   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;
        }
	}
	if (iprint > 0)
    {   MaxMonitor("SQPF", itno, vp, <>, vdelta, dfinit,
			double(df), dsteplen, 0, ret_val, bcompact);
	}
	if (isarray(amHessian))
		amHessian[0] = mhess;
	return ret_val;
}
/*------------------------------- END MaxSQPF ------------------------------*/

/*--------------------------------- MaxSQP ---------------------------------*/
MaxSQP(const func, const avP, const adFunc, const amHessian, const fNumDer,
	const cfunc_gt0, const cfunc_eq0, vLo, vHi, ...)
{
	// first process default arguments settings
	decl fnjaci, fnjace, deps1, deps2, mxiter, iprint, bcompact;
	[fnjaci, fnjace, deps1, deps2, mxiter, iprint, bcompact] = maxSQP_getparams(va_arglist());

	decl itno, ifail, ireset = -100, ret_val = MAX_NOCONV, retqp;
	decl cc, ci, ce, cci, cp, cactn, cactbn, fbounds, istep;
	decl isactn, isactbn;
	decl dsteplen, dmxstep, dfinit, dfprev, df, normd, mui, mue, d0,
		ddiag, tol2, dpen_i, dpen_e, dslopef, dslopep;
	decl vscore, vdelta, vlambda, vci, vce, vc, vdc, vp, vdprev,
		vscorelag, vscorelagprev, vg;
	decl mhess, mchol, mjaci, mjace, mj;
	// the following are constants
	decl sqrtepsm = sqrt(DBL_EPSILON), dsteptol = 100 * DBL_EPSILON,
		alpha = 0.1, beta = 0.5, delta = 0.01;

	[vp, fbounds, vLo, vHi, mhess, mchol] = maxSQP_checkargs("MaxSQP",
		func, avP, adFunc, amHessian, cfunc_gt0, cfunc_eq0, vLo, vHi);
		
	if (vp == <>)
        return MAX_FUNC_FAIL;

	// evaluate constraints
	mui = mue = 1.0;
	[vci, vce, dpen_i, dpen_e] = maxSQP_c(vp, cfunc_gt0, cfunc_eq0);
	ci = cci = sizer(vci);
	ce = sizer(vce);
	cc = ce + ci;

	// initialize variables
	cp = sizer(vp);
	avP[0] = vp;
	vscorelag = vscore = vdelta = vdprev = vdc = zeros(cp, 1);
	isactbn = <>;
	normd = dsteplen = dmxstep = tol2 = 1;

	// evaluate at starting values, with score
	if (!MaxFuncEval_01(func, vp, &df, &vscore, fNumDer))
		return MAX_FUNC_FAIL;
	adFunc[0] = df = double(df);
	df += dpen_i / mui + dpen_e / mue;
	dfinit = df;

	// first do some iterations on equality constraints only
	if (ce && ci)
		istep = ci = 0, vci = <>;
	else
		istep = 1;

    for (itno = ifail = 0; itno <= mxiter; )
    {   
		// already have constraints; get Jacobian function at vp
		[mjaci, mjace] = maxSQP_jac(vp, istep == 0 ? 0 : cfunc_gt0, cfunc_eq0, fnjaci, fnjace);
		// step 1a: solve the QP problem
		[retqp, vdelta, vlambda] = SolveQP_run(mchol, -vscore,
			mjaci, -vci, mjace, -vce,
			fbounds ? vLo - vp : <>, fbounds ? vHi - vp : <>,
			<>, zeros(cp, 1), range(0, ce - 1, 1) ~ ce + isactbn',
			1e-10, 100 * cp, TRUE);

		// check for convergence
		normd = norm(vdelta, 2);
		vg = vp .? 1.0 / sqr(cp) + fabs(vp) .: 1;
		tol2 = norm(vdelta ./ (10 * vg));
		tol2 = max(tol2, normd);

		if (cci && istep == 0 && (norm(vce, 2) < max(normd, delta) || tol2 < deps2))
		{	ci = cci, istep = 1;
			ifail = 0;
			[vci, vce, dpen_i, dpen_e] = maxSQP_c(vp, cfunc_gt0, istep == 0 ? 0 : cfunc_eq0);
			continue;
		}

		if (tol2 < deps1)
		{
			d0 = 0;
			if (sizer(vci))
				d0 = -max(vci .>= 0 .? 0 .: vci);
			if (sizer(vce))
				d0 = max(fabs(vce), d0);
			if (d0 < deps1/10)
			{	ret_val = MAX_CONV;
				break;
			}
		}

		isactbn = isactn = <>;
		cactbn = cactn = 0;
		if (ci)
		{
			isactbn = vecindex(vlambda[ce : cc - 1] .> 0);
			isactn = vecindex(vci .< 0);
			cactbn = sizerc(isactbn);
			cactn = sizerc(isactn);
		}
			
		if (ce)
		{	d0 = norm(vlambda[ : ce - 1]);
			if (1 / mue < d0 + delta)
				mue = 1 / (d0 + 2 * delta);
		}
		if (ci)
		{	d0 = norm(vlambda[ce : cc - 1]);
			if (1 / mui < d0 + delta)
				mui = 1 / (d0 + 2 * delta);
		}

		if (itno == 0)
			df = dfinit = adFunc[0] + dpen_i / mui + dpen_e / mue;
		
		if (iprint > 0 && imod(itno, iprint) == 0)
		{   MaxMonitor("SQP", itno, vp, <>, vdelta, dfinit,
				df, dsteplen, 0, ret_val, bcompact);
		}

		vp = avP[0];
		dslopef = double(vscore'vdelta);
		dslopep = 0;

		// compute slope w.r.t. constraints in merit function
		if (cactn + ce > 0)
		{
			mj = mjaci[isactn][] | mjace;
			vc = vci[isactn] | vce;
			vdc = vc .< 0 .? -mj * vdelta .: vc .> 0 .? mj * vdelta .: mj * fabs(vdelta);
			if (cactn) vdc[ : cactn - 1] /= mui;
			if (ce) vdc[cactn : ] /= mue;
			dslopep = -double(sumc(vdc));
			while (dslopef + dslopep < 0)
			{
				dslopep *= 10;
				if (ci) mui /= 10;
				if (ce) mue /= 10;
			}
		}
		else
			mj = vc = <>;

		// step 2: do the L1 linesearch
		dfprev = df;
		vdc = 0;

		for (dsteplen = dmxstep; dsteplen >= dsteptol; dsteplen *= beta)
		{
			vp = avP[0] + dsteplen * vdelta + sqr(dsteplen) * vdc;
			
			if (!func(vp, &df, 0, 0))  /* yes: failed to evaluate at step 1 */
            {   dmxstep = max(dsteplen, 0.1);/*switch to restr. step method */
				continue;
			}
			adFunc[0] = df = double(df);
			[vci, vce, dpen_i, dpen_e] = maxSQP_c(vp, istep == 0 ? 0 : cfunc_gt0, cfunc_eq0);
			df += dpen_i / mui + dpen_e / mue;

			if (df >= dfprev + alpha * dsteplen * dslopef)
				break;

			if (df == dfprev)
			{	dsteplen = dsteptol;
				break;
			}
			//	compute	Maratos correction
			if (dsteplen == dmxstep && cactbn + ce)
			{	//	better to use QR decomposition
				[vci, vce] = maxSQP_c(vp, istep == 0 ? 0 : cfunc_gt0, cfunc_eq0);
				vc = vci[isactbn] | vce;
				mj = mjaci[isactbn][] | mjace;
				
				vdc = vdelta - mj'invertsym(mj*mj') * vc;

				if (norm(vdc, 2) <= min(2 * normd, 10))
				{	if (fbounds)
					{	vdc = vdc .< vLo - avP[0] .? vLo - avP[0] .: vdc;
						vdc = vdc .> vHi - avP[0] .? vHi - avP[0] .: vdc;
					}
					vdc -= vdelta;
				}
				else
					vdc = 0;
			}
		}

		// if no improvement in arc search, try some drastic measures
		if (dsteplen > dsteptol)
			ifail = 0;
		else 
	    {
			++ifail;
			if (ifail <= 1)			// try once more, increasing mu	and diagonal Hessian
		    {
				ireset = itno;
				vg = diagonal(mhess);
				vg = vg .< 1e-2 .? 1e-2 .: vg .> 1e2 .? 1e2 .: vg;
				mhess = diag(vg);
				mchol = diag(sqrt(vg));
				if (ci) mui *= 10;
				if (ce) mue *= 10;
		    }
			else if (ifail <= 2)
		    {
				if (ci) mui *= 10;
				if (ce) mue *= 10;
		    }
			else
		    {	ret_val = normd < deps2 ? MAX_WEAK_CONV : MAX_LINE_FAIL;
		        break;    			// stop iterating
		    }
	        continue;			
	    }
		
		// update parameters
		vdelta = vp - avP[0];
		avP[0] = vp;
		vdprev = vdelta;
		++itno;
        // get new score, already have function value and constraints
		if (!MaxFuncEval_01(func, vp, 0, &vscore, fNumDer))
			return MAX_FUNC_FAIL;

		// update quasi-Hessian
		vscorelagprev = vscorelag;
		vscorelag = vscore;
		if (cactbn + ce)
		{
			mj = mjaci[isactbn][] | mjace;
			olsc(vscore, mj', &vlambda);
			vscorelag += mj'vlambda;
		}
		ireset = maxSQP_chol(vscorelag, vscorelagprev, vdelta, &mhess, &mchol, itno, ireset);

		// update `restricted' steplength
        if (dsteplen > dmxstep)
        {   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;
        }
	}
	if (iprint > 0)
    {   MaxMonitor("SQP", itno, vp, <>, vdelta, dfinit,
			double(df), dsteplen, 0, ret_val, bcompact);
	}
	if (isarray(amHessian))
		amHessian[0] = mhess;
	return ret_val;
}
/*------------------------------ END MaxSQP --------------------------------*/
