/*--------------------------------------------------------------------------
 * solveqp.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>
#include <solveqp.h>

/*--------------------------------------------------------------------------*/
static solveqp_readvec(const f, const asRownames, const asTypes, const bIsQuad, const bGetNames)
{
	decl line, len, v0, v1, v2, v3, v4, v5;
	decl colname = {}, col = "", ccol = -1, icol = -1, columns = <>;
	decl crows = sizeof(asRownames), row = "", crow = -1;
	decl ctypes = sizeof(asTypes), itype;
	decl ca1, ca2, cb = 1024, i;
	
	if (bGetNames)
		colname = new array[cb];
	columns = new matrix[cb][3 + (ctypes > 0)];

	for (ca1 = ca2 = cb, i = 0; ; )
	{
		line = "";
		if (fscan(f, "%z", &line) < 0 || line[0] != ' ')
			break;
		len = sizeof(line);
		if (len < 22)
			continue;
			
		if (ctypes)
			itype = strfind(asTypes, line[1:2]);

		v1 = strtrim(line[4:11]);
		v2 = strtrim(line[14:21]);
		v3 = -.Inf;
		if (len >= 25)
			sscan(line[24:35], " %g", &v3);
		
		if (v1 != col)
		{
			++ccol;
			if (bGetNames)
			{	if (ccol >= ca1)
				{
					ca1 += cb;
					colname |= new array[cb];
				}
				colname[ccol] = v1;
			}
			col = v1;
			if (bIsQuad)
			{	++icol;
				if (icol >= crows || asRownames[icol] != v1)
					icol = strfind(asRownames, v1);
			}
			else
				icol = ccol;
		}
		if (v2 != row)
		{
			++crow;
			row = v2;
			if (crow >= crows || asRownames[crow] != v2)
				crow = strfind(asRownames, v2);
		}
		if (i + 2 >= ca2)
		{
			ca2 += cb;
			columns |= new matrix[cb][3 + (ctypes > 0)];
		}
		columns[i][0] = icol;
		columns[i][1] = crow;
		columns[i][2] = v3;
		if (ctypes)
			columns[i][3] = itype;
		++i;

		if (len >= 50)
		{
			v4 = strtrim(line[39:46]);
			sscan(line[49:], " %g", &v5);

			if (v4 != row)
			{
				++crow;
				row = v4;
				if (crow >= crows || asRownames[crow] != v4)
					crow = strfind(asRownames, v4);
			}
			columns[i][0] = icol;
			columns[i][1] = crow;
			columns[i][2] = v5;
			++i;
		}
	}
	columns = i ? columns[ : i - 1][] : <>;
	++ccol;
	if (bGetNames) colname = ccol ? colname[ : ccol - 1] : <>;
	
	return {line, columns, ccol, colname};
}	
enum
{	QPS_BOUNDS, QPS_COLUMNS, QPS_ENDATA, QPS_NAME, QPS_QUADOBJ,
	QPS_RANGES, QPS_RHS,     QPS_ROWS
};
enum
{	QPS_G = 1, QPS_L = -1, QPS_E = 0
};
SolveQP_readqps(const sfile)
{
	// QPS is extension to MPS
	// (www-fp.mcs.anl.gov/otc/Guide/OptWeb/continuous/constrained/linearprog/mps.html)
	decl f = fopen(sfile), line, v1, v2, v3;
	decl ascards = {"BOUNDS", "COLUMNS", "ENDATA", "NAME", "QUADOBJ", "RANGES", "RHS", "ROWS"};
	decl icard;
	decl name = "", rowtype = <>, rowname = {}, objname = "";
	decl colname = {}, ccolname = 0, columns = <>, rhs = <>, crhsname = 0;
	decl crngname = 0, ranges = <>, cbndname = 0, bounds = <>;
	decl cquaname = 0, quads = <>;
	decl ca, cb = 1024, i;
	
	if (!isfile(f))
	{
		oxrunerror("failed to open " ~ sfile, 0);
		return 0;
	}
	for (line = ""; ; )
	{
		if (line == "" && fscan(f, "%z", &line) < 0)
		{
			oxrunerror("unexpected end-of-file", 0);
			break;
		}
		sscan(&line, " %s", &v1);
		icard = strfind(ascards, v1);
		if (icard < 0)
		{
			oxrunerror("unknown card " ~ v1, 0);
			break;
		}
		else if (icard == QPS_ENDATA)
		{	break;
		}
		else if (icard == QPS_NAME)
		{
			name = strtrim(line[4:]);
			if (sizeof(name) > 8)
				name = name[:7];
			line = "";
		}
		else if (icard == QPS_ROWS) 			// type label
		{
			rowname = new array[cb];
			rowtype = new matrix[cb][1];

			for (ca = cb, i = 0; ; )
			{
				line = "";
				if (fscan(f, "%z", &line) < 0 || line[0] != ' ')
				{
					rowname = rowname[ : i - 1];
					rowtype = rowtype[ : i - 1];
					break;
				}
				v1 = strtrim(line[1:2])[0];
				v2 = strtrim(line[4:]);

				if (v1 == 'G')
					v1 = 1;
				else if (v1 == 'L')
					v1 = -1;
				else if (v1 == 'E')
					v1 = 0;
				else
				{	if (v1 == 'N' && objname == "")
						objname = v2;
					continue;
				}
				if (i >= ca)
				{
					ca += cb;
					rowname |= new array[cb];
					rowtype |= new matrix[cb][1];
				}
				rowtype[i] = v1;
				rowname[i] = v2;
				++i;
			}
		}
		else if (icard == QPS_COLUMNS) 	// col-label row-label value [row-label value]
		{
			[line, columns, ccolname, colname] = solveqp_readvec(f, rowname, {}, 0, 1);
		}
		else if (icard == QPS_RHS)
		{
			[line, rhs, crhsname] = solveqp_readvec(f, rowname, {}, 0, 0);
		}
		else if (icard == QPS_RANGES)
		{
			[line, ranges, crngname] = solveqp_readvec(f, rowname, {}, 0, 0);
		}
		else if (icard == QPS_BOUNDS)
		{
			[line, bounds, cbndname] = solveqp_readvec(f, colname, {"LO","UP","FX","FR","MI","PL"}, 0, 0);
		}
		else if (icard == QPS_QUADOBJ)
		{
			[line, quads, cquaname] = solveqp_readvec(f, colname, {}, 1, 0);
		}
	}
	fclose(f);

	decl m, b, blo, bhi, idx, sel, ind, n, xlo, xhi, g, g0;

	m = sizeof(rowname);
	n = max(ccolname, cbndname, cquaname);

	g0 = 0;
	if (sizer(rhs))
	{	idx = vecindex(rhs[][1] .== -1);
		if (sizerc(idx))
		{
			g0 = -rhs[idx][2];
			rhs = dropr(rhs, idx);
		}
	}
	g = zeros(n, 1);
	idx = vecindex(columns[][1] .== -1);
	if (sizerc(idx))
	{
		g[ (columns[][0])[idx] ] = (columns[][2])[idx];
		columns = dropr(columns, idx);
	}

	b = zeros(m, 1);
	if (sizer(rhs))
		b[rhs[][1]] = rhs[][2];

	blo = bhi = <>;
	if (crngname)
	{
		// handle ranges
		// both a lower bound lo[i] and upper bound up[i] exist for the row.
		// The range of the constraint is r[i]=up[i]-lo[i] specified in ranges.
		// The value of b[i] is specified in the RHS section data,
		// and the value of r[i] is specified in the RANGES section data, then
		// row type   sign of r[i]    lo[i]        up[i]
		//	 G			 + or -	       b[i]		 b[i]+|r[i]|    
		//	 L			 + or -	    b[i]-|r[i]|	    b[i]
		//	 E			   +		   b[i]    	 b[i]+|r[i]|    
		//	 E			   -		b[i]-|r[i]|	    b[i]
		blo = constant(-.Inf, m, 1);
		bhi = constant(+.Inf, m, 1);
		ind = ranges[][1];
		sel = rowtype[ind] .== 1 .|| (rowtype[ind] .== 0 .&& ranges[][2] .>= 0);
		blo[ind] = sel .? b[ind] .: b[ind] - fabs(ranges[][2]);
		bhi[ind] = sel .? b[ind] + fabs(ranges[][2]) .: b[ind];
	}

	xlo = zeros(n, 1);
	xhi = constant(+.Inf, n, 1);
	if (cbndname)
	{	// 0	 LO 	 b[i]  +.Inf    
		// 1	 UP 	 0      b[i]
		// 2	 FX 	 b[i]   b[i]
		// 3	 FR 	-.Inf  +.Inf
		// 4	 MI 	-.Inf   0
		// 5	 PL 	 0     +.Inf
		sel = bounds[][3];
		ind = bounds[][1];
		idx = vecindex(sel .== 3 .|| sel .== 4);
		if (sizerc(idx))  xlo[ind[idx]] = -.Inf;
		idx = vecindex(sel .== 0 .|| sel .== 2);
		if (sizerc(idx))  xlo[ind[idx]] = bounds[idx][2];

		idx = vecindex(sel .== 4);
		if (sizerc(idx))  xhi[ind[idx]] = 0;
		idx = vecindex(sel .== 1 .|| sel .== 2);
		if (sizerc(idx))  xhi[ind[idx]] = bounds[idx][2];
	}
	if (sumc(xlo .== -.Inf) == n) xlo = <>;
	if (sumc(xhi .== +.Inf) == n) xhi = <>;
	
	return {name, b, blo, bhi, rowtype, xlo, xhi, g, g0, columns, quads};
}
SolveQP_read(const sfile)
{
	decl m, n, name, vb, blo, bhi, btype, xlo, xhi, vg, g0, columns, quadobj;

	[name, vb, blo, bhi, btype, xlo, xhi, vg, g0, columns, quadobj] = SolveQP_readqps(sfile);
	m = sizer(vb);
	n = sizer(vg);

	decl i, j, k, ma, mc = <>, vd = <>, mg, sel;

	if (sizer(quadobj) <= n && quadobj[][0] == quadobj[][1])
	{
		mg = zeros(n, 1);
		mg[quadobj[][0]] = quadobj[][2];
	}
	else
	{
		mg = zeros(n, n);
		for (k = sizer(quadobj) - 1; k >= 0; --k)
		{
			i = quadobj[k][0];
			j = quadobj[k][1];
			mg[i][j] = mg[j][i] = quadobj[k][2];
		}
	}
	ma = zeros(m, n);
	for (k = sizer(columns) - 1; k >= 0; --k)
	{
		ma[columns[k][1]][columns[k][0]] = columns[k][2];
	}

	if (sizer(blo))
	{
		sel = vecindex(blo .> -.Inf);
		if (sizerc(sel))
			vb[sel] = blo[sel];
	}
	if (sizer(bhi))
	{
		sel = vecindex(bhi .< +.Inf);
		if (sizerc(sel))
		{	vb |= -bhi[sel];
			ma |= -ma[sel][];
		}
	}
	
	sel = vecindex(btype .== QPS_L);
	if (sizerc(sel))
	{
		ma[sel][] = -ma[sel][];
		vb[sel] = -vb[sel];
	}
	sel = vecindex(btype .== QPS_E);
	if (sizerc(sel))
	{
		mc = ma[sel][];
		vd = vb[sel];
		ma = dropr(ma, sel);
		vb = dropr(vb, sel);
	}
	return {name, mg, vg, g0, ma, vb, mc, vd, xlo, xhi};
}
/*--------------------------------------------------------------------------*/

/*--------------------------------------------------------------------------*/
static solveqp_getdel(const vLambda, const vMult, decl dRes, const isActive,
	decl me, decl dTol)
{
	decl kdrop, vc;
	
	vc = vLambda ./ vMult;
	vc[ vecindex(isActive .< me .|| dRes * vMult .>= dTol) ] = .NaN;

	kdrop = int(mincindex(fabs(vc')));

	if (kdrop >= 0)
		return {kdrop, vc[kdrop]};
	return {-1, 0};
}

SolveQP_chol(const mG, const adDiag)
{
	decl n = sizer(mG), ve, ddiag = 0, mz, eps = 2 * inverteps(0), flold;

	if (sizec(mG) == 1)
	{
		ddiag = min(mG);
		eps *= norm(mG);
		ddiag = ddiag >= eps ? 0 : max(fabs(ddiag) + eps, eps);
		mz = sqrt(mG + ddiag);
	}
	else
	{	
		flold = oxwarning(0);
		ve = diagonal(mG);
		if (min(ve) == 0)
		{
			ddiag = eps * norm(ve);
			mz = choleski(mG + ddiag * unit(n));
		}
		else
			mz = choleski(mG);
		
		if (mz == 0)
		{
			decl normg = norm(mG);
			if (normg == 0)
				oxrunerror("norm(mG) = 0", 0);
			eigensym(mG / normg, &ve);
			ddiag = ve[n - 1];
			eps *= ve[0];
			ddiag = ddiag >= eps ? 0 : max(fabs(ddiag) + eps, eps);
			for (;;)
			{	mz = choleski(mG / normg + ddiag * unit(n)) * sqrt(normg);
				if (mz == 0)
					ddiag *= 2;
				else
					break;
			}
			ddiag *= normg;
		}
		oxwarning(flold);
	}
	adDiag[0] = ddiag;
	return mz;
}
static solveqp_cholinv(const mG, const adDiag)
{
	decl mz;

	if (sizec(mG) == 1)
	{
		decl ddiag = min(mG);
		decl eps = 2 * inverteps(0) * norm(mG);
		ddiag = ddiag >= eps ? 0 : max(fabs(ddiag) + eps, eps);
		mz = diag(1 ./ sqrt(mG + ddiag));
		adDiag[0] = ddiag;
	}
	else
	{
		mz = SolveQP_chol(mG, adDiag);
		mz = solvelu(mz, 0, 0, unit(sizer(mG)))';// Z=invert(P)'
	}
	return mz;
}

static solveqp_getAb(const isActive, const mA, const vB, const mC,
	const vD, const vLo, const vHi)
{
	// gets Ax, l'A and b for active constraints
	decl n = max(sizec(mA), sizec(mC), sizer(vLo)), cact = sizec(isActive);
	decl sel, idx, vb = zeros(cact, 1);
	decl ma = zeros(cact, n);
	decl me = sizer(mC), m = me + sizer(mA), mn = m + n;
	
	sel = vecindex(isActive .< me);
	if (sizerc(sel))
	{	idx = isActive[sel];
		vb[sel] = vD[idx];
		ma[sel][] = mC[idx][];
	}
	sel = vecindex(isActive .>= me .&& isActive .< m);
	if (sizerc(sel))
	{	idx = isActive[sel] - me;
		vb[sel] = vB[idx];
		ma[sel][] = mA[idx][];
	}
	if (sizerc(vLo))	// assumes both or no bounds at all are specified
	{
		sel = vecindex(isActive .>= m .&& isActive .< mn);
		if (sizerc(sel))
		{	idx = isActive[sel] - m;
			vb[sel] = vLo[idx];
			ma[sel][] = unit(n)[idx][];
		}
		sel = vecindex(isActive .>= mn);
		if (sizerc(sel))
		{	idx = isActive[sel] - mn;
			vb[sel] = -vHi[idx];
			ma[sel][] = -unit(n)[idx][];
		}
	}
	return {ma, vb};
}
static solveqp_getScale(const mA, const mC, const vDelA, const cBounds)
{
	decl vscale = <>, me = sizer(mC);
	if (me)
		vscale ~= sqrt(sumr(sqr(mC))');
	if (sizer(mA))
	{	vscale ~= sqrt(sumr(sqr(mA))');
		if (sizerc(vDelA))
			vscale[me + vecindex(vDelA)] = 0;
	}
	vscale = vscale .== 0 .? 0 .: 1 ./ vscale;	// reciprocal of length of constraints
	vscale ~= ones(1, 2 * cBounds);

	return vscale;
}

SolveQP_decqr(const mA, const vB, const mC,
	const vD, const vLo, const vHi, const mZ, isActive)
{
	// gets Ax, l'A and b for active constraints
	decl n = max(sizec(mA), sizec(mC), sizer(vLo)), cact = sizec(isActive);
	decl ma, mh, vpiv, idx, csel, mzq, mr;
	if (cact == 0)
		return {mZ, <>, <>, <>};

	if (isActive == -1 && mZ == <> && vLo == <> && vHi == <>)
	{
		cact = sizer(mC) + sizer(mA);
		if (cact == 0)
			return {mZ, <>, <>, <>};
		isActive = range(0, cact - 1);
		decqr(mC' | mA', &mh, &mr, &vpiv);
	}
	else
	{
		[ma] = solveqp_getAb(isActive, mA, vB, mC, vD, vLo, vHi);
		decqr(mZ == <> ? ma' : mZ' * ma', &mh, &mr, &vpiv);
	}
	// did QR with pivoting to allow removing redundant constraints
	idx = range(0, cact - 1)';
	vpiv = sortbyc(idx ~ vpiv[0][]', 1)[][0];

	// check if any constraints can be removed
	csel = int(sumr(diagonal(mr .!= 0)));
	if (csel < cact)
	{
		if (csel == 0)
			mr = vpiv = <>;
		else
		{	mr = mr[ : csel - 1][ : csel - 1];
			vpiv = vpiv[ : csel - 1];
		}
		cact = csel;
	}
	mr |= zeros(n - cact, cact);
	if (csel == 0)
		mzq = mZ;
	else if (mZ == <>)
		mzq = decqrmul(mh[ : csel - 1][])';
	else
		mzq = decqrmul(mh[ : csel - 1][], mZ' )';
	
	return {mzq, mr, isActive[vpiv], vpiv};
}

static SolveQP_nsolve(const mZ, const mR, const cAct, const mG, const vG, const vB)
{
	decl n = sizer(mZ), vx, p1, p2, gp, z2, mzgz, vlam, ddiag = 0;
	
	// solve: R1'p1 = -c for p1 and add Q1*p1 to x
	if (cAct)
	{
		p1 = solvelu(mR[ : cAct - 1][]', 0, 0, vB);
		p1 = mZ[][ : cAct - 1] * p1;
		vx = p1';
	}
	else
	{	mzgz = SolveQP_chol(mG, &ddiag);
		if (sizec(mG) == 1)
			vx = -vG ./ sqr(mzgz);
		else
		{	p2 = solvelu(mzgz, 0, 0, vG);
			vx = -solvelu(0, mzgz', 0, p2);
		}
		vx = vx';
	}
	
	if (cAct && cAct < n)
	{
		z2 = mZ[][cAct : ]';
		// solve Q2'GQ2 = -Q2'(GQ1p1+h), h = Gx+q, Q1p1+x=new x
		if (sizec(mG) == 1)
		{	mzgz = z2 * (mG .* z2');
			gp = vx .* mG' + vG';
		}
		else
		{	mzgz = outer(z2, mG);  // directed Hessian Z'GZ
			gp = vx * mG' + vG';
		}
		gp *= z2';
		mzgz = SolveQP_chol(mzgz, &ddiag);
		
		p2 = solvelu(mzgz, 0, 0, gp');
		p2 = solvelu(0, mzgz', 0, p2);
		vx -= p2' * z2;
	}

	if (cAct)
	{
		gp = sizec(mG) == 1 ? vx .* mG' : vx * mG';
		gp += (1 + ddiag) * vG';
		vlam = solvelu(0, mR[ : cAct - 1][], 0, mZ[][ : cAct - 1]'gp') ';
	}
	else
		vlam = <>;

	return {vx, vlam};
}

SolveQP_run(const mG, const vG, const mA, const vB, const mC, const vD, const vLo, const vHi,
	const vDelA, vX, isActive, dTol, const iMaxit, const fGisChol)
{	// solves min(x'Gx/2 + g'x) s.t. Ax>=b, Cx=d, L<=x<=H
	// if A is the set of active constraints and l the lagrange multipliers
	// with a = #constraints:
	//  KKT system:		[G  -A'] [x^]  =  [-g]
	//  				[A   0 ] [l^]	  [ b]
	//  rewriting x* = x + p:
	//  				[G   A'] [-p]  =  [g+Gx == h]		Gp-A'l^=-h	(1)
	//  				[A   0 ] [l^]	  [Ax-b == c]		Ap     =-c	(2)
	//  (modified) Choleski decomposition of G: G=PP' --> inv(G)=VV'
	//  inv(G)A' = VV'A' = VX = VQ'QX with Q orthogonal and QX upper triangular
	//			 = ZR    which will be updated/downdated
	//			 = Z1R1	 R'=(R1':R2') with R2=0 and R1 is (a x a) upper triangular
	//  so ZZ' = VQQ'V' = inv(G) and Z'GZ=I
	//  updating:
	//  	V_0 V_0'A_0' --> V_1 R_1 = V_0 Q_0 Q_0' V_0'A_0'
	//  	V_1(R_1:V_1'A_1) --> V_2 Q_1 Q_1'R_2
	//		 = V_0 Q_0 Q_1 Q_1'(Q_0'V_0'A_0':Q_0'V_0'A_1') = V_0 Q_0 Q_1 Q_1'Q_0'V_0'A'
	//  so ZR=ZZ'A' and R'=AZ=(R1':R2')=(L:0)
	//  now Z=Z1:Z2 and write p as Z1 p1+Z2 p2, subsituting in (2):
	//  (2') A Z1 p1 + 0 = -c  -->   (4)  R1'p1 = -c  solve for p1
	//  (1') G Z1 p1 + G Z2 p2 - A'l^ = -h multiply by Z2':
	//		 0 + p2 - 0 = -Z2'h -->	 (5)  p2 = -Z2'h
	//  (3') multiply (1') by Z1':
	//		 p1+0- R1 l^= -Z1'h -->	 (6)  R1 l^ = Z1'h + p1 solve for l^
	// NB. in notation above all vectors are columns, in the code some are rows
	
	decl n = sizer(mG), me = sizer(mC), mi = sizer(mA);
	decl m = me + mi, mn = m + n, mnn = mn + n;
	decl itno, istep, kadd, kdel, cact, csel, ireset, isinfeasible;
	decl fbounds = sizer(vLo), fvecg = sizec(mG) == 1 && n > 1, infeas = 0;
	decl ddiag, dlamnew, dlaminc, dxstep, dratio, dsumy, dres;
	decl vscale, vs, vw, vlam, vp, vpcv, vmult, vb, ax;
	decl mr, mz;

	if (dTol < 0)
		dTol = 100 * inverteps(0);

	vlam = <>;						// [cact] lagrange multipliers
	mr = <>;						// [n][cact] triangularized active constraints
	mz = <>;						// [n][n]
	vX = vec(vX)';					// use as row vector
	vb = <>;
	if (me)		 vb ~= vD';
	if (mi)		 vb ~= vB';
	if (fbounds) vb ~= vLo' ~ -vHi';
	isinfeasible = zeros(1, sizec(vb));
	ddiag = 0;
	
	// construct constraint scale factors
	vscale = solveqp_getScale(mA, mC, vDelA, fbounds ? n : 0);

	for (istep = ireset = 0; ; ++istep)
	{
		// count active constraints
		cact = sizec(isActive);

		// set x, lambda to zero if requested
		if (ireset == 1)
		{
			vX = zeros(1, n);									   
			vlam = zeros(1, cact);
			istep = 0;
		}
		else if (ireset == 2)
		{
			istep = 0;
		}
		else if (ireset == 3)
		{
			mz = <>;
			vX[] = 0;
			istep = 0;
		}
		ireset = 0;
		
		// construct initial Z and R if not done yet
		if (sizerc(mz) == 0)
		{
			if (fGisChol)
				mz = solvelu(mG, 0, 0, unit(n))';	// Z=invert(P)'
			else
				mz = solveqp_cholinv(mG, &ddiag);	// G = PP'
			if (mz == 0)
				return { 3, <>, <> };

			if (cact)								// do QR of Z'A'
			{	[mz, mr, isActive] = SolveQP_decqr(mA, vB, mC, vD, vLo, vHi, mz, isActive);
				cact = sizec(isActive);
			}
			else
				mr = <>;
		}

		// solve the KKT system for current active set; drop inactive constraints
		// repeat if constraints have been dropped
		do
		{
			if (cact == 0)
			{	// x = -inv(G)g  (with correction for non pd-ness)
				vX = -(vG'mz) * mz' - ddiag * vG';
				break;
			}

			// get Ax and b for active constraints, as well as s=Zh
			ax = <>;
			if (me)		 ax ~= vX * mC';
			if (mi)		 ax ~= vX * mA';
			if (fbounds) ax ~= vX ~ -vX;

			if (fvecg)
				vs = vX .* mG' + ddiag * vX + vG';
			else if (fGisChol)
				vs = (vX * mG) * mG' + vG';
			else
				vs = vX * mG + ddiag * vX + vG';
			vs *= mz;

			// solve (4): R1'p1 = -c for p1 and add Z1*p1 to x
			vp = solvelu(mr[ : cact - 1][]', 0, 0, vb[isActive]' - ax[isActive]') ';
			vX += vp * mz[][ : cact - 1]';
			// use (5): vp = -Z2'h
			if (cact < n)
				vX -= vs[cact : n - 1] * mz[][cact : n - 1]';
			// solve (6): R1 l^ = Z1'h + p1 for lambda
			vlam = solvelu(0, mr[ : cact - 1][], 0, vs[ : cact - 1]' + vp') ';

			// remove inequality constraints with lambda < 0
			vs = vecindex(vlam .< 0 .&& isActive .>= me);
			csel = sizer(vs);
			if (csel)
			{
				mr = dropc(mr, vs);
				decqrupdate(&mz, &mr, csel);

				isActive = dropc(isActive, vs);	// fill gap in active set
				vlam = dropc(vlam, vs);			//  and lagrange multipliers
				cact -= csel;
			}
		} while (csel);

		// if x nearly on boundary, move it there
		if (istep && fbounds)
		{
			vX = fabs(vX - vLo') .< dTol .* fabs(vLo' .== 0 .? 1e-2 .: vLo') .? vLo' .: vX;
			vX = fabs(vX - vHi') .< dTol .* fabs(vHi' .== 0 .? 1e-2 .: vHi') .? vHi' .: vX;
		}

		// phase 2
		for (itno = 0; ; ++itno)
		{
			// find largest constraint violation (normalized, i.e.: .* vscale)
			ax = <>;
			if (me)		 ax ~= vX * mC';
			if (mi)		 ax ~= vX * mA';
			if (fbounds) ax ~= vX ~ -vX;
			vpcv = vp = vb - ax;
			if (me) vpcv[ : me - 1] = fabs(vpcv[ : me - 1]);
			// exclude already active constraints
			if (cact) vpcv[isActive] = 0;
			// scale (which could drop constraints
			vpcv .*= vscale;
			vs = vpcv .* (isinfeasible .<= 1);
			// find largest constraint violation
			kadd = maxcindex(vs');
			// check for convergence (or failure)

			if (kadd == -1 || kadd == <> || vs[kadd] <= dTol || itno >= iMaxit)
			{
				if (istep == 0 && fbounds)
				{
					vX = vX .< vLo' .? vLo' .: vX;
					vX = vX .> vHi' .? vHi' .: vX;
				}
				if (istep == 0) // do non-iterative step if not done before
					break;
				infeas = any(vpcv .> dTol);
				// map to return values
				vs = zeros(fbounds ? mnn : m, 1);
				if (cact) vs[isActive] = vlam;
				return {itno >= iMaxit ? 2 : infeas, vX', vs};
			}
			kadd = int(kadd);
	
			// add product of constraint normal and Z
			if (kadd >= mn)		  mr ~= -mz[kadd - mn][]';
			else if (kadd >= m)	  mr ~= mz[kadd - m][]';
		    else if (kadd >= me)  mr ~= (mA[kadd - me][] * mz)';
		    else				  mr ~= (mC[kadd][] * mz)';
			
			// add the constraint and re-triangularize column 
			decqrupdate(&mz, &mr, cact, cact);


			dlamnew = 0.0;
			dres = -vp[kadd];

			do
			{
				dsumy = cact < n ? mr[cact][cact] : 0;
				if (cact < n && dsumy == 0)
					break;
				dxstep = -dres / dsumy;
				dlaminc = dxstep / dsumy;
				
				kdel = -1;

				if (cact)
				{
					vmult = solvelu(0, mr[ : cact - 1][ : cact - 1], 0, mr[ : cact - 1][cact]) ';
					[kdel, dratio] = solveqp_getdel(vlam, vmult, dres, isActive, me, 0);

					if (cact == n)						// must delete a constraint
					{
						if (kdel < 0)					// failed to find one
							break;
						dxstep = 0;
						dlaminc = dratio;
					}
					else
					{	if (1 - dratio / dlaminc <= 0)
							kdel = -1;
							
						if (kdel >= 0)
						{
							dres *= 1 - dratio / dlaminc;
							dxstep = dratio * dsumy;
						    dlaminc = dratio;
						}
					}
					// update lambda
					vlam -= dlaminc * vmult;
					vlam = isActive .>= me .&& vlam .< 0 .? 0 .: vlam;
				}
				if (cact < n) 
					vX += dxstep * mz[][cact]';			// update x
			    dlamnew += dlaminc;

				if (kdel >= 0)
				{
					// drop column and re-triangularize the subdiagonal
					mr = dropc(mr, kdel);
					decqrupdate(&mz, &mr, 1);

					isActive = dropc(isActive, kdel);	// fill gap in active set   
					vlam = dropc(vlam, kdel);			//  and lagrange multipliers
					--cact;
				}
			} while (kdel >= 0);

			// check for incompatible constraints
			if (cact == n || (cact && fabs(mr[cact][cact]) <= DBL_EPSILON * norm(diagonal(mr))))
			{
				ireset = 2;
				isinfeasible[kadd] += 1;
				if (isinfeasible[kadd] > 1)
				{	
					if (infeas == 0)
						ireset = 1;
					infeas = 1;
				}
				mr = dropc(mr, cact);		// delete this offending one
				decqrupdate(&mz, &mr, 1);
				break;						// retry
			}
			
			// add new constraint
			vlam ~= dlamnew;
		    isActive ~= kadd;
		    ++cact;
		}
	}
}
/*--------------------------------------------------------------------------*/

/*--------------------------------------------------------------------------*/
SolveQP_bounds(const mA, const vB, vLo, vHi, const fTighten)
{
	decl m = rows(mA), n = columns(mA), m1, idx, del, i, j, k, d, fnew;
	decl vbhi, vblo;

	if (m == 0 || n == 0)
		return {vLo, vHi, <>};
		
	m1 = mA .!= 0;
	idx = vecindex(sumr(m1) .== 1);
	if (sizerc(idx))
	{	if (sizer(vLo) < n)
			vLo |= -.Inf * ones(n - sizer(vLo), 1);
		if (sizer(vHi) < n)					  	
			vHi |= +.Inf * ones(n - sizer(vHi), 1);
	}
	for (i = 0; i < sizerc(idx); ++i)
	{
		j = idx[i];	 k = vecindex(m1[j][])[0];
		d = mA[j][k];
		if (d < 0)
			vHi[k] = min(vHi[k], vB[j] / d);
		else
			vLo[k] = max(vLo[k], vB[j] / d);
	}
	// mark all restrictions that are bounds or void for deletion
	del = sumr(m1) .<= 1;

	// determine tightest bounds if requested
	if (fTighten && sizer(vLo) == n && sizer(vHi) == n && isdotinf(vLo) == 0 && isdotinf(vHi) == 0)
	{
		m1 = sumr(mA .!= 0);

		vbhi = (mA .< 0 .? mA .: 0) * vLo + (mA .> 0 .? mA .: 0) * vHi;
		decl vbhii, vxloi, vxhii, vai;

		for (i = 0; i < n; ++i)
		{
			vai = mA[][i];
			vbhii = vbhi - (vai .< 0 .? vai .: 0) .* vLo[i] - (vai .> 0 .? vai .: 0) .* vHi[i];

			vxloi = vai .> 0 .? (vB - vbhii) ./ vai .: -.Inf;
			vxhii = vai .< 0 .? (vB - vbhii) ./ vai .: +.Inf;

			fnew = FALSE;
			d = max(vxloi);
			if (d > vLo[i])
			{
				vLo[i] = d;
				fnew = TRUE;
			}
			d = min(vxhii);
			if (d < vHi[i])
			{
				vHi[i] = d;
				fnew = TRUE;
			}
			if (fnew)
				vbhi = (mA .< 0 .? mA .: 0) * vLo + (mA .> 0 .? mA .: 0) * vHi;
		}
		// delete redundant constraints
		vblo = (mA .< 0 .? mA .: 0) * vHi + (mA .> 0 .? mA .: 0) * vLo;
		del = del .&& (vblo .>= vB);
	}

	return {vLo, vHi, del};
}	
PreprocessLC(mA, vB, vLo, vHi, const fBoundsOnly)
{
	decl del;
	
	[vLo, vHi, del] = SolveQP_bounds(mA, vB, vLo, vHi, !fBoundsOnly);

	if (sizer(mA))
		return {mA, vB, vLo, vHi, del};
	else
		return { deleteifr(mA, del), deleteifr(vB, del), vLo, vHi, del };
}
SolveQP_init(const mG, const vG, const mA, const vB, const mC, const vD, const vLo, const vHi)
{
	decl n = sizer(mG), me = sizer(mC), mi = sizer(mA);
	decl vlo, vhi, fbounds, retval = n, smsg = "";

	if (me && sizec(mC) != n)
	{
		smsg = sprint("C must have n columns");
		retval = -1;
	}
	if (me && sizer(vD) != me)
	{
		smsg = sprint("d must have ", me, " columns");
		retval = -1;
	}
	if (mi && sizec(mA) != n)
	{
		smsg = sprint("A must have ", n, " columns");
		retval = -1;
	}
	if (mi && sizer(vB) != mi)
	{
		smsg = sprint("b must have ", mi, " rows");
		retval = -1;
	}
	if (mi && sizer(vB) != mi)
	{
		smsg = sprint("b must have ", mi, " rows");
		retval = -1;
	}
	if (sizer(vG) && sizer(vG) != n)
	{
		smsg = sprint("g must have ", n, " rows");
		retval = -1;
	}
	vlo = vec(vLo);  vhi = vec(vHi);
	fbounds = sizer(vlo) || sizer(vhi);
	if (fbounds)
	{
		if (sizer(vlo) < n)
			vlo |= -.Inf * ones(n - sizer(vlo), 1);
		if (sizer(vhi) < n)					  	
			vhi |= +.Inf * ones(n - sizer(vhi), 1);
	}
	return { retval, vlo, vhi, smsg };
}
SolveQPS(const sFile, const iVerbose, ...)
{
	decl name, mg, vg, g0, ma, vb, mc, vd, vlo, vhi, crit, time,
		st1, st2, result, vx, vlam, fnsolve = SolveQP;
	decl arg = va_arglist();

	if (sizeof(arg) > 0 && isfunction(arg[0]))
		fnsolve = arg[0];

	time = timer();
	[name, mg, vg, g0, ma, vb, mc, vd, vlo, vhi] = SolveQP_read(sFile);
	st1 = timespan(time);

	if (iVerbose == 2) 
		println("G=","%7.6g", mg, "g'=", "%6.5g", vg', "g0=", g0,
			" A=","%5.3g", ma, "b'=","%5.3g", vb', "C=","%5.3g", mc,
			"d'=","%5.3g", vd',	"bounds:", vlo ~ vhi);
	
	if (iVerbose) 
		print("%-10s", name, 
			" n=", "%4d", sizer(vg), " I=", "%4d", sizer(ma), " E=", "%4d", sizer(mc),
			" B=", "%4d", max(sizer(vlo),sizer(vhi)), sizec(mg) == 1 ? " sep" : "    ");

	time = timer();
	[result, vx, vlam] = fnsolve(mg, vg, ma, vb, mc, vd, vlo, vhi);
	st2 = timespan(time);

	if (sizec(mg) == 1)
		crit = double(vx'(mg.*vx)/2 + vx'vg + g0);
	else
		crit = double(vx'mg*vx/2 + vx'vg + g0);

	if (iVerbose) 
	{
		print(" ret=", result, "%#18.10g", crit, "  run-time:", "%8s", st2, "  read-time:", "%8s", st1);
		if (result == 1)
			print(" Infeasible constraints");
		else if (result == 2)
			print(" Iteration limit reached");
		else if (result == 3)
			print(" Choleski decomposition failed");

		decl tol = 1e-8;
		if ( (sizer(ma) && (ma*vx - vb) < -tol) ||
			 (sizer(mc) && any(fabs(mc * vx - vd) .> tol)) ||
			 (sizer(vlo) && any(vlo - vx .> tol)) ||
			 (sizer(vhi) && any(vx - vhi .> tol)) )
			print(" Some constraints are violated by ", tol, " or more!");
		print("\n");
	}
	if (iVerbose == 2) 
		println("x = ", "%#17.10g", vx, "lambda = ", "%#17.10g", vlam);

return {result, vx, vlam, crit};
}
SolveQP(const mG, const vG, const mA, const vB, const mC, const vD, const vLo, const vHi)
{
	decl n = sizer(mG), me = sizer(mC), mi = sizer(mA);
	decl maxit = 100 * (me + mi + n) * n, iret;
	decl vlo, vhi, aret, smsg, vdela = <>, isactive;

	[iret, vlo, vhi, smsg] = SolveQP_init(mG, vG, mA, vB, mC, vD, vLo, vHi);
	if (iret <= 0)
	{
		oxrunerror(smsg, 0);
		return { -1, <>, <> };
	}
	[vlo, vhi, vdela] = SolveQP_bounds(mA, vB, vlo, vhi, TRUE);

	// starting with all equalities active tends to be faster
	isactive = range(0, me - 1, 1);
	if (sizer(vlo) && sizer(vhi) && any (vlo .== vhi))
		isactive ~= me + mi + vecindex(vlo .== vhi)';

	aret = SolveQP_run(mG, vG, mA, vB, mC, vD, vlo, vhi,
		vdela, zeros(n, 1), isactive, 1e-10, maxit, FALSE);

	if (me && aret[0] == 1)
	{
		aret = SolveQP_run(mG, vG, mA, vB, mC, vD, vlo, vhi,
			vdela, zeros(n, 1), <>, 1e-8, maxit, FALSE);
	}
//	if (aret[0] == 1)
//		println("SolveQP: infeasible constraints");
//	else if (aret[0] == 2)
//		println("SolveQP: no convergence in ", maxit, " iterations");
//	else if (aret[0] == 3)
//		println("SolveQP: Choleski decomposition failed");
		
	return aret;
}
SolveQPE(const mG, const vG, const mC, const vD)
{
	decl me = sizer(mC);
	decl isactive, vx, vlam, mz, mr;

	[mz, mr, isactive] = SolveQP_decqr(<>, <>, mC, vD, <>, <>, <>, -1);
	[vx, vlam] = SolveQP_nsolve(mz, mr, sizec(isactive), mG, vG, vD[isactive]);
	return {vx', vlam', exclusion(range(0, me - 1, 1), isactive) };
}
SolveQPIF(vX, const mG, const vG, const mA, const vB,
	const vLo, const vHi, eps, tol, maxit)
{
	if (sizerc(vLo) || sizerc(vHi))
	{
		decl vlo, vhi, vdela;

		[vlo, vhi, vdela] = SolveQP_bounds(mA, vB, vLo, vHi, TRUE);
	
		return SolveQP_run(mG, vG, mA, vB, <>, <>, vlo, vhi,
			vdela, vX, <>, tol, maxit, FALSE);
	}
	return SolveQP_run(mG, vG, mA, vB, <>, <>, <>, <>,
		<>, vX, <>, tol, maxit, FALSE);
}
/*--------------------------------------------------------------------------*/
