//////////////////////////////////////////////////////////////////////////////////
//
//		title		:	Optimization with Constraints
//						
//		version		:	v0.8
//		author		:	Jinwook Kim (jwkim at imrc dot kist dot re dot kr)
//		last update	:	99.8.1
//		
//		Note		:	compatible with RMatrix v2.0 or later
//
//////////////////////////////////////////////////////////////////////////////////

#include "rmatrix.h"
#include "optim.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <fstream>

#define _EPS 2.2204E-16
#define _INF 1E100

#define max(a, b)  (((a) > (b)) ? (a) : (b))
#define min(a, b)  (((a) < (b)) ? (a) : (b))

int log_flag = 0;
char fname[1024];

void log2file(char *file_name)
{
	if ( !strlen(file_name) ) log_flag = 0;
	else
	{
		remove(file_name);
		log_flag = 1;
		strcpy(fname, file_name);
	}
}

void opt_out(char *msg)
{
	cout << msg << endl;
	
	if ( log_flag )
	{
		ofstream fout(fname, ios::app);
		fout << msg << endl;
		fout.close();
	}
}

void planerot(RMatrix &G, RMatrix &x)
// function [G,x] = planerot(x)
// PLANEROT Given's plane rotation.
//   [G,Y] = PLANEROT(X), where X is a 2-component column vector,
//   returns a 2-by-2 orthogonal matrix G so that Y = G*X has Y(2) = 0.
{
	if ( x(2) != 0 )
	{
		real r = Norm(x);
		G = RMatrix(2,2, x(1), -x(2), x(2), x(1))/r;
		x(1) = r; 
		x(2) = 0.0;
	} else G = Eye(2);
}

void qrinsert(RMatrix &Q, RMatrix &R, int j, const RMatrix &x)
// function [Q,R] = qrinsert(Q,R,j,x)
// QRINSERT Insert column in QR factorization.
//   If [Q,R] = QR(A) is the original QR factorization of A, then [Q,R] = QRINSERT(Q,R,j,x) changes Q and R to be the
//   factorization of the matrix obtained by inserting an extra column, x, before A(:,j).  (If A has n columns and j = n+1,
//   then x is inserted after the last column of A.) 
{
	if ( R.ColSize() == 0 )
	{
		QR(x, Q, R);
		return;
	}

	RMatrix m = Q*R;
	m.ReSize(m.RowSize(),m.ColSize()+1);
	for ( int i = m.ColSize(); i > j; i-- )	m.ColSwap(i,i-1);
	m.SetCol(j,x);
	QR(m,Q,R);	
}

void qrdelete(RMatrix &Q, RMatrix &R, int j)
//function [Q,R] = qrdelete(Q,R,j)
//QRDELETE Delete column from QR factorization. If [Q,R] = QR(A) is the original QR factorization of A,
//   then [Q,R] = QRDELETE(Q,R,j) changes Q and R to be the factorization of the matrix with A(:,j) removed. 
{
	RMatrix m = Q*R;
	m.RemoveCol(j);
	QR(m,Q,R);
}

void compdir(RMatrix &SD, char *dirType, const RMatrix &Z, const RMatrix &H, const RMatrix &gf,int nvars, const RMatrix &f)
// function [SD, dirType] = compdir(Z,H,gf,nvars,f);
//    COMPDIR Computes a search direction in a subspace defined by Z. Helper function for NLCONST.
//    Returns Newton direction if possible. Returns random direction if gradient is small.
//    Otherwise, returns steepest descent direction.  If the steepest descent direction is small it computes a negative
//    curvature direction based on the most negative eigenvalue. For singular matrices, returns steepest descent even if small.
{
	//  SD=-Z*((Z'*H*Z)\(Z'*gf));
	dirType[0] = '\0';
	// Compute the projected Newton direction if possible
	RMatrix projH = ~Z*H*Z, R, stpDesc, smallRealEig, Zev, DD, VV, ev;
	real SDtol;
	int p, eigind;				

	Chol(projH, R, p);

	if ( !p )  // positive definite: use Newton direction
	{
		SD = -Z*(R % (~R % (~Z*gf)));
		strcpy(dirType, "Newton");
	} else // not positive definite
	{
	//	If the gradient is small, try a random direction: Sometimes the search direction goes to zero in negative
	//	definite problems when the current point rests on the top of the quadratic function. In this case we can move in
	//	any direction to get an improvement in the function so foil search direction by giving a random gradient.
		if ( Norm(gf) < sqrt(_EPS) )
		{
			SD = -Z*~Z*(Rand(nvars,1) - 0.5);
			strcpy(dirType, "Random");
		} else
		{
			// steepest descent
			stpDesc = - Z*(~Z*gf);
			// check if ||SD|| is close to zero 
			if ( Norm(stpDesc) > sqrt(_EPS) )
			{
				SD = stpDesc;
				strcpy(dirType, "steepest descent");
			} else
			{
	        	// Look for a negative curvature direction
				//options.disp = 0;
				//[ev, smallRealEig, flag] = eigs(projH,1,'sr',options);
				//if flag  // Call to eigs failed
				Eig(projH, VV, DD);
				smallRealEig = MinVec(DD, &eigind);
				ev = VV.GetRow(eigind);
	       
				if ( IsTrue(smallRealEig < 0) )
				{
					// check the sign of SD and the magnitude.
					SDtol = 100*_EPS*Norm(gf); // Note: we know norm(gf) > sqrt(eps)
					Zev = Z*ev;
					if ( IsTrue(~Zev*gf > SDtol) )
					{
						SD = -Zev;
						strcpy(dirType, "eigenvector");
					} else if ( IsTrue(~Zev*gf < SDtol) )
					{
						SD = Zev;
						strcpy(dirType, "eigenvector");
					} else 
					{ 
			            SD = stpDesc;
			            strcpy(dirType, "steepest descent");
					}
				} else // The projected Hessian is singular,i.e., zero direction is ok
				{
					//  -- will propagate thru the algorithm.
					SD = stpDesc;
					strcpy(dirType, "steepest descent");
				} // smallRealEig < 0
			} // randSD'*(gf) < -SDtol
		} //  norm(stpDesc) > sqrt(eps)
	} // ~p
}   

void eqnsolv(RMatrix &Q, RMatrix &R, RMatrix &A, RMatrix &B, int &CIND, RMatrix &X, RMatrix &Z,
RMatrix &actlambda, char *how, RMatrix &ACTSET, RMatrix &ACTIND, int &ACTCNT, RMatrix &aix, RMatrix &eqix, 
int neqcstr, int ncstr, RMatrix &remove, int nvars, int LLS, const RMatrix &H, RMatrix &f, real normf, const RMatrix &normA, int verbosity)
// function[Q,R,A,B,CIND,X,Z,actlambda,how,ACTSET,ACTIND,ACTCNT,aix,eqix,neqcstr,ncstr,remove] =
//    eqnsolv(A,B,eqix,neqcstr,ncstr,nvars,LLS,H,X,f,normf,normA,verbosity,aix,how)
//    EQNSOLV Helper function for QPSUB.
//    Finds a feasible point with respect to the equality constraints.
//    If the equalities are dependent but not consistent, warning
//    messages are given. If the equalities are dependent but consistent, 
//    the redundant constraints are removed and the corresponding variables 
//    adjusted.
{
	RMatrix Anew, Bnew, depInd, bdepInd, Qa, Ra, Ea, i, j, Qat, Rat, Eat;
	int numDepend;
	// set tolerances
	real tolDep = 100*nvars*_EPS, tolCons = 1E-10;

	actlambda = RMatrix();
	aix.Replace(eqix, Ones(neqcstr,1));
	ACTSET = A.GetRow(eqix);
	ACTIND = eqix;
	ACTCNT = neqcstr;
	CIND = neqcstr+1;
	Z = RMatrix();
	remove = RMatrix();
	
	// See if the equalities form a consistent system:
	//   QR factorization of A
	QR(A.GetRow(eqix), Qa, Ra, Ea);
	// Now need to check which is dependent
	if ( IsVector(Ra) ) // Make sure Ra isn't a vector
		depInd = (fabs(Ra(1)) < tolDep) ? RMatrix(1.0): RMatrix();
	else
		depInd = Find( Abs(Diag(Ra)) < tolDep );

	if ( neqcstr > nvars )
	   depInd = RMatrix(2,1, depInd, ColIndex(nvars+1,neqcstr));

	if ( !IsEmpty(depInd) )
	{
		if ( verbosity > -1 ) opt_out("The equality constraints are dependent.");
		strcpy(how,"dependent");
		bdepInd = (Abs( ~(Qa.GetCol(depInd)) * B(eqix) ) >= tolDep) ;
		if ( IsTrue(Any( bdepInd )) ) // Not consistent
		{
			strcpy(how, "infeasible");
			if ( verbosity > -1 )
			{
				opt_out("The system of equality constraints is not consistent.");
				if ( ncstr > neqcstr ) opt_out("The inequality constraints may or may not be satisfied.");
				opt_out("  There is no feasible solution.");
			}
		} else // the equality constraints are consistent
		{
			numDepend = NNZ(depInd);
			// delete the redundant constraints:
			// By QR factoring the transpose, we see which columns of A' (rows of A) move to the end
			QR(~ACTSET, Qat, Rat, Eat);
			Find(Eat, i, j);
			remove = i(depInd);
			if ( verbosity > -1 )
			{
				opt_out("The system of equality constraints is consistent. Removing");
				opt_out("the following dependent constraints before continuing:");
				char tmp_str[1024]; 
				for ( int tmpi = 1; tmpi <= remove.RowSize()*remove.ColSize(); tmpi++ ) sprintf(tmp_str, "%d ", remove(tmpi) );
				opt_out(tmp_str);
			}
			A.RemoveRow(eqix(remove));
			B.Remove(eqix(remove));
			neqcstr -= NNZ(remove);
			ncstr -= NNZ(remove);
			eqix = RowIndex(1,neqcstr);
			aix = RMatrix(2,1, Ones(neqcstr,1), Zeros(ncstr-neqcstr,1));
			ACTIND = eqix;
			ACTSET = A.GetRow(eqix);
			CIND = neqcstr+1;
			ACTCNT = neqcstr;
		} // consistency check
	} // dependency check

	if ( strcmp(how,"infeasible") ) // Find a feasible point
		if ( MaxVec( Abs( A.GetRow(eqix)*X-B(eqix) ) ) > tolCons ) X = A.GetRow(eqix) % B(eqix);

	QR(~ACTSET,Q,R);
	Z = Q.GetCol(neqcstr+1,nvars);
// End of eqnsolv.m
}

void qpsub( RMatrix &X, RMatrix &lambda, char *how, const RMatrix &H, RMatrix f,
			RMatrix A, RMatrix B, RMatrix vlb, RMatrix vub,	const RMatrix &Xi, int neqcstr, int verbosity, 
			char *caller, int ncstr, int nvars )
// function [X,lambda,how]=qpsub(H,   f, A,  B, vlb,vub,X,neqcstr,    verbosity,caller, ncstr, nvars)
// QP Quadratic programming subproblem. Handles qp and constrained
//    linear least-squares as well as subproblems generated from NLCONST.
//
//    X=QP(H,f,A,b) solves the quadratic programming problem:
//
//             min 0.5*x'Hx + f'x   subject to:  Ax <= b 
//              x    
{
	X = Xi;
	// Define constant strings
	// if isempty(verbosity), verbosity = 0; end
	// if isempty(neqcstr), neqcstr = 0; end

	int LLS, simplex_iter, rowH, colH, normalize, is_qp, i, mm, lenvlb, lenvub, ACTCNT, CIND, m, cindmax, cindcnt, newactcnt, delete_constr, nn, int_lind, ind, ind2, eigind;
	real normf, n, errcstr, sqrt_eps, errnorm, tolDep, minl, gradsd, err, mc, STEPMIN, smallRealEig;
	RMatrix ACTSET, ACTIND, cstr, A2, XS, lambdas, gf, SD, HXf, HZ, RHZ, QHZ, Pd, rlambda, depInd, dist;
	RMatrix ZHZ, VV, DD, ev, projH, Zgf, projSD, indlam, lind, eqix, aix, GSD, indf, Q, R, indepInd, Z, actlambda, remove;
	char dirType[1024], tmphow[1024];

	LLS = 0;
	if ( !strcmp(caller, "conls") )
	{
		LLS = 1;
		Size(H, &rowH, &colH);
		nvars = colH;
	}
	if ( !strcmp(caller, "qpsub") ) normalize = -1;
	else normalize = 1;

	simplex_iter = 0;
	if ( InftyNorm(H) == 0.0 || IsEmpty(H) ) is_qp = 0; 
	else is_qp = 1;
	strcpy(how, "ok");

	if ( LLS == 1 ) is_qp = 0;

	normf = 1;
	if ( normalize > 0 ) // Check for lp
		if ( !is_qp && !LLS )
		{
			normf = Norm(f);
			if ( normf > 0 ) f /= normf;
		}

	// Handle bounds as linear constraints
	lenvlb = Length(vlb);
	if ( lenvlb > 0 )
	{
		A = RMatrix(2,1, A, -Eye(lenvlb,nvars));
		B = RMatrix(2,1, B, -vlb.MakeColVector());		
	}
	lenvub = Length(vub);
	if ( lenvub > 0 )
	{
		A = RMatrix(2,1, A, Eye(lenvub,nvars));
		B = RMatrix(2,1, B, vub.MakeColVector());
	}
	ncstr += (lenvlb + lenvub);
	errcstr = 100.0 * sqrt(_EPS) * Norm(A); 
	// Used for determining threshold for whether a direction will violate a constraint.
	RMatrix normA = Ones(ncstr,1);
	if ( normalize > 0 )
	{
		for ( i = 1; i <= ncstr; i++ )
		{
			n = Norm(A.GetRow(i));
			if ( n != 0.0 )
			{
				A.SetRow(i, A.GetRow(i)/n);
				B(i) /= n;
				normA(i) = n;
			}
		}
	}else  normA = Ones(ncstr,1);

	sqrt_eps = sqrt(_EPS);
	errnorm = 0.01*sqrt_eps;
	
	tolDep = 100.0*nvars*_EPS;
	lambda = Zeros(ncstr,1);
	aix = lambda;
	ACTCNT = 0;
	CIND = 1;
	eqix = RowIndex(1,neqcstr);

	//------------EQUALITY CONSTRAINTS---------------------------
	Q = Zeros(nvars,nvars);
	indepInd = RowIndex(1,ncstr);

	if ( neqcstr > 0 )
	{
		// call equality constraint solver
		eqnsolv(Q, R, A, B, CIND, X, Z, actlambda, how, ACTSET, ACTIND, ACTCNT, aix, eqix, 
		neqcstr, ncstr, remove, nvars, LLS, H, f, normf, normA, verbosity);
	   			
		if ( !IsEmpty(remove) )
		{
			indepInd.Remove(remove);
			normA = normA(indepInd);
		}

		if ( ACTCNT >= nvars - 1 ) simplex_iter = 1; 
		m = RowSize(ACTSET); n = ColSize(ACTSET);
		
		// Equalities are inconsistent, so X and lambda have no valid values
		// Return original X and zeros for lambda.
		if ( !strcmp(how, "infeasible") ) return;
		  
		err = 0.0;
		if ( neqcstr > nvars )
		{
			err = MaxVec(Abs(A.GetRow(eqix)*X-B(eqix)));
			if ( err > 1E-8 )  // Equalities not met
			{
				strcpy(how, "infeasible");
				if ( verbosity > -1 )
				{
					opt_out("Warning: The equality constraints are overly stringent;");
					opt_out("         there is no feasible solution.");
				}
				// Equalities are inconsistent, X and lambda have no valid values. Return original X and zeros for lambda.
				return;
			} else // Check inequalities
			{
				if ( IsTrue(Max(A*X-B) > 1E-8) )
				{
					strcpy(how, "infeasible");
					if ( verbosity > -1 )
					{
						opt_out("Warning: The constraints or bounds are overly stringent;");
						opt_out("         there is no feasible solution.");
						opt_out("         Equality constraints have been met.");
					}
				}
			}
			
			if ( is_qp ) actlambda = -R % (~Q*(H*X+f));
			else if ( LLS ) actlambda = -R % (~Q*(~H*(H*X-f)));
			else actlambda = -R % (~Q*f);

			lambda.Push(indepInd(eqix), normf * ElDivid(actlambda, normA(eqix)));
			return;
		}

		if ( IsEmpty(Z) )
		{
			if ( is_qp ) actlambda = -R % (~Q*(H*X+f));
			else if ( LLS ) actlambda = -R % (~Q*(~H*(H*X-f)));
			else actlambda = -R % (~Q*f);
			lambda.Push(indepInd(eqix), normf * ElDivid(actlambda,normA(eqix)));
			if ( IsTrue(Max(A*X-B) > 1E-8) )
			{
				strcpy(how, "infeasible");
				if ( verbosity > -1 )
				{
					opt_out("Warning: The constraints or bounds are overly stringent;");
					opt_out("         there is no feasible solution.");
					opt_out("         Equality constraints have been met.");
				}
			}
			return;
		}
		
		// Check whether in Phase 1 of feasibility point finding. 
		if ( verbosity == -2 )
		{
			cstr = A*X-B; 
			mc = MaxVec(cstr(RowIndex(neqcstr+1,ncstr)));
			if ( mc > 0 ) X(nvars) = mc + 1;
		}
	} else Z = 1.0;

	// Find Initial Feasible Solution
	cstr = A*X-B;
	mc = MaxVec(cstr(RowIndex(neqcstr+1,ncstr)));

	if ( mc > _EPS )
	{
		A2 = RMatrix(1,2, RMatrix(2,1,A,Zeros(1,nvars)), RMatrix(2,1, Zeros(neqcstr,1), -Ones(ncstr+1-neqcstr,1)));
		RMatrix nul1, nul2;

		qpsub(XS, lambdas, tmphow, RMatrix(), RMatrix(2,1,Zeros(nvars,1),RMatrix(1)), A2, RMatrix(2,1,B,RMatrix(1E-5)), nul1, nul2,
		RMatrix(2,1,X,RMatrix(mc+1)), neqcstr,-2, "qpsub" , A2.RowSize(), nvars+1);

		X = XS(ColIndex(1,nvars));

		cstr = A*X-B;
		if ( XS(nvars+1) > _EPS )
		{
			if ( XS(nvars+1) > 1E-8 )
			{
				strcpy(how, "infeasible");
				if ( verbosity > -1 )
				{
					opt_out("Warning: The constraints are overly stringent;");
					opt_out("         there is no feasible solution.");
				}
			} else strcpy(how, "overly constrained");
			lambda.Push(indepInd, normf * ElDivid(lambdas(ColIndex(1,ncstr)),normA));
			return;
		}
	}

	if ( is_qp )
	{
		gf = H*X + f;
		compdir(SD, dirType, Z,H,gf,nvars,f);
		// Check for -ve definite problems: if SD'*gf>0, is_qp = 0; SD=-SD; end
	} else if ( LLS )
	{
		HXf = H * X - f;
		gf = ~H * HXf;
		HZ = H * Z;
		Size(HZ, &mm, &nn);
		if ( mm >= nn )
		{
			//   SD =-Z*((HZ'*HZ)\(Z'*gf));
			QR(HZ, QHZ, RHZ);
			Pd = ~QHZ * HXf;
			// Now need to check which is dependent
			if ( IsVector(RHZ) ) // Make sure RHZ isn't a vector
				depInd = (fabs(RHZ(1)) < tolDep) ? RMatrix(1.0) : RMatrix();
			else
				depInd = Find( Abs(Diag(RHZ)) < tolDep );
		}
		if ( mm >= nn && IsEmpty(depInd) ) // Newton step
		{
			SD = - Z*(RHZ.Sub(1,nn, 1,nn) % Pd.GetRow(1,nn) );
			strcpy(dirType, "Newton");
		} else // steepest descent direction
		{
			SD = -Z*(~Z*gf);
			strcpy(dirType, "steepest descent");
		}
	} else // lp
	{
		gf = f;
		SD = -Z*~Z*gf;
		strcpy(dirType, "steepest descent");
		if ( Norm(SD) < 1E-10 && neqcstr )
		{
		  // This happens when equality constraint is perpendicular to objective function f.x.
			actlambda = -R % (~Q*gf);
			lambda.Push(indepInd(eqix), normf * ElDivid(actlambda , normA(eqix)));
			return;
		}
	}
	int oldind = 0; 
	// The maximum number of iterations for a simplex type method is:
	// maxiters = prod(1:ncstr)/(prod(1:nvars)*prod(1:max(1,ncstr-nvars)));

	//--------------Main Routine-------------------
	while ( 1 )
	{
		// Find distance we can move in search direction SD before a  constraint is violated.
		// Gradient with respect to search direction.
		GSD = A*SD;

		// Note: we consider only constraints whose gradients are greater than some threshold. If we considered all gradients greater than 
		// zero then it might be possible to add a constraint which would lead to a singular (rank deficient) working set. The gradient (GSD) of such
		// a constraint in the direction of search would be very close to zero.
		indf = Find( (GSD > errnorm * Norm(SD)) &&  !aix);

		if ( IsEmpty(indf) ) // No constraints to hit
		{
			STEPMIN = 1E16;
			dist = RMatrix();
		} else // Find distance to the nearest constraint
		{
			dist = Abs(ElDivid(cstr(indf),GSD(indf)));
			STEPMIN = MinVec(dist, &ind2);
			// Bland's rule for anti-cycling: if there is more than one blocking constraint then add the one with the smallest index.
			ind = int(indf(ind2));
			// Non-cycling rule:
			// ind = indf(ind2(1));
		}
	   // -----Update X-------------

	   // Assume we do not delete a constraint
	   delete_constr = 0;   

	   if ( !IsEmpty(indf) ) // Hit a constraint
	   {
			if ( !strcmp(dirType, "Newton") )
			{
				// Newton step and hit a constraint: LLS or is_qp
				if ( STEPMIN > 1 ) // Overstepped minimum; reset STEPMIN
				{
					STEPMIN = 1.0;
					delete_constr = 1;
				}
				X += STEPMIN*SD;
			} else X += STEPMIN*SD; // Not a Newton step and hit a constraint: is_qp or LLS or maybe lp
		} else // isempty(indf) | ~isfinite(STEPMIN)
		{
		   	// did not hit a constraint
			if ( !strcmp(dirType, "Newton") )
			{
				// Newton step and no constraint hit: LLS or maybe is_qp
				STEPMIN = 1.0;   // Exact distance to the solution. Now delete constr.
				X += SD;
				delete_constr = 1;
			} else // Not a Newton step: is_qp or lp or LLS
			{
				if ( is_qp )
				{
					// Is it semi-def, neg-def or indef?
					Eig(ZHZ, VV, DD );
					smallRealEig = MinVec(DD, &eigind);
					ev = VV.GetRow(eigind);
				} else smallRealEig = 0.0; // define smallRealEig for LLS
					 
				if ( (!is_qp && !LLS) || smallRealEig < -_EPS ) // LP or neg def: not LLS
				{
					// neg def -- unbounded
					if ( Norm(SD) > errnorm )
					{
						STEPMIN = ( normalize < 0 ? fabs((X(nvars)+1E-5)/(SD(nvars)+_EPS)) : 1E16 );
						X += STEPMIN*SD;
						strcpy(how, "unbounded");
					} else strcpy(how, "ill posed"); // norm(SD) <= errnorm
					if ( verbosity > -1 )
					{
						if ( Norm(SD) > errnorm )
						{
							opt_out("Warning: The solution is unbounded and at infinity;");
							opt_out("         the constraints are not restrictive enough.");
						} else
						{
							opt_out("Warning: The search direction is close to zero; ");
							opt_out("      the problem is ill-posed.");
							opt_out("      The gradient of the objective function may be zero");
							opt_out("         or the problem may be badly conditioned.");
						}
					} // if verbosity > -1
					return;
				} else // singular: solve compatible system for a solution: is_qp or LLS
				{
					if ( is_qp )
					{
						projH = ~Z*H*Z; 
						Zgf = ~Z*gf;
						projSD = pInv(projH)*(-Zgf);
					} else // LLS
					{
						projH = ~HZ*HZ; 
						Zgf = ~Z*gf;
						projSD = pInv(projH)*(-Zgf);
					}
					
					// Check if compatible
					if ( Norm(projH*projSD+Zgf) > 10.0*_EPS*(Norm(projH) + Norm(Zgf)) )
					{
						// system is incompatible --> it's a "chute": use SD from compdir
						// unbounded in SD direction
						if ( Norm(SD) > errnorm )
						{
							STEPMIN = ( normalize < 0 ? fabs((X(nvars)+1E-5)/(SD(nvars)+_EPS)) : 1E16 );
							X += STEPMIN*SD;
							strcpy(how, "unbounded");
						} else strcpy(how, "ill posed"); // norm(SD) <= errnorm
						
						if ( verbosity > -1 )
						{
							if ( Norm(SD) > errnorm )
							{
								 opt_out("Warning: The solution is unbounded and at infinity;");
								 opt_out("         the constraints are not restrictive enough.");
							} else
							{
								 opt_out("Warning: The search direction is close to zero; ");
								 opt_out("         the problem is ill-posed.");
								 opt_out("         The gradient of the objective function may be zero");
								 opt_out("         or the problem may be badly conditioned.");
							}
						} // if verbosity > -1
						return;
					} else // Convex -- move to the minimum (compatible system)
					{
						SD = Z * projSD;
						strcpy(dirType, "singular");
						// First check if constraint is violated.
						GSD = A*SD;
						indf = Find((GSD > errnorm * Norm(SD)) && !aix );
						if ( IsEmpty(indf) ) // No constraints to hit
						{
							STEPMIN = 1.0;
							delete_constr = 1;
							dist = RMatrix();
						} else // Find distance to the nearest constraint
						{
							dist = Abs(ElDivid(cstr(indf),GSD(indf)));
							STEPMIN = MinVec(dist, &ind2);
							// Bland's rule for anti-cycling: if there is more than one blocking constraint then add the one with the smallest index.
							ind = int(indf(ind2));
						}
						if ( STEPMIN > 1 ) // Overstepped minimum; reset STEPMIN
						{
							STEPMIN = 1.0;
							delete_constr = 1;
						}
						X += STEPMIN*SD; 
					}
				} // if ~is_qp | smallRealEig < -eps
			} // if strcmp(dirType, NewtonStep)
		} // if ~isempty(indf)& isfinite(STEPMIN) // Hit a constraint
	
		if ( delete_constr )
		{
			// Note: only reach here if a minimum in the current subspace found
			if ( ACTCNT > 0 )
			{
				if ( ACTCNT >= nvars-1 ) // Avoid case when CIND is greater than ACTCNT
				{
					if ( CIND <= ACTCNT )
					{
						ACTSET.RemoveRow(CIND);
						ACTIND.Remove(CIND);
					}
				}
				if ( is_qp ) rlambda = -R % (~Q*(H*X+f));
				else if ( LLS ) rlambda = -R % (~Q*(~H*(H*X-f)));
				// else: lp does not reach this point

				actlambda = rlambda;
				actlambda(eqix) = Abs(rlambda(eqix));
				indlam = Find(actlambda < 0);
				if ( !Length(indlam) )
				{
					lambda.Push((indepInd(ACTIND)), normf * ElDivid(rlambda,normA(ACTIND)));
					return;
				}
				// Remove constraint
				lind = Find(ACTIND == Min(ACTIND(indlam)));
				int_lind = int(lind(1)); lind = int_lind;
				ACTSET.RemoveRow(int_lind);
				aix(int(ACTIND(int_lind))) = 0.0;
				qrdelete(Q,R,int_lind);
				ACTIND.Remove(int_lind);
				ACTCNT -= 2;
				simplex_iter = 0;
				ind = 0;
			} else return; // ACTCNT == 0
			delete_constr = 0;
		}
	   
		// Calculate gradient w.r.t objective at this point
		if ( is_qp ) gf = H * X + f;
		else if ( LLS ) gf = ~H*(H*X-f); // LLS
		// else gf=f still true.

		// Update X and calculate constraints
		cstr = A*X - B;
		cstr.Push(eqix, Abs(cstr(eqix)));
		// Check no constraint is violated
		if ( normalize < 0 )
			if ( X(nvars,1) < _EPS ) return;

		if ( MaxVec(cstr) > 1E5 * errnorm )
		{
			if ( MaxVec(cstr) > Norm(X) * errnorm )
			{
				if ( verbosity > -1 )
				{
					opt_out("Warning: The problem is badly conditioned;");
					opt_out("         the solution is not reliable");
					verbosity = -1;
				}
				strcpy(how, "unreliable"); 
				if ( 0 )
				{
					X = X - STEPMIN * SD;
					return;
				}
			}
		}

		if ( ind ) // Hit a constraint
		{
			aix(ind) = 1;
			ACTSET.SetRow(CIND, A.GetRow(ind));
			ACTIND(CIND) = ind;
			m = RowSize(ACTSET); n = ColSize(ACTSET);
			qrinsert(Q, R, CIND, ~(A.GetRow(ind)));
		}

		if ( oldind ) aix(oldind) = 0; 
	
		if ( !simplex_iter )
		{
			// Z = null(ACTSET);
			m = RowSize(ACTSET); n = ColSize(ACTSET);
			Z = Q.GetCol(m+1,int(n));
			ACTCNT++;
			if ( ACTCNT == nvars - 1 ) simplex_iter = 1;
			CIND = ACTCNT+1;
			oldind = 0; 
		} else
		{
			rlambda = -R % (~Q*gf);
		  
			if ( rlambda(1) < -_INF )
			{
				opt_out("         Working set is singular; results may still be reliable.");
				m = RowSize(ACTSET); n = ColSize(ACTSET);
				rlambda = -( ~(ACTSET + sqrt_eps * Rand(m,int(n)) ) ) % gf;				
			}
			actlambda = rlambda;
			actlambda.Push(eqix,Abs(actlambda(eqix)));
			indlam = Find( actlambda < 0 );

			if ( Length(indlam) )
			{
				if ( STEPMIN > errnorm )
				{
					// If there is no chance of cycling then pick the constraint which causes the biggest reduction in the cost function. 
					// i.e the constraint with the most negative Lagrangian multiplier. Since the constraints are normalized this may 
					// result in less iterations.
					minl = MinVec(actlambda, &CIND);
				} else
				{
					// Bland's rule for anti-cycling: if there is more than one 
					// negative Lagrangian multiplier then delete the constraint
					// with the smallest index in the active set.
					MinVec(ACTIND(indlam), &CIND);
					//CIND = Find(ACTIND == Min(ACTIND(indlam)));
				}
			 
				qrdelete(Q,R,CIND);
				Z = Q.GetCol(nvars);
				oldind = int(ACTIND(CIND));
			} else
			{
				lambda.Push(indepInd(ACTIND), normf * ElDivid(rlambda,normA(ACTIND)) );
				return;
			}
		} // if ACTCNT<nvars
		
		if ( is_qp )
		{
			Zgf = ~Z*gf; 
			if ( !IsEmpty(Zgf) && (Norm(Zgf) < 1E-15) ) SD = Zeros(nvars,1); 
			else compdir(SD, dirType, Z,H,gf,nvars,f);
		} else if ( LLS )
		{
			Zgf = ~Z*gf;
			HZ = H*Z;
			if ( Norm(Zgf) < 1E-15 ) SD = Zeros(nvars,1);
			else
			{
				HXf = H*X-f;
				gf = ~H*(HXf);
				Size(HZ, &mm, &nn);
				if ( mm >= nn )
				{
					QR(HZ, QHZ, RHZ);
					Pd = ~QHZ*HXf;
					// SD = - Z*(RHZ(1:nn, 1:nn) \ Pd(1:nn,:));
					// Now need to check which is dependent
					if ( IsVector(RHZ) ) // Make sure RHZ isn't a vector
						depInd = Find( Abs(RHZ(1,1)) < tolDep );
					else
						depInd = Find( Abs(Diag(RHZ)) < tolDep );
				}
				if ( mm >= nn && IsEmpty(depInd) ) // Newton step
				{
					SD = - Z*(RHZ.Sub(1,nn, 1,nn) % Pd.GetRow(1,nn));
					strcpy(dirType, "Newton");
				} else // steepest descent direction
				{
					SD = -Z*(~Z*gf);
					strcpy(dirType, "steepest descent");
				}
			}
		} else // LP
		{
			if ( !simplex_iter )
			{
				SD = -Z*(~Z*gf);
				gradsd = Norm(SD);		
			} else
			{
				gradsd = Inner(Z,gf);
				if ( gradsd > 0 ) SD = -Z;
				else SD = Z;
			}
			if ( fabs(gradsd) < 1E-10 )  // Search direction null
			{
				// Check whether any constraints can be deleted from active set.
				// rlambda = -ACTSET'\gf;
				if ( !oldind ) rlambda = -R % (~Q*gf);
				actlambda = rlambda;
				actlambda.Push(RowIndex(1,neqcstr), Abs(actlambda(RowIndex(1,neqcstr))));
				indlam = Find(actlambda < errnorm);
				lambda.Push( indepInd(ACTIND), normf * ElDivid(rlambda,normA(ACTIND)) );
				if ( !Length(indlam) ) return;
				cindmax = Length(indlam);
				cindcnt = 0;
				newactcnt = 0;
				while ( fabs(gradsd) < 1E-10 && cindcnt < cindmax )
				{
					cindcnt++;
					if ( oldind ) // Put back constraint which we deleted
						qrinsert(Q,R,CIND,~(A.GetRow(oldind)));
					else
					{
						simplex_iter = 0;
						if ( !newactcnt ) newactcnt = ACTCNT - 1;
					}
					CIND = int(indlam(cindcnt));
					oldind = int(ACTIND(CIND));
					qrdelete(Q,R,CIND);
					m = RowSize(ACTSET); n = ColSize(ACTSET);
					Z = Q.GetCol(m,int(n));
					if ( m != nvars )
					{
						SD = -Z*~Z*gf;
						gradsd = Norm(SD);
					} else
					{
						gradsd = Inner(Z,gf);
						if ( gradsd > 0 ) SD = -Z;
						else SD = Z;
					}
				}
				if ( fabs(gradsd) < 1E-10 ) return; // Search direction still null
				lambda = Zeros(ncstr,1);
				if ( newactcnt ) ACTCNT = newactcnt;
			}
		}

		if ( simplex_iter && oldind )
		{
			// Avoid case when CIND is greater than ACTCNT
			if ( CIND <= ACTCNT )
			{
				ACTIND.Remove(CIND);
				ACTSET.RemoveRow(CIND);
				CIND = nvars;
			}
		}		
	} // while 1
}

void v2sort(RMatrix &v1, RMatrix &v2)
// function v2=v2sort(v1,v2)
// V2SORT Sorts two vectors and then removes missing elements.
//    Given two complex vectors v1 and v2, v2 is returned with
//       the nearest elements which are missing from v1 removed.
//     Syntax:  v2=V2SORT(v1,v2) 
{
	int lv1 = Length(v1);
	int lv2 = Length(v2);
	int lv2init = lv2;
	RMatrix v1ones = Ones(1,lv1);
	RMatrix v2ones = Ones(lv2,1);
	RMatrix diff = Abs( (v2ones*~v1)-(v2*v1ones) );
	if ( lv1 > lv2 )
	{
		RMatrix temp = Min(diff), ind;
		Sort(-temp, ind);
		for ( int i = 1; i <= lv1-lv2; i++ )
		{
			int i2 = min(lv2,int(ind(i))-1);
			if ( ind(i)-1 <= lv2init && i < lv2init )
				v2 = RMatrix(3,1, v2(ColIndex(1,i2)), RMatrix(v1(int(ind(i)))), v2(ColIndex(i2+1,lv2)));
			else
				v2 = RMatrix(2,1,v2,v1(lv2+1));
			lv2++;
		}
	} else
	{
		RMatrix temp = Min(~diff), ind;
		Sort(-temp, ind);
		v2.Remove( ind(RowIndex(1,lv2-lv1)) );
	}
}

int nlconst(RMatrix &x, RMatrix &OPTIONS, RMatrix &lambda, RMatrix &HESS, void (FUNfcn)(const RMatrix &, const RMatrix &, real &, RMatrix &), RMatrix VLB, RMatrix VUB, void (GRADfcn)(const RMatrix &, const RMatrix &, RMatrix &, RMatrix &), const RMatrix &varargin)
//   function [x,OPTIONS,lambda,HESS]=nlconst(FUNfcn,x,OPTIONS,VLB,VUB,GRADfcn,varargin)
//
//   NLCONST is a helper function for SIMCNSTR to find the constrained minimum 
//   of a function of several variables.
//
//   [X,OPTIONS,LAMBDA,HESS]=NLCONST('FUN',X0,OPTIONS,VLB,VUB,'GRADFUN',...
//   varargin{:}) starts at X0 and finds a constrained minimum to the function 
//   which is described in FUN. FUN is a four element cell array set up by 
//   PREFCNCHK.  It contains the call to the objective/constraint function, the 
//   gradients of the objective/constraint functions, the calling type (used by 
//   OPTEVAL), and the calling function name. 
//
{
	RMatrix XOUT, CHG, GNEW, gf_user, gg_user, g, OLDX, OLDG, oldg, OLDgf, gf, OLDAN, LAMBDA, NEWLAMBDA, OLDLAMBDA, POINT, NPOINT, gfFD, ggFD, gg, AN, schg, YL, sdiff, GOLD, LOLD, XN, ga, MATX, FACTOR, GT, SD, bestx, bestHess, bestlambda, active_const;
	real f, bestf, temp, oldf, WT, mg, gamma, MATL, mf, OLDF, MERIT, MERIT2, MATL2, YMAX;
	int lenvlb, lenvub, nvars, i, sizep, ncstr, analytic_gradient, status, FLAG, gcnt, diff, ma, na, infeas, YIND;
	char how[1024], howqp[1024], comment[1024];

	// Expectations: GRADfcn must be [] if it does not exist.

	// Initialize so if OPT_STOP these have values
	lambda = RMatrix(); HESS = RMatrix();

	// Set up parameters.
	XOUT = ColVector(x);
	VLB.MakeColVector();
	VUB.MakeColVector();
	lenvlb = Length(VLB);
	lenvub = Length(VUB);
	bestf = _INF;
	nvars = Length(XOUT);
	CHG = 1E-7*Abs(XOUT) + 1E-7*Ones(nvars,1);

	if ( lenvlb*lenvlb > 0 ) if ( IsTrue( Any( VLB(ColIndex(1,lenvub)) > VUB ) ) ) { opt_out("constr : Bounds Infeasible"); return 0; }

	for ( i = 1; i <= lenvlb; i++ ) 
		if ( lenvlb > 0 )
			if ( XOUT(i) < VLB(i) ) XOUT(i) = VLB(i) + 1E-4;

	for ( i=1; i <= lenvub; i++ )
		if ( lenvub > 0 )
			if ( XOUT(i) > VUB(i) )
			{
				XOUT(i) = VUB(i);
				CHG(i) = -CHG(i);
			}
	// Used for semi-infinite optimization:
	FLAG = 2;
	x = XOUT;  // Set x to have user expected size
	// Compute the objective function and constraints
	FUNfcn(x, varargin, f, g);
	g = ColVector(g);
	ncstr = Length(g);

	// Evaluate gradients and check size
	if ( GRADfcn == 0 ) analytic_gradient = 0;
	else
	{
		analytic_gradient = 1;
		GRADfcn(x, varargin, gf_user, gg_user);
		gf_user.MakeColVector();
		// Both might evaluate to empty when expression syntax is used
		if ( IsEmpty(gf_user) && IsEmpty(gg_user) ) analytic_gradient = 0;
		else  // Either gf or gg is defined
		{
			if ( Length(gf_user) != nvars ) { opt_out("constr : The objective gradient is the wrong size."); return 0; }
			if ( IsEmpty(gg_user) && IsEmpty(g) ) // Make gg compatible
			{
				gg = ~g;
				gg_user = ~g;
			} else // Check size of gg
			{
			   int ggrow = RowSize(gg_user);
			   int ggcol = ColSize(gg_user);
			   if ( ggrow != nvars ) { opt_out("constr : The constraint gradient has the wrong number of rows."); return 0; }
			   if ( ggcol != ncstr ) { opt_out("constr : The constraint gradient has the wrong number of columns."); return 0; }
			}
		}
	}
	OLDX = XOUT;
	OLDG = g;
	OLDgf = Zeros(nvars,1);
	gf = Zeros(nvars,1);
	OLDAN = Zeros(ncstr,nvars);
	LAMBDA = Zeros(ncstr,1);
	sizep = Length(OPTIONS);
	OPTIONS = foptions(OPTIONS);
	if ( lenvlb*lenvlb > 0 )
		if ( IsTrue(Any(VLB(ColIndex(1,lenvub)) > VUB))) { opt_out("constr : Bounds Infeasible"); return 0; }
	for ( i = 1; i <= lenvlb; i++ )
		if ( lenvlb > 0 ) 
			if ( XOUT(i) < VLB(i) ) XOUT(i) = VLB(i) + _EPS;
	OPTIONS(18) = 1;
	if ( OPTIONS(1) > 0 )
	{
		if ( OPTIONS(7) == 1 ) opt_out("f-COUNT     MAX{g}         STEP  Procedures");
		else opt_out("f-COUNT   FUNCTION       MAX{g}         STEP  Procedures");
	}
	HESS = Eye(nvars);
	if ( sizep < 1 || OPTIONS(14) == 0 ) OPTIONS(14) = nvars*100;
	OPTIONS(10) = 1;
	OPTIONS(11) = 1;
	GNEW = 1E8*CHG;
	
	//---------------------------------Main Loop-----------------------------
	status = 0;
	while ( status != 1 )
	{

	//----------------GRADIENTS----------------

		if ( !analytic_gradient || OPTIONS(9) )
		{
			// Finite Difference gradients (even if just checking analytical)
			POINT = NPOINT; 
			oldf = f;
			oldg = g;
			ncstr = Length(g);
			FLAG = 0; // For semi-infinite
			gg = Zeros(nvars, ncstr);  // For semi-infinite
			// Try to make the finite differences equal to 1e-8.
			CHG = ElDivid(RMatrix(-1E-8), (GNEW+_EPS));
			CHG = ElMult(Sign(CHG+_EPS) , Min( Max(Abs(CHG),OPTIONS(16)),OPTIONS(17)) );
			for ( gcnt = 1; gcnt <= nvars; gcnt++ )
			{
				if ( gcnt == nvars ) FLAG = -1; 
				temp = XOUT(gcnt);
				XOUT(gcnt)= temp + CHG(gcnt);
				x = XOUT;
            
				FUNfcn(x, varargin, f, g);
				// Next line used for problems with varying number of constraints
				if ( ncstr != Length(g) )
				{
					diff = Length(g); 
					v2sort(oldg,g); 
				}
				gf(gcnt,1) = (f-oldf)/CHG(gcnt);
				if (!IsEmpty(g) ) gg.SetRow(gcnt, ~(g - oldg)/CHG(gcnt) );
				XOUT(gcnt) = temp;			
    		} // for 
			// Gradient check
			if ( OPTIONS(9) == 1 && analytic_gradient )
			{
				gfFD = gf;
				ggFD = gg; 
				gg = gg_user;
				gf = gf_user;

				opt_out("Function derivative");
				if ( !IsEmpty(gg) )	opt_out("Constraint derivative");
				OPTIONS(9) = 0;
			} // OPTIONS(9) == 1 & analytic_gradient
			FLAG = 1; // For semi-infinite
			OPTIONS(10) += nvars;
			f = oldf;
			g = oldg;
		} else // analytic_gradient & options(9)=0
		{
			// User-supplied gradients
			// gf and gg already computed first time through loop
			if ( OPTIONS(10) > 1 )
			{
				gg = Zeros(nvars, ncstr);
				GRADfcn(x, varargin, gf, gg);
				gf.MakeColVector();
				if ( IsEmpty(gg) && IsEmpty(g) ) gg = ~g;
			} else
			{
				// First time through loop
				gg = gg_user;
				gf = gf_user;				
			}
		}  //if ~analytic_gradient | OPTIONS(9)
		AN = ~gg;
		how[0] = '\0';

		//-------------SEARCH DIRECTION---------------
		for ( i = 1; i <= OPTIONS(13); i++ )
		{
			schg = AN.GetRow(i) * gf;
			if ( IsTrue(schg > 0) )
			{
				AN.SetRow(i, -AN.GetRow(i));
				g(i) = -g(i);
			}
		}

		if ( OPTIONS(11) > 1 )  // Check for first call    
		{
			// For equality constraints make gradient face in 
			// opposite direction to function gradient.
			if ( OPTIONS(7) != 5 ) NEWLAMBDA = LAMBDA;         
			ma = RowSize(AN); na = ColSize(AN);
			GNEW = gf + ~AN * NEWLAMBDA;
			GOLD = OLDgf + ~OLDAN * LAMBDA;
			YL = GNEW - GOLD;
			sdiff = XOUT - OLDX;
			// Make sure Hessian is positive definite in update.
			if ( IsTrue(~YL*sdiff < OPTIONS(18)*OPTIONS(18)*1E-3) )
			{
				while ( IsTrue(~YL*sdiff < -1E-5) )
				{
					YMAX = MinVec(ElMult(YL,sdiff), &YIND);
					YL(YIND) /= 2;
				}

				if ( IsTrue(~YL*sdiff < (_EPS*FNorm(HESS))) )
				{
					strcpy(how, "Hessian modified twice");
					FACTOR = ~AN*g - ~OLDAN*OLDG;

					FACTOR = ElMult(ElMult(FACTOR , (ElMult(sdiff,FACTOR)>0) ) , (ElMult(YL,sdiff)<=_EPS) );
					WT = 1E-2;
					if ( MaxVec(Abs(FACTOR)) == 0 ) FACTOR = 1E-5*Sign(sdiff);
					while ( IsTrue( (~YL*sdiff < _EPS*FNorm(HESS)) && WT < 1/_EPS ))
					{
						YL += WT*FACTOR;
						WT *= 2;
					}
				} else strcpy(how, "Hessian modified");
			}

			//----------Perform BFGS Update If YL'S Is Positive---------
			if ( IsTrue(~YL*sdiff > _EPS) )
			{
				HESS += ( (YL*~YL)/(~YL*sdiff) - ((HESS*sdiff)*(~sdiff*~HESS))/(~sdiff*HESS*sdiff) );
			}
			// BFGS Update using Cholesky factorization  of Gill, Murray and Wright.
			// In practice this was less robust than above method and slower.
			//   R=chol(HESS); 
			//   s2=R*S; y=R'\YL; 
			//   W=eye(nvars,nvars)-(s2'*s2)\(s2*s2') + (y'*s2)\(y*y');
			//   HESS=R'*W*R;
			else strcpy( how, "Hessian not updated");
		} else // First call
		{
			OLDLAMBDA = ElDivid((_EPS+Inner(gf,gf))*Ones(ncstr,1), (~Sum(ElMult(~AN,~AN))+_EPS) );
		} // if OPTIONS(11)>1
		OPTIONS(11)++;

		LOLD = LAMBDA;
		OLDAN = AN;
		OLDgf = gf;
		OLDG = g;
		OLDF = f;
		OLDX = XOUT;
		XN = Zeros(nvars,1);
		if ( OPTIONS(7) > 0 && OPTIONS(7) < 5 )
		{
			// Minimax and attgoal problems have special Hessian:
			HESS.Push(nvars,ColIndex(1,nvars), Zeros(1,nvars));
			HESS.Push(RowIndex(1,nvars),nvars, Zeros(nvars,1));
			HESS(nvars,nvars) = 1E-8*InftyNorm(HESS);
			XN(nvars) = MaxVec(g); // Make a feasible solution for qp
		}
		if ( lenvlb > 0 )
		{
			AN = RMatrix(2,1, AN, -Eye(lenvlb,nvars));
			GT = RMatrix(2,1, g, -XOUT(ColIndex(1,lenvlb))+VLB);
		} else GT = g;
		if ( lenvub > 0 )
		{
			AN = RMatrix(2,1, AN, Eye(lenvub,nvars));
			GT = RMatrix(2,1, GT, XOUT(ColIndex(1,lenvub))-VUB);
		}
		HESS = (HESS + ~HESS)*0.5;

		qpsub(SD, lambda, howqp, HESS, gf, AN, -GT, RMatrix(), RMatrix(), XN, int(OPTIONS(13)), -1, "nlconst", RowSize(AN), nvars);
		lambda.Push(ColIndex(1,OPTIONS(13)), Abs( lambda( ColIndex(1,OPTIONS(13)) ) ));
		ga = RMatrix(2,1, Abs( g( ColIndex(1,OPTIONS(13))) ) , g( ColIndex(OPTIONS(13)+1,ncstr) ) );

		if ( !IsEmpty(g) ) mg = MaxVec(ga);
		else mg = 0;
    
		if ( OPTIONS(1) > 0 )
		{
			if ( !strncmp(howqp,"ok",2) ) howqp[0] = '\0';
			if ( strlen(how) && strlen(howqp) ) strcat(how, "; ");
			if ( OPTIONS(7) == 1 )
			{
				gamma = mg + f;
				
				sprintf(comment, "%5.0f %12.6f %12.3f  %s %s",OPTIONS(10), gamma, OPTIONS(18), how, howqp);
				opt_out(comment);
			} else 
			{
				sprintf(comment, "%5.0f %12.6f % 12.6f %12.3f  %s %s",OPTIONS(10), f, mg, OPTIONS(18), how, howqp);
				opt_out(comment);
			}			
		}
		LAMBDA = lambda(ColIndex(1,ncstr));
		OLDLAMBDA = ~Max( RMatrix( 2,1, ~LAMBDA, 0.5*~(LAMBDA+OLDLAMBDA) ) );
		//---------------LINESEARCH--------------------
		MATX = XOUT;

		MATL = f + Todouble(Sum( ElMult(ElMult(OLDLAMBDA, ga>0) , ga)) ) + 1E-30;

		infeas = (howqp[0] == 'i');
		if ( OPTIONS(7) == 0 || OPTIONS(7) == 5 )
		{
			// This merit function looks for improvement in either the constraint or the objective function unless the sub-problem is infeasible in which
			// case only a reduction in the maximum constraint is tolerated. This less "stringent" merit function has produced faster convergence in
			// a large number of problems. 
			if ( mg > 0 ) MATL2 = mg;
			else if ( f >= 0 ) MATL2 = -1/(f+1);
			else MATL2 = 0;
			
			if ( !infeas && f < 0 ) MATL2 += (f - 1);		
		} else
		{
			// Merit function used for MINIMAX or ATTGOAL problems.
			MATL2 = mg + f;
		}
		if ( mg < _EPS && f < bestf )
		{
			bestf = f;
			bestx = XOUT;
			bestHess = HESS;
			bestlambda = lambda;
		}
		MERIT = MATL + 1;
		MERIT2 = MATL2 + 1; 
		OPTIONS(18) = 2;
		while ( (MERIT2 > MATL2) && (MERIT > MATL) && (OPTIONS(10) < OPTIONS(14)) )
		{
			OPTIONS(18) /= 2;
			if ( OPTIONS(18) < 1E-4 ) 
			{
				OPTIONS(18) = -OPTIONS(18); 
				 // Semi-infinite may have changing sampling interval so avoid too stringent check for improvement
				if ( OPTIONS(7) == 5 )
				{
					OPTIONS(18) = -OPTIONS(18); 
					MATL2 += 10; 
				}
			}
			XOUT = MATX + OPTIONS(18)*SD;
			x = XOUT; 
			FUNfcn(x, varargin, f, g);
			g.MakeColVector();

			OPTIONS(10)++;
			ga = RMatrix(2,1, Abs(g( ColIndex(1,OPTIONS(13)) )) , g( ColIndex(OPTIONS(13)+1,Length(g)) ) );
			if ( !IsEmpty(g) ) mg = MaxVec(ga);
			else mg = 0;

			MERIT = f + Todouble(Sum( ElMult(ElMult(OLDLAMBDA,ga>0),ga) ));
			if ( OPTIONS(7) == 0 || OPTIONS(7) == 5 )
			{
				if ( mg > 0 ) MERIT2 = mg;
				else if ( f >= 0 ) MERIT2 = -1/(f+1);
				else MERIT2 = 0;
				
				if ( !infeas && f < 0 ) MERIT2 += (f - 1);			
			} else MERIT2 = mg + f;        
		}
		//------------Finished Line Search-------------

		if ( OPTIONS(7) != 5 )
		{
			mf = fabs(OPTIONS(18));
			LAMBDA = mf*LAMBDA + (1-mf)*LOLD;
		}
		
		if ( MaxVec(Abs(SD))< 2*OPTIONS(2) && IsTrue(Abs(~gf*SD) < 2*OPTIONS(3)) && ( mg < OPTIONS(4) || ( mg > 0 && howqp[0] == 'i') ) )
		{
			if ( OPTIONS(1) > 0 )
			{
				if ( OPTIONS(7) == 1 )
				{
					gamma = mg+f;
					sprintf(comment, "%5.0f %12.6f %12.3f  %s %s",OPTIONS(10), gamma, OPTIONS(18), how, howqp);
					opt_out(comment);
				} else 
				{
					sprintf(comment, "%5.0f %12.6f % 12.6f %12.3f  %s %s",OPTIONS(10), f, mg, OPTIONS(18), how, howqp);
					opt_out(comment);
				}
				
				if ( howqp[0] != 'i' )
				{
					opt_out("Optimization Converged Successfully");
					active_const = Find( LAMBDA > 0 );
					if ( IsTrue(active_const) )
					{
						char tmp_int[1024];
						sprintf(comment, "Active Constraints: ");
						for ( i = 1; i <= Length(active_const); i++ )
						{
							sprintf(tmp_int, "%d ", active_const(i) );
							strcat(comment, tmp_int);
						}
						opt_out(comment);						
					}
					else // active_const == 0
						opt_out(" No Active Constraints");
				}
			}
			if ( howqp[0] == 'i' && mg > 0 )
				opt_out("Warning: No feasible solution found.");
			status=1;
		} else
		{
			// NEED=[LAMBDA>0]|G>0
			if ( OPTIONS(10) >= OPTIONS(14) )
			{
				XOUT = MATX;
				f = OLDF;
				status=1;
			}
		}
	}

	// If a better unconstrained solution was found earlier, use it:
	if ( f > bestf )
	{
		XOUT = bestx;
		f = bestf;
		HESS = bestHess;
		lambda = bestlambda;
	}

	OPTIONS(8) = f;
	x = XOUT;
	return 1;
}

void (*vfun)(const RMatrix &, real &, RMatrix &);

void func(const RMatrix &x, const RMatrix &varargin, real &f, RMatrix &g)
{
	if ( vfun ) vfun(x, f, g);
}

void (*vgradfun)(const RMatrix &, RMatrix &, RMatrix &);

void gradfcnc(const RMatrix &x, const RMatrix &varargin, RMatrix &df, RMatrix &dg)
{
	if ( vgradfun ) vgradfun(x, df, dg);
}

RMatrix constr(void (*fun)(const RMatrix &, real &, RMatrix &), const RMatrix &x0)
{
	RMatrix OPTIONS = foptions(), lambda, HESS, VLB, VUB, varargin, x = x0;
	vfun = fun;
	nlconst(x, OPTIONS, lambda, HESS, func, VLB, VUB, 0, varargin);
	return x;
}

RMatrix constr(void (*fun)(const RMatrix &, const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &varargin)
{
	RMatrix OPTIONS = foptions(), lambda, HESS, VLB, VUB, x = x0;
	nlconst(x, OPTIONS, lambda, HESS, fun, VLB, VUB, 0, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, real &, RMatrix &), const RMatrix &x0)
{
	RMatrix lambda, HESS, VLB, VUB, varargin, x = x0;
	OPTIONS = foptions(OPTIONS);
	vfun = fun;
	nlconst(x, OPTIONS, lambda, HESS, func, VLB, VUB, 0, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &varargin)
{
	RMatrix lambda, HESS, VLB, VUB, x = x0;
	OPTIONS = foptions(OPTIONS);
	nlconst(x, OPTIONS, lambda, HESS, fun, VLB, VUB, 0, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &vlb, const RMatrix &vub)
{
	RMatrix lambda, HESS, varargin, x = x0;
	OPTIONS = foptions(OPTIONS);
	vfun = fun;
	nlconst(x, OPTIONS, lambda, HESS, func, vlb, vub, 0, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &vlb, const RMatrix &vub, const RMatrix &varargin)
{
	RMatrix lambda, HESS, x = x0;
	OPTIONS = foptions(OPTIONS);
	nlconst(x, OPTIONS, lambda, HESS, fun, vlb, vub, 0, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &vlb, const RMatrix &vub, void (*gradfcn)(const RMatrix &, RMatrix &, RMatrix &))
{
	RMatrix lambda, HESS, varargin, x = x0;
	OPTIONS = foptions(OPTIONS);
	vfun = fun;
	vgradfun = gradfcn;
	nlconst(x, OPTIONS, lambda, HESS, func, vlb, vub, gradfcnc, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, void (*fun)(const RMatrix &, const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &vlb, const RMatrix &vub, void (*gradfcn)(const RMatrix &, const RMatrix &, RMatrix &, RMatrix &), const RMatrix &varargin)
{
	RMatrix lambda, HESS, x = x0;
	OPTIONS = foptions(OPTIONS);
	nlconst(x, OPTIONS, lambda, HESS, fun, vlb, vub, gradfcn, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, RMatrix &lambda, RMatrix &HESS, void (*fun)(const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &VLB, const RMatrix &VUB, void (*gradfcn)(const RMatrix &, RMatrix &, RMatrix &))
{
	RMatrix x = x0, varargin;
	OPTIONS = foptions(OPTIONS);
	vfun = fun;
	vgradfun = gradfcn;
	nlconst(x, OPTIONS, lambda, HESS, func, VLB, VUB, gradfcnc, varargin);
	return x;
}

RMatrix constr(RMatrix &OPTIONS, RMatrix &lambda, RMatrix &HESS, void (*fun)(const RMatrix &, const RMatrix &, real &, RMatrix &), const RMatrix &x0, const RMatrix &VLB, const RMatrix &VUB, void (*gradfcn)(const RMatrix &, const RMatrix &, RMatrix &, RMatrix &), const RMatrix &varargin)
{
	RMatrix x = x0;
	OPTIONS = foptions(OPTIONS);
	nlconst(x, OPTIONS, lambda, HESS, fun, VLB, VUB, gradfcn, varargin);
	return x;
}