/*
 *  This file is part of qpOASES.
 *
 *  qpOASES -- An Implementation of the Online Active Set Strategy.
 *  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
 *  Christian Kirches et al. All rights reserved.
 *
 *  qpOASES is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  qpOASES is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *  See the GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with qpOASES; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


/**
 *  \file src/SQProblemSchur.cpp
 *  \author Andreas Waechter and Dennis Janka, based on QProblem.cpp by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
 *  \version 3.2
 *  \date 2012-2015
 *
 *  Implementation of the SQProblemSchur class which is able to use the newly
 *  developed online active set strategy for parametric quadratic programming.
 *  This implementation uses a Schur complement approach to solve the linear
 *  systems.
 */

#include <qpOASES/SQProblemSchur.hpp>


#ifndef __MATLAB__
# include <cstdarg>
void MyPrintf(const char* pformat, ... )
{
  va_list ap;
  va_start(ap, pformat);

  vfprintf(stdout, pformat, ap);

  va_end(ap);
}
#else
# include <mex.h>
# define MyPrintf mexPrintf
#endif


BEGIN_NAMESPACE_QPOASES


/*****************************************************************************
 *  P U B L I C                                                              *
 *****************************************************************************/


/*
 *  Q P r o b l e m
 */
SQProblemSchur::SQProblemSchur( ) : SQProblem( )
{
#ifdef SOLVER_MA57
    sparseSolver = new Ma57SparseSolver();
#elif defined SOLVER_MA27
    sparseSolver = new Ma27SparseSolver();
#elif defined SOLVER_NONE
    sparseSolver = new UserSparseSolver(0, 0, 0, 0, 0);
#endif

    nSmax = 0;
    nS = -1;
    S = 0;
    Q_ = 0;
    R_ = 0;
    detS = 0.0;
    rcondS = 0.0;
    schurUpdateIndex = 0;
    schurUpdate = 0;
    numFactorizations = 0;

    M_physicallength = 0;
    M_vals = 0;
    M_ir = 0;
    M_jc = 0;
}


/*
 *  Q P r o b l e m
 */
SQProblemSchur::SQProblemSchur( int_t _nV, int_t _nC, HessianType _hessianType, int_t maxSchurUpdates,
  linsol_memory_t _linsol_data, linsol_init_t _linsol_init,
  linsol_sfact_t _linsol_sfact, linsol_nfact_t _linsol_nfact, linsol_solve_t _linsol_solve)
  : SQProblem( _nV,_nC,_hessianType ),
    linsol_data(_linsol_data), linsol_init(_linsol_init),
    linsol_sfact(_linsol_sfact), linsol_nfact(_linsol_nfact), linsol_solve(_linsol_solve) {
    /* We use the variables Q and R to store the QR factorization of S.
     * T is not required. */
    delete [] R; R = 0;
    delete [] Q; Q = 0;
    delete [] T; T = 0;

    /* The interface to the sparse linear solver.  In the long run,
       different linear solvers might be optionally chosen. */
#ifdef SOLVER_MA57
    sparseSolver = new Ma57SparseSolver();
#elif defined SOLVER_MA27
    sparseSolver = new Ma27SparseSolver();
#elif defined SOLVER_NONE
    sparseSolver = new UserSparseSolver(linsol_data, linsol_init, linsol_sfact, linsol_nfact, linsol_solve);
#endif

    nSmax = maxSchurUpdates;
    nS = -1;
    if ( nSmax > 0 )
    {
        S = new real_t[nSmax*nSmax];
        schurUpdateIndex = new int_t[nSmax];
        schurUpdate = new SchurUpdateType[nSmax];
        Q_ = new real_t[nSmax*nSmax];
        R_ = new real_t[nSmax*nSmax];
        M_physicallength = 10*nSmax;  /* TODO: Decide good default. */
        M_vals = new real_t[M_physicallength];
        M_ir = new sparse_int_t[M_physicallength];
        M_jc = new sparse_int_t[nSmax+1];
        detS = 1.0;
        rcondS = 1.0;
    }
    else
    {
        S = 0;
        Q_ = 0;
        R_ = 0;
        detS = 0.0;
        rcondS = 0.0;
        schurUpdateIndex = 0;
        schurUpdate = 0;
        M_physicallength = 0;
        M_vals = 0;
        M_ir = 0;
        M_jc = 0;
    }
    numFactorizations = 0;
}


/*
 *  Q P r o b l e m
 */
SQProblemSchur::SQProblemSchur( const SQProblemSchur& rhs ) : SQProblem( rhs ),
  linsol_data(rhs.linsol_data), linsol_init(rhs.linsol_init),
  linsol_sfact(rhs.linsol_sfact), linsol_nfact(rhs.linsol_nfact), linsol_solve(rhs.linsol_solve) {
#ifdef SOLVER_MA57
    sparseSolver = new Ma57SparseSolver();
#elif defined SOLVER_MA27
    sparseSolver = new Ma27SparseSolver();
#elif defined SOLVER_NONE
    sparseSolver = new UserSparseSolver(linsol_data, linsol_init, linsol_sfact, linsol_nfact, linsol_solve);
#endif
    copy( rhs );
}


/*
 *  ~ Q P r o b l e m
 */
SQProblemSchur::~SQProblemSchur( )
{
    delete sparseSolver;

    clear( );
}


/*
 *  o p e r a t o r =
 */
SQProblemSchur& SQProblemSchur::operator=( const SQProblemSchur& rhs )
{
    if ( this != &rhs )
    {
        clear( );
        SQProblem::operator=( rhs );
        copy( rhs );
    }
    return *this;
}


/*
 *  r e s e t
 */
returnValue SQProblemSchur::reset( )
{
    /* AW: We probably want to avoid resetting factorization in QProblem */
    if ( SQProblem::reset( ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_RESET_FAILED );

    sparseSolver->reset();
    nS = -1;

    return SUCCESSFUL_RETURN;
}


/*****************************************************************************
 *  P R O T E C T E D                                                        *
 *****************************************************************************/

/*
 *  c l e a r
 */
returnValue SQProblemSchur::clear( )
{
    nSmax = 0;
    nS = -1;
    detS = 0.0;
    rcondS = 0.0;
    numFactorizations = 0;
    delete [] S; S=0;
    delete [] Q_; Q_=0;
    delete [] R_; R_=0;
    delete [] schurUpdateIndex; schurUpdateIndex=0;
    delete [] schurUpdate; schurUpdate=0;
    M_physicallength = 0;
    delete [] M_vals; M_vals=0;
    delete [] M_ir; M_ir=0;
    delete [] M_jc; M_jc=0;

    return SUCCESSFUL_RETURN;
}


/*
 *  c o p y
 */
returnValue SQProblemSchur::copy(   const SQProblemSchur& rhs
                                    )
{
    int_t i, j, length;

    *sparseSolver = *(rhs.sparseSolver);

    nS = rhs.nS;
    nSmax = rhs.nSmax;
    if ( nSmax > 0 )
    {
        detS = rhs.detS;
        rcondS = rhs.rcondS;
        S = new real_t[nSmax*nSmax];
        Q_ = new real_t[nSmax*nSmax];
        R_ = new real_t[nSmax*nSmax];
        schurUpdateIndex = new int_t[nSmax];
        schurUpdate = new SchurUpdateType[nSmax];

        if ( nS>0 )
        {
            for ( i=0; i<nS; i++)
                for ( j=0; j<nS; j++)
                {
                    S[i*nSmax + j] = rhs.S[i*nSmax + j];
                    Q_[i*nSmax + j] = rhs.Q_[i*nSmax + j];
                    R_[i*nSmax + j] = rhs.R_[i*nSmax + j];
                }

            memcpy( schurUpdateIndex, rhs.schurUpdateIndex, ((unsigned int)nS)*sizeof(int_t));
            memcpy( schurUpdate, rhs.schurUpdate, ((unsigned int)nS)*sizeof(SchurUpdateType));
        }

        M_physicallength = rhs.M_physicallength;
        if ( M_physicallength>0 )
        {
            M_vals = new real_t[M_physicallength];
            M_ir = new sparse_int_t[M_physicallength];
            M_jc = new sparse_int_t[nSmax+1];

            if ( nS>0 )
            {
                memcpy(M_jc, rhs.M_jc, ((unsigned int)(nS+1))*sizeof(sparse_int_t));
                length = M_jc[nS];
                memcpy(M_vals, rhs.M_vals, ((unsigned int)length)*sizeof(real_t));
                memcpy(M_ir, rhs.M_ir, ((unsigned int)length)*sizeof(sparse_int_t));
            }
            else if ( nS==0 )
                M_jc[0] = rhs.M_jc[0];
        }
    }
    else
    {
        S = 0;
        Q_ = 0;
        R_ = 0;
        detS = 0.0;
        rcondS = 0.0;
        schurUpdateIndex = 0;
        schurUpdate = 0;
        M_physicallength = 0;
        M_vals = 0;
        M_ir = 0;
        M_jc = 0;
    }
    numFactorizations = rhs.numFactorizations;

    boundsFreeStart = rhs.boundsFreeStart;
    constraintsActiveStart = rhs.constraintsActiveStart;

    return SUCCESSFUL_RETURN;
}


/*
 *  s e t u p A u x i l i a r y Q P
 */
returnValue SQProblemSchur::setupAuxiliaryQP(   SymmetricMatrix *H_new,
                                                Matrix *A_new,
                                                const real_t *lb_new,
                                                const real_t *ub_new,
                                                const real_t *lbA_new,
                                                const real_t *ubA_new
                                                )
{
    int_t i;
    int_t nV = getNV( );
    int_t nC = getNC( );
    returnValue returnvalue;

    if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
         ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
         ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
    {
        return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
    }

    status = QPS_PREPARINGAUXILIARYQP;


    /* I) SETUP NEW QP MATRICES AND VECTORS: */
    /* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
     *    that old optimal solution remains feasible for new QP data. */
    /*    Firstly, shift by -A'*x_opt and ... */
    if ( nC > 0 )
    {
        if ( A_new == 0 )
            return THROWERROR( RET_INVALID_ARGUMENTS );

        for ( i=0; i<nC; ++i )
        {
            lbA[i] = -Ax_l[i];
            ubA[i] =  Ax_u[i];
        }

        /* Set constraint matrix as well as ... */
        setA( A_new );

        /* ... secondly, shift by +A_new'*x_opt. */
        for ( i=0; i<nC; ++i )
        {
            lbA[i] += Ax[i];
            ubA[i] += Ax[i];
        }

        /* update constraint products. */
        for ( i=0; i<nC; ++i )
        {
            Ax_u[i] = ubA[i] - Ax[i];
            Ax_l[i] = Ax[i] - lbA[i];
        }
    }

    /* 2) Set new Hessian matrix,determine Hessian type and
     *    regularise new Hessian matrix if necessary. */
    /* a) Setup new Hessian matrix and determine its type. */
    if ( H_new != 0 )
    {
        setH( H_new );

        hessianType = HST_UNKNOWN;
        if ( determineHessianType( ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

        /* b) Regularise new Hessian if necessary. */
        if ( ( hessianType == HST_ZERO ) ||
             ( hessianType == HST_SEMIDEF ) ||
             ( usingRegularisation( ) == BT_TRUE ) )
        {
            regVal = 0.0; /* reset previous regularisation */

            if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
        }
    }
    else
    {
        if ( H != 0 )
            return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
    }

    /* 3) Setup QP gradient. */
    if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

    /* II) SETUP WORKING SET AND MATRIX FACTORISATION: */

    /* 1) Check if current active set is linearly independent and has the correct inertia */
    returnvalue = resetSchurComplement( BT_FALSE );
    int_t neig = sparseSolver->getNegativeEigenvalues( );

    if ( returnvalue == SUCCESSFUL_RETURN && neig == getNAC( ) )
    {
        /* a) This means the proposed working set is linearly independent and
         *    leaves no zero curvature exposed in the nullspace and can be used to start QP solve. */
        if ( options.printLevel == PL_HIGH )
            MyPrintf( "In hotstart for new matrices, old working set is linearly independent and has correct inertia.\n");

        status = QPS_AUXILIARYQPSOLVED;
        return SUCCESSFUL_RETURN;
    }
    else if ( returnvalue == SUCCESSFUL_RETURN && neig > getNAC( ) )
    {
        /* b) KKT matrix has too many negative eigenvalues. Try to correct the inertia by adding bounds (reduce nullspace dimension). */
        if ( options.printLevel == PL_HIGH )
            MyPrintf( "WARNING: In hotstart for new matrices, reduced Hessian for initial working set has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );

        /* If enabling inertia correction is disabled, exit here */
        if ( options.enableInertiaCorrection )
        {
            returnvalue = correctInertia();
            if ( returnvalue == SUCCESSFUL_RETURN )
            {
                status = QPS_AUXILIARYQPSOLVED;
                return SUCCESSFUL_RETURN;
            }
        }
        else
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
    }

    /* 2) If inertia correction has failed or factorization yielded some other error,
     *    try to rebuild the active set with all simple bounds set according to initialStatusBounds
     *    (Note: in exact arithmetic, this cannot happen) */
    if ( options.printLevel == PL_HIGH )
        MyPrintf( "WARNING: hotstart for old active set failed. Trying to rebuild a working set.\n");

    Bounds      oldBounds      = bounds;
    Constraints oldConstraints = constraints;

    /* Move all inactive variables to a bound */
    for ( i=0; i<nV; i++ )
    {
        #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
        if ( bounds.getType( i ) == ST_EQUALITY )
        {
            if ( oldBounds.setStatus( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
        }
        else
        #endif
        {
            if ( oldBounds.getStatus( i ) == ST_INACTIVE )
                if ( oldBounds.setStatus( i, options.initialStatusBounds ) != SUCCESSFUL_RETURN )
                    return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
        }
    }

    /* Set all equalities active */
    #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
    for( i=0; i<nC; ++i )
    {
        if ( constraints.getType( i ) == ST_EQUALITY )
            if ( oldConstraints.setStatus( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
    }
    #endif

    /* Set all inequalities inactive */
    for( i=0; i<nC; ++i )
    {
        if ( constraints.getType( i ) != ST_EQUALITY )
            if ( oldConstraints.setStatus( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
    }

    /* Reset bounds and constraints */
    bounds.init( nV );
    constraints.init( nC );

    if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

    if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

    if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

    /* Setup working sets afresh. */
    if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

    /* adjust lb/ub */
    for (int_t ii = 0; ii < nC; ++ii)
        Ax_l[ii] = Ax_u[ii] = Ax[ii];
    setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);

    status = QPS_AUXILIARYQPSOLVED;

    return SUCCESSFUL_RETURN;
}


/*
 *  s e t u p A u x i l i a r y W o r k i n g S e t
 */
returnValue SQProblemSchur::setupAuxiliaryWorkingSet(   const Bounds* const auxiliaryBounds,
                                                        const Constraints* const auxiliaryConstraints,
                                                        BooleanType setupAfresh
                                                        )
{
    int_t i;
    int_t nV = getNV( );
    int_t nC = getNC( );

    /* consistency checks */
    if ( auxiliaryBounds != 0 )
    {
        for( i=0; i<nV; ++i )
            if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
                return THROWERROR( RET_UNKNOWN_BUG );
    }
    else
    {
        return THROWERROR( RET_INVALID_ARGUMENTS );
    }

    if ( auxiliaryConstraints != 0 )
    {
        for( i=0; i<nC; ++i )
            if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
                return THROWERROR( RET_UNKNOWN_BUG );
    }
    else
    {
        return THROWERROR( RET_INVALID_ARGUMENTS );
    }

    /* I.) REMOVE INEQUALITY BOUNDS/CONSTRAINTS */

    /* I.1) Remove inequality bounds that are active now but shall be
     *      inactive or active at the other bound according to auxiliaryBounds */
    for( i=0; i<nV; ++i )
    {
        if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
            if ( bounds.moveFixedToFree( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );

        if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
            if ( bounds.moveFixedToFree( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
    }

    /* I.2.) Remove inequality constraints that are active now but shall be
     *       inactive or active at the other bound according to auxiliaryConstraints */
    for( i=0; i<nC; ++i )
    {
        if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
            if ( constraints.moveActiveToInactive( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );

        if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
            if ( constraints.moveActiveToInactive( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
    }

    /* II.) ADD BOUNDS/CONSTRAINTS */

    /* II.1.) Add bounds according to auxiliaryBounds */
    for( i=0; i<nV; ++i )
    {
        if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
            if ( bounds.moveFreeToFixed( i, auxiliaryBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
    }

    /* II.2.) Add constraints according to auxiliaryConstraints */
    for( i=0; i<nC; ++i )
    {
        if ( ( constraints.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
            if ( constraints.moveInactiveToActive( i,auxiliaryConstraints->getStatus( i ) ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
    }

    /* III) FACTORIZATION */

    /* III.1.) Factorize (resolves linear dependency) */
    if( resetSchurComplement( BT_FALSE ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_SETUP_WORKINGSET_FAILED );

    /* III.2.) Check if inertia is correct. If so, we now have a linearly independent working set with a pos def reduced Hessian */
    int_t neig = sparseSolver->getNegativeEigenvalues( );
    if ( neig == getNAC( ) )
    {
        /* We now have a linearly independent working set with a pos def reduced Hessian.
         * We need to correct the QP bounds and gradient after this. */
        return SUCCESSFUL_RETURN;
    }

    /* IV.) INERTIA CORRECTION IF NECESSARY */

    /* We now have a fresh factorization and can start the usual inertia correction routine */
    if ( options.printLevel == PL_HIGH )
        MyPrintf( "WARNING: In setupAuxiliaryWorkingSet: Initial working set reduced Hessian has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );

    if ( options.enableInertiaCorrection == BT_TRUE )
        return correctInertia( );
    else
        return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}


/*
 *  c h o l e s k y D e c o m p o s i t i o n P r o j e c t e d
 */
returnValue SQProblemSchur::computeProjectedCholesky( )
{
    return SUCCESSFUL_RETURN;
}


/*
 *  c o m p u t e I n i t i a l C h o l e s k y
 */
returnValue SQProblemSchur::computeInitialCholesky( )
{
    return SUCCESSFUL_RETURN;
}


/*
 *  s e t u p T Q f a c t o r i s a t i o n
 */
returnValue SQProblemSchur::setupTQfactorisation( )
{
    return SUCCESSFUL_RETURN;
}


/*
 *  a d d C o n s t r a i n t
 */
returnValue SQProblemSchur::addConstraint(  int_t number,
                                            SubjectToStatus C_status,
                                            BooleanType updateCholesky,
                                            BooleanType ensureLI
                                            )
{
    int_t idxDeleted = -1;

    /* consistency checks */
    if ( constraints.getStatus( number ) != ST_INACTIVE )
        return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );

    if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
        return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );

    if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
         ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
         ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
         ( getStatus( ) == QPS_SOLVED )            )
    {
        return THROWERROR( RET_UNKNOWN_BUG );
    }


    /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
     *    i.e. remove a constraint or bound if linear dependence occurs. */
    if ( ensureLI == BT_TRUE )
    {
        returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );

        switch ( ensureLIreturnvalue )
        {
            case SUCCESSFUL_RETURN:
                break;

            case RET_LI_RESOLVED:
                break;

            case RET_ENSURELI_FAILED_NOINDEX:
                return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;

            case RET_ENSURELI_FAILED_CYCLING:
                return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;

            case RET_ENSURELI_DROPPED:
                return SUCCESSFUL_RETURN;

            default:
                return THROWERROR( RET_ENSURELI_FAILED );
        }
    }

    /* IV) UPDATE INDICES */
    tabularOutput.idxAddC = number;
    if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_ADDCONSTRAINT_FAILED );

    /* Also update the Schur complement. */

    /* First check if this constraint had been removed before. In that
       case delete this constraint from the Schur complement. */
    bool found = false;
    for ( int_t i=0; i<nS; i++ )
    {
        if ( schurUpdate[i] == SUT_ConRemoved && number == schurUpdateIndex[i] )
        {
            if ( deleteFromSchurComplement( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ADDCONSTRAINT_FAILED );
            found = true;
            idxDeleted = i;
            break;
        }
    }
    if ( !found )
    {
        if ( nS < 0 || nS==nSmax )
        {
            /* The schur complement has become too large, reset. */
            /* Correct inertia if necessary. */
            returnValue retval = resetSchurComplement( BT_TRUE );
            if ( retval != SUCCESSFUL_RETURN )
            {
                if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                    MyPrintf( "In addConstraint: KKT matrix singular when resetting Schur complement\n" );
                else if ( options.printLevel == PL_HIGH )
                    MyPrintf( "In addConstraint, resetSchurComplement failed with retval = %d\n", retval);
                return THROWERROR( RET_ADDCONSTRAINT_FAILED );
            }
            found = true;
        }
        else
        {
            /* If the constraint was not yet in Schur complement, add it now. */
            int_t nFRStart = boundsFreeStart.getLength();
            int_t* FR_idxStart;
            boundsFreeStart.getNumberArray( &FR_idxStart );

            sparse_int_t* MNpos = new sparse_int_t[nFRStart+nS]; // This is an overestimate
            real_t* MNvals = new real_t[nFRStart+nS];

            int_t* irn = new int_t[nFRStart+nS];
            int_t* jcn = new int_t[nFRStart+nS];
            real_t* vals = new real_t[nFRStart+nS];
            int_t* icolsNumber = new int_t[nFRStart+nS];
            int_t* icolsSIdx = new int_t[nS];

            for ( int_t i=0; i<nFRStart; i++)
                icolsNumber[i] = FR_idxStart[i];

            int_t icolsLength = nFRStart;
            for ( int_t i=0; i<nS; i++)
                if ( schurUpdate[i] == SUT_VarFreed )
                {
                    icolsNumber[icolsLength] = schurUpdateIndex[i];
                    icolsSIdx[icolsLength-nFRStart] = i;
                    icolsLength++;
                }

            if ( constraintProduct != 0 )
            {
                MyPrintf( "In SQProblemSchur::addConstraint, constraintProduct not yet implemented.\n");
                return THROWERROR(RET_NOT_YET_IMPLEMENTED);
            }
            int_t numNonzerosA;
            A->getSparseSubmatrix( 1, &number, icolsLength, icolsNumber, 0, 0, numNonzerosA, irn, jcn, vals );
            delete [] irn;

            int_t numNonzerosM = 0;
            int_t numNonzerosN = 0;
            for ( int_t i=0; i<numNonzerosA; i++ )
                if ( jcn[i] < nFRStart )
                {
                    MNpos[numNonzerosM] = jcn[i];
                    MNvals[numNonzerosM] = vals[i];
                    numNonzerosM++;
                }
                else
                {
                    MNpos[nFRStart+numNonzerosN] = icolsSIdx[jcn[i]-nFRStart];
                    MNvals[nFRStart+numNonzerosN] = vals[i];
                    numNonzerosN++;
                }

            returnValue returnvalue = addToSchurComplement( number, SUT_ConAdded, numNonzerosM, MNpos, MNvals, numNonzerosN, MNpos+nFRStart, MNvals+nFRStart, 0.0 );

            delete [] icolsSIdx;
            delete [] icolsNumber;
            delete [] vals;
            delete [] jcn;
            delete [] MNvals;
            delete [] MNpos;

            if ( returnvalue != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ADDCONSTRAINT_FAILED );

            found = true;
        }
    }

    if ( !found )
        return THROWERROR( RET_ADDCONSTRAINT_FAILED );

    updateSchurQR( idxDeleted );

    /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
    if( rcondS < options.rcondSMin )
    {
        returnValue retval = resetSchurComplement( BT_TRUE );
        if ( retval != SUCCESSFUL_RETURN )
        {
            if ( retval == RET_KKT_MATRIX_SINGULAR  && options.printLevel == PL_HIGH )
                MyPrintf( "In addConstraint: KKT matrix singular when resetting Schur complement\n" );
            else if ( options.printLevel == PL_HIGH )
                MyPrintf( "In addConstraint, resetSchurComplement failed with retval = %d\n", retval);
            return THROWERROR( RET_ADDCONSTRAINT_FAILED );
        }
    }

    return SUCCESSFUL_RETURN;
}



/*
 *  a d d C o n s t r a i n t _ c h e c k L I
 */
returnValue SQProblemSchur::addConstraint_checkLI( int_t number )
{
    /* Get space for the multipliers xi in linear independence test */
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    real_t *xiC = new real_t[nAC];
    real_t *xiB = new real_t[nFX];

    /* I) Check if new constraint is linearly independent from the active ones. */
    returnValue returnvalueCheckLI = addConstraint_checkLISchur( number, xiC, xiB );

    delete [] xiB;
    delete [] xiC;

    return returnvalueCheckLI;
}


/*
 *  a d d C o n s t r a i n t _ c h e c k L I S c h u r
 */
returnValue SQProblemSchur::addConstraint_checkLISchur( int_t number, real_t* xiC, real_t* xiB )
{
    returnValue returnvalue = RET_LINEARLY_DEPENDENT;

    int_t ii;
    int_t nV  = getNV( );
    int_t nFR = getNFR( );
    int_t nC  = getNC( );
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    int_t *FR_idx;

    bounds.getFree( )->getNumberArray( &FR_idx );

    /* For the Schur complement version we only use options.enableFullLITests = TRUE */
    {
        /*
         * expensive LI test. Backsolve with refinement using special right
         * hand side. This gives an estimate for what should be considered
         * "zero". We then check linear independence relative to this estimate.
         */

        int_t *FX_idx, *AC_idx, *IAC_idx;

        real_t *delta_g   = new real_t[nV];
        real_t *delta_xFX = new real_t[nFX];
        real_t *delta_xFR = new real_t[nFR];
        real_t *delta_yAC = xiC;
        real_t *delta_yFX = xiB;

        bounds.getFixed( )->getNumberArray( &FX_idx );
        constraints.getActive( )->getNumberArray( &AC_idx );
        constraints.getInactive( )->getNumberArray( &IAC_idx );

        int_t dim = (nC>nV)?nC:nV;
        real_t *nul = new real_t[dim];
        for (ii = 0; ii < dim; ++ii)
            nul[ii]=0.0;

        A->getRow (number, 0, 1.0, delta_g);

        returnValue dsdreturnvalue = determineStepDirection ( delta_g,
                                              nul, nul, nul, nul,
                                              BT_FALSE, BT_FALSE,
                                              delta_xFX, delta_xFR, delta_yAC, delta_yFX);
        if (dsdreturnvalue!=SUCCESSFUL_RETURN)
            returnvalue = dsdreturnvalue;

        delete[] nul;

        /* compute the weight in inf-norm */
        real_t weight = 0.0;
        for (ii = 0; ii < nAC; ++ii)
        {
            real_t a = getAbs (delta_yAC[ii]);
            if (weight < a) weight = a;
        }
        for (ii = 0; ii < nFX; ++ii)
        {
            real_t a = getAbs (delta_yFX[ii]);
            if (weight < a) weight = a;
        }

        /* look at the "zero" in a relative inf-norm */
        real_t zero = 0.0;
        for (ii = 0; ii < nFX; ++ii)
        {
            real_t a = getAbs (delta_xFX[ii]);
            if (zero < a) zero = a;
        }
        for (ii = 0; ii < nFR; ++ii)
        {
            real_t a = getAbs (delta_xFR[ii]);
            if (zero < a) zero = a;
        }

        /* relative test against zero in inf-norm */
        if (zero > options.epsLITests * weight)
            returnvalue = RET_LINEARLY_INDEPENDENT;

        delete[] delta_xFR;
        delete[] delta_xFX;
        delete[] delta_g;

    }
    return THROWINFO( returnvalue );
}


/*
 *  a d d C o n s t r a i n t _ e n s u r e L I
 */
returnValue SQProblemSchur::addConstraint_ensureLI( int_t number, SubjectToStatus C_status )
{
    /* Get space for the multipliers xi in linear independence test */
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    real_t *xiC = new real_t[nAC];
    real_t *xiB = new real_t[nFX];

    /* I) Check if new constraint is linearly independent from the active ones. */
    returnValue returnvalueCheckLI = addConstraint_checkLISchur( number, xiC, xiB );

    if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
    {
        delete [] xiB;
        delete [] xiC;
        return THROWERROR( RET_ENSURELI_FAILED );
    }

    if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
    {
        delete [] xiB;
        delete [] xiC;
        return SUCCESSFUL_RETURN;
    }

    /* II) NEW BOUND IS LINEARLY DEPENDENT: */
    /* 1) Coefficients of linear combination, have already been computed, but we need to correct the sign.  */
    int_t i, ii;

    if ( C_status != ST_LOWER )
    {
        for( i=0; i<nAC; ++i )
            xiC[i] = -xiC[i];
        for( i=0; i<nFX; ++i )
            xiB[i] = -xiB[i];
    }

    int_t nV  = getNV( );

    int_t* FX_idx;
    bounds.getFixed( )->getNumberArray( &FX_idx );

    int_t* AC_idx;
    constraints.getActive( )->getNumberArray( &AC_idx );

    real_t* num = new real_t[nV];

    real_t y_min = options.maxDualJump;
    int_t y_min_number = -1;
    int_t y_min_number_bound = -1;
    BooleanType y_min_isBound = BT_FALSE;

    returnValue returnvalue = SUCCESSFUL_RETURN;

    /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */

    /* 1) Constraints. */
    for( i=0; i<nAC; ++i )
    {
        ii = AC_idx[i];
        num[i] = y[nV+ii];
    }

    performRatioTest (nAC, AC_idx, &constraints, num, xiC, options.epsNum, options.epsDen, y_min, y_min_number);

    /* 2) Bounds. */
    for( i=0; i<nFX; ++i )
    {
        ii = FX_idx[i];
        num[i] = y[ii];
    }

    performRatioTest (nFX, FX_idx, &bounds, num, xiB, options.epsNum, options.epsDen, y_min, y_min_number_bound);

    if ( y_min_number_bound >= 0 )
    {
        y_min_number = y_min_number_bound;
        y_min_isBound = BT_TRUE;
    }

    #ifndef __XPCTARGET__
    /* setup output preferences */
    char messageString[80];
    #endif

    /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
    if ( y_min_number >= 0 )
    {
        /* Update Lagrange multiplier... */
        for( i=0; i<nAC; ++i )
        {
            ii = AC_idx[i];
            y[nV+ii] -= y_min * xiC[i];
        }
        for( i=0; i<nFX; ++i )
        {
            ii = FX_idx[i];
            y[ii] -= y_min * xiB[i];
        }

        /* ... also for newly active constraint... */
        if ( C_status == ST_LOWER )
            y[nV+number] = y_min;
        else
            y[nV+number] = -y_min;

        /* ... and for constraint to be removed. */
        if ( y_min_isBound == BT_TRUE )
        {
            #ifndef __XPCTARGET__
            snprintf( messageString,80,"bound no. %d.",(int)y_min_number );
            getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
            #endif

            if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
            {
                returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
                goto farewell;
            }
            tabularOutput.excRemB = 1;

            y[y_min_number] = 0.0;
        }
        else
        {
            #ifndef __XPCTARGET__
            snprintf( messageString,80,"constraint no. %d.",(int)y_min_number );
            getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
            #endif

            if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
            {
                returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
                goto farewell;
            }
            tabularOutput.excRemC = 1;

            y[nV+y_min_number] = 0.0;
        }
    }
    else
    {
        if (options.enableDropInfeasibles == BT_TRUE) {
            /* dropping of infeasible constraints according to drop priorities */
            returnvalue = dropInfeasibles (number, C_status, BT_FALSE, xiB, xiC);
        }
        else
        {
            /* no constraint/bound can be removed => QP is infeasible! */
            returnvalue = RET_ENSURELI_FAILED_NOINDEX;
            setInfeasibilityFlag( returnvalue );
        }
    }

farewell:
    delete[] num;
    delete [] xiB;
    delete [] xiC;

    getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );

    return (returnvalue != SUCCESSFUL_RETURN) ? THROWERROR (returnvalue) : returnvalue;
}


/*
 *  a d d B o u n d
 */
returnValue SQProblemSchur::addBound(   int_t number,
                                        SubjectToStatus B_status,
                                        BooleanType updateCholesky,
                                        BooleanType ensureLI
                                        )
{
    int_t idxDeleted = -1;

    /* consistency checks */
    if ( bounds.getStatus( number ) != ST_INACTIVE )
        return THROWERROR( RET_BOUND_ALREADY_ACTIVE );

    if ( getNFR( ) == bounds.getNUV( ) )
        return THROWERROR( RET_ALL_BOUNDS_ACTIVE );

    if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
         ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
         ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
         ( getStatus( ) == QPS_SOLVED )            )
    {
        return THROWERROR( RET_UNKNOWN_BUG );
    }


    /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
     *    i.e. remove a constraint or bound if linear dependence occurs. */
    if ( ensureLI == BT_TRUE )
    {
        returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );

        switch ( ensureLIreturnvalue )
        {
            case SUCCESSFUL_RETURN:
                break;

            case RET_LI_RESOLVED:
                break;

            case RET_ENSURELI_FAILED_NOINDEX:
                return RET_ADDBOUND_FAILED_INFEASIBILITY;

            case RET_ENSURELI_FAILED_CYCLING:
                return RET_ADDBOUND_FAILED_INFEASIBILITY;

            case RET_ENSURELI_DROPPED:
                return SUCCESSFUL_RETURN;

            default:
                return THROWERROR( RET_ENSURELI_FAILED );
        }
    }

    /* II) UPDATE INDICES */
    tabularOutput.idxAddB = number;
    if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_ADDBOUND_FAILED );

    /* Also update the Schur complement. */

    /* First check if this variable had been freed before. In that
       case delete this variable from the Schur complement. */
    bool found = false;
    for ( int_t i=0; i<nS; i++ )
    {
        if ( schurUpdate[i] == SUT_VarFreed && number == schurUpdateIndex[i] )
        {
            if ( deleteFromSchurComplement( i ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ADDBOUND_FAILED );
            found = true;
            idxDeleted = i;
            break;
        }
    }
    if ( !found )
    {
        if ( nS < 0 || nS==nSmax )
        {
            /* The schur complement has become too large, reset. */
            /* Correct inertia if necessary. */
            returnValue retval = resetSchurComplement( BT_TRUE );
            if ( retval != SUCCESSFUL_RETURN )
            {
                if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                    MyPrintf( "In addBound: KKT matrix singular when resetting Schur complement\n" );
                else if ( options.printLevel == PL_HIGH )
                    MyPrintf( "In addBound, resetSchurComplement failed with retval = %d\n", retval);
                return THROWERROR( RET_ADDBOUND_FAILED );
            }
            found = true;
        }
        else
        {
            /* If the variable was not yet in Schur complement, add it now. */
            int_t nFRStart = boundsFreeStart.getLength();
            int_t* FR_idxStart;
            boundsFreeStart.getNumberArray( &FR_idxStart );
            for ( int_t i=0; i<nFRStart; i++ )
                if ( FR_idxStart[i] == number )
                {
                    real_t one = 1.0;
                    sparse_int_t pos = i;
                    if ( addToSchurComplement( number, SUT_VarFixed, 1, &pos, &one, 0, 0, 0, 0.0 ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_ADDBOUND_FAILED );
                    found = true;
                    break;
                }
        }
    }

    if ( !found )
        return THROWERROR( RET_ADDBOUND_FAILED );

    updateSchurQR( idxDeleted );

    /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
    if( rcondS < options.rcondSMin )
    {
        returnValue retval = resetSchurComplement( BT_TRUE );
        if ( retval != SUCCESSFUL_RETURN )
        {
            if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                MyPrintf( "In addBound: KKT matrix singular when resetting Schur complement\n" );
            else if ( options.printLevel == PL_HIGH )
                MyPrintf( "In addBound, resetSchurComplement failed with retval = %d\n", retval);
            return THROWERROR( RET_ADDCONSTRAINT_FAILED );
        }
    }

    return SUCCESSFUL_RETURN;
}


/*
 *  a d d B o u n d _ c h e c k L I
 */
returnValue SQProblemSchur::addBound_checkLI( int_t number )
{
    /* Get space for the multipliers xi in linear independence test */
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    real_t *xiC = new real_t[nAC];
    real_t *xiB = new real_t[nFX];

    /* I) Check if new constraint is linearly independent from the active ones. */
    returnValue returnvalueCheckLI = addBound_checkLISchur( number, xiC, xiB );

    delete [] xiB;
    delete [] xiC;

    return returnvalueCheckLI;
}

/*
 *  a d d B o u n d _ c h e c k L I S c h u r
 */
returnValue SQProblemSchur::addBound_checkLISchur( int_t number, real_t* xiC, real_t* xiB )
{
    returnValue returnvalue = RET_LINEARLY_DEPENDENT;


    int_t ii;
    int_t nV  = getNV( );
    int_t nFR = getNFR( );
    int_t nC  = getNC( );
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    int_t *FR_idx;

    bounds.getFree( )->getNumberArray( &FR_idx );

    /* For the Schur complement version we only use options.enableFullLITests = TRUE */
    {
        /*
         * expensive LI test. Backsolve with refinement using special right
         * hand side. This gives an estimate for what should be considered
         * "zero". We then check linear independence relative to this estimate.
         */

        real_t *delta_g   = new real_t[nV];
        real_t *delta_xFX = new real_t[nFX];
        real_t *delta_xFR = new real_t[nFR];
        real_t *delta_yAC = xiC;
        real_t *delta_yFX = xiB;

        for (ii = 0; ii < nV; ++ii)
            delta_g[ii] = 0.0;
        delta_g[number] = 1.0;

        int_t dim = (nC>nV)?nC:nV;
        real_t *nul = new real_t[dim];
        for (ii = 0; ii < dim; ++ii)
            nul[ii]=0.0;

        returnValue dsdReturnValue = determineStepDirection (
                delta_g, nul, nul, nul, nul, BT_FALSE, BT_FALSE,
                delta_xFX, delta_xFR, delta_yAC, delta_yFX);
        if (dsdReturnValue != SUCCESSFUL_RETURN)
            returnvalue = dsdReturnValue;

        /* compute the weight in inf-norm */
        real_t weight = 0.0;
        for (ii = 0; ii < nAC; ++ii)
        {
            real_t a = getAbs (delta_yAC[ii]);
            if (weight < a) weight = a;
        }
        for (ii = 0; ii < nFX; ++ii)
        {
            real_t a = getAbs (delta_yFX[ii]);
            if (weight < a) weight = a;
        }

        /* look at the "zero" in a relative inf-norm */
        real_t zero = 0.0;
        for (ii = 0; ii < nFX; ++ii)
        {
            real_t a = getAbs (delta_xFX[ii]);
            if (zero < a) zero = a;
        }
        for (ii = 0; ii < nFR; ++ii)
        {
            real_t a = getAbs (delta_xFR[ii]);
            if (zero < a) zero = a;
        }

        /* relative test against zero in inf-norm */
        if (zero > options.epsLITests * weight)
            returnvalue = RET_LINEARLY_INDEPENDENT;

        delete[] nul;
        delete[] delta_xFR;
        delete[] delta_xFX;
        delete[] delta_g;

    }
    return THROWINFO( returnvalue );
}


/*
 *  a d d B o u n d _ e n s u r e L I
 */
returnValue SQProblemSchur::addBound_ensureLI( int_t number, SubjectToStatus B_status )
{
    /* Get space for the multipliers xi in linear independence test */
    int_t nAC = getNAC();
    int_t nFX = getNFX();
    real_t *xiC = new real_t[nAC];
    real_t *xiB = new real_t[nFX];

    /* I) Check if new constraint is linearly independent from the active ones. */
    returnValue returnvalueCheckLI = addBound_checkLISchur( number, xiC, xiB );

    if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
    {
        delete [] xiB;
        delete [] xiC;
        return THROWERROR( RET_ENSURELI_FAILED );
    }

    if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
    {
        delete [] xiB;
        delete [] xiC;
        return SUCCESSFUL_RETURN;
    }

    /* II) NEW BOUND IS LINEARLY DEPENDENT: */
    /* 1) Coefficients of linear combination, have already been computed, but we need to correct the sign.  */
    int_t i, ii;

    if ( B_status != ST_LOWER )
    {
        for( i=0; i<nAC; ++i )
            xiC[i] = -xiC[i];
        for( i=0; i<nFX; ++i )
            xiB[i] = -xiB[i];
    }

    int_t nV  = getNV( );

    int_t* FX_idx;
    bounds.getFixed( )->getNumberArray( &FX_idx );

    int_t* AC_idx;
    constraints.getActive( )->getNumberArray( &AC_idx );

    real_t* num = new real_t[nV];

    real_t y_min = options.maxDualJump;
    int_t y_min_number = -1;
    int_t y_min_number_bound = -1;
    BooleanType y_min_isBound = BT_FALSE;

    returnValue returnvalue = SUCCESSFUL_RETURN;

    /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */

    /* 1) Constraints. */
    for( i=0; i<nAC; ++i )
    {
        ii = AC_idx[i];
        num[i] = y[nV+ii];
    }

    performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsNum, options.epsDen, y_min,y_min_number );

    /* 2) Bounds. */
    for( i=0; i<nFX; ++i )
    {
        ii = FX_idx[i];
        num[i] = y[ii];
    }

    performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsNum, options.epsDen, y_min,y_min_number_bound );

    if ( y_min_number_bound >= 0 )
    {
        y_min_number = y_min_number_bound;
        y_min_isBound = BT_TRUE;
    }

    /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
    char messageString[80];

    if ( y_min_number >= 0 )
    {
        /* Update Lagrange multiplier... */
        for( i=0; i<nAC; ++i )
        {
            ii = AC_idx[i];
            y[nV+ii] -= y_min * xiC[i];
        }
        for( i=0; i<nFX; ++i )
        {
            ii = FX_idx[i];
            y[ii] -= y_min * xiB[i];
        }

        /* ... also for newly active bound ... */
        if ( B_status == ST_LOWER )
            y[number] = y_min;
        else
            y[number] = -y_min;

        /* ... and for bound to be removed. */
        if ( y_min_isBound == BT_TRUE )
        {
            #ifndef __XPCTARGET__
            snprintf( messageString,80,"bound no. %d.",(int)y_min_number );
            getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
            #endif

            if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
            {
                returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
                goto farewell;
            }
            tabularOutput.excRemB = 1;

            y[y_min_number] = 0.0;
        }
        else
        {
            #ifndef __XPCTARGET__
            snprintf( messageString,80,"constraint no. %d.",(int)y_min_number );
            getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
            #endif

            if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
            {
                returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
                goto farewell;
            }
            tabularOutput.excRemC = 1;

            y[nV+y_min_number] = 0.0;
        }
    }
    else
    {
        if (options.enableDropInfeasibles == BT_TRUE) {
            /* dropping of infeasible constraints according to drop priorities */
            returnvalue = dropInfeasibles (number, B_status, BT_TRUE, xiB, xiC);
        }
        else
        {
            /* no constraint/bound can be removed => QP is infeasible! */
            returnvalue = RET_ENSURELI_FAILED_NOINDEX;
            setInfeasibilityFlag( returnvalue );
        }
    }

farewell:
    delete[] num;
    delete[] xiB;
    delete[] xiC;

    getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );

    return (returnvalue != SUCCESSFUL_RETURN) ? THROWERROR (returnvalue) : returnvalue;
}



/*
 *  r e m o v e C o n s t r a i n t
 */
returnValue SQProblemSchur::removeConstraint(   int_t number,
                                                BooleanType updateCholesky,
                                                BooleanType allowFlipping,
                                                BooleanType ensureNZC
                                                )
{
    returnValue returnvalue = SUCCESSFUL_RETURN;

    int_t sModType = 0;
    int_t idxDeleted = -1;
    SubjectToStatus oldStatus;
    real_t oldDet, newDet;

    /* consistency check */
    if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
         ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
         ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
         ( getStatus( ) == QPS_SOLVED )            )
    {
        return THROWERROR( RET_UNKNOWN_BUG );
    }

    /* some definitions */
    int_t nAC = getNAC( );
    int_t number_idx = constraints.getActive( )->getIndex( number );

    int_t addIdx;
    BooleanType addBoundNotConstraint;
    SubjectToStatus addStatus;
    BooleanType exchangeHappened = BT_FALSE;


    /* consistency checks */
    if ( constraints.getStatus( number ) == ST_INACTIVE )
        return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );

    if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
        return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );

    /* N) PERFORM ZERO CURVATURE TEST. */
    if (ensureNZC == BT_TRUE)
    {
        returnvalue = ensureNonzeroCurvature(BT_FALSE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);

        if (returnvalue != SUCCESSFUL_RETURN)
            return returnvalue;
    }

    /* save old constraint status and determinant of old S for flipping strategy */
    oldStatus = constraints.getStatus( number );
    oldDet = detS;

    /* I) UPDATE INDICES */
    tabularOutput.idxRemC = number;
    if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );

    /* Also update the Schur complement. */

    /* First check if this constraint had been added before. In that
       case delete this constraint from the Schur complement. */
    bool found = false;
    for ( int_t i=0; i<nS; i++ )
    {
        if ( schurUpdate[i] == SUT_ConAdded && number == schurUpdateIndex[i] )
        {
            if ( deleteFromSchurComplement( i, BT_TRUE ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
            found = true;
            idxDeleted = i;
            sModType = 2;
            break;
        }
    }
    if ( !found )
    {
        if ( nS < 0 || nS==nSmax )
        {
            /* The schur complement has become too large, reset. */
            /* Don't check inertia here, may be corrected later! */
            returnValue retval = resetSchurComplement( BT_FALSE );
            if ( retval != SUCCESSFUL_RETURN )
            {
                if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                    MyPrintf( "In removeConstraint: KKT matrix singular when resetting Schur complement\n" );
                else if ( options.printLevel == PL_HIGH )
                    MyPrintf( "In removeConstraint, resetSchurComplement failed with retval = %d\n", retval);
                return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
            }
            found = true;
            sModType = 3;
        }
        else
        {
            /* If the constraint was not yet in Schur complement, add it now. */
            int_t nFRStart = boundsFreeStart.getLength();
            int_t nACStart = constraintsActiveStart.getLength();
            int_t* AC_idxStart;
            constraintsActiveStart.getNumberArray( &AC_idxStart );

            for ( int_t i=0; i<nACStart; i++ )
                if ( AC_idxStart[i] == number )
                {
                    real_t one = 1.0;
                    sparse_int_t pos = nFRStart+i;
                    if ( addToSchurComplement( number, SUT_ConRemoved, 1, &pos, &one, 0, 0, 0, 0.0 ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
                    found = true;
                    break;
                }

            sModType = 1;
        }
    }

    if ( !found )
        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );

    /* Now we have a new Schur complement (might be smaller, larger, or empty). Update QR factorization. */

    /* Flipping bounds strategy */
    if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
    {
        if ( sModType == 1 )
        {/* Case 1: We added a row and column to S. */

            /* Check if a direction of negative curvature showed up, i.e. determinants have THE SAME sign */
            newDet = calcDetSchur( idxDeleted );

            if ( oldDet * newDet > 0 )
            {
                hessianType = HST_SEMIDEF;

                /* Restore old S */
                nS--;

                /* Flip bounds */
                tabularOutput.idxAddC = number;
                tabularOutput.excAddC = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        constraints.moveInactiveToActive( number, ST_UPPER );
                        ubA[number] = lbA[number];
                        Ax_l[number] = -Ax_u[number];
                        break;
                    case ST_UPPER:
                        constraints.moveInactiveToActive( number, ST_LOWER );
                        lbA[number] = ubA[number];
                        Ax_u[number] = -Ax_l[number];
                        break;
                    default:
                        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
                }
            }
            else
            {/* Determinants have the correct sign, compute QR of new (larger) S */
                updateSchurQR( idxDeleted );
            }
        }
        else if ( sModType == 2 )
        {/* Case 2: We deleted a row and column of S. */

            /* Check if a direction of negative curvature showed up, i.e. determinants have DIFFERENT signs */
            newDet = calcDetSchur( idxDeleted );

            if ( oldDet * newDet < 0.0 )
            {
                hessianType = HST_SEMIDEF;

                /* Restore old S */
                undoDeleteFromSchurComplement( idxDeleted );

                /* Flip bounds */
                tabularOutput.idxAddC = number;
                tabularOutput.excAddC = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        constraints.moveInactiveToActive( number, ST_UPPER );
                        ubA[number] = lbA[number];
                        Ax_l[number] = -Ax_u[number];
                        break;
                    case ST_UPPER:
                        constraints.moveInactiveToActive( number, ST_LOWER );
                        lbA[number] = ubA[number];
                        Ax_u[number] = -Ax_l[number];
                        break;
                    default:
                        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
                }
            }
            else
            {/* Determinants have the correct sign, compute QR of new (smaller) S */
                updateSchurQR( idxDeleted );
            }
        }
        else if ( sModType == 3 )
        {/* Case 3: S was reset. */

            /* Check inertia of new factorization given by the sparse solver: must be ( nFR, nAC, 0 ) */
            int_t neig = sparseSolver->getNegativeEigenvalues( );
            if( neig > getNAC( ) ) // Wrong inertia!
            {
                /* Flip bounds and update Schur complement */
                tabularOutput.idxAddC = number;
                tabularOutput.excAddC = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        ubA[number] = lbA[number];
                        Ax_l[number] = -Ax_u[number];
                        addConstraint( number, ST_UPPER, BT_TRUE, BT_FALSE );
                        break;
                    case ST_UPPER:
                        lbA[number] = ubA[number];
                        Ax_u[number] = -Ax_l[number];
                        addConstraint( number, ST_LOWER, BT_TRUE, BT_FALSE );
                        break;
                    default:
                        return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
                }
            }

            /* Check if flipping deleted the negative eigenvalue */
            if( correctInertia( ) )
                return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
        }
        else
        {/* None of the three cases happened */
            return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
        }
    }
    else
    {/* No flipping strategy, update QR factorization of S */
        updateSchurQR( idxDeleted );
    }

    /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
    if( rcondS < options.rcondSMin )
    {
        returnValue retval = resetSchurComplement( BT_TRUE );
        if ( retval != SUCCESSFUL_RETURN )
        {
            if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                MyPrintf( "In removeConstraint: KKT matrix singular when resetting Schur complement\n" );
            else if ( options.printLevel == PL_HIGH )
                MyPrintf( "In removeConstraint, resetSchurComplement failed with retval = %d\n", retval);
            return THROWERROR( RET_ADDCONSTRAINT_FAILED );
        }
    }

    if ( exchangeHappened == BT_TRUE )
    {
        /* add bound or constraint */

        if ( addBoundNotConstraint )
        {
            addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
            tabularOutput.excAddB = 1;
        }
        else
        {
            addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
            tabularOutput.excAddC = 1;
        }
    }

    return SUCCESSFUL_RETURN;
}


/*
 *  r e m o v e B o u n d
 */
returnValue SQProblemSchur::removeBound(    int_t number,
                                            BooleanType updateCholesky,
                                            BooleanType allowFlipping,
                                            BooleanType ensureNZC
                                            )
{
    returnValue returnvalue = SUCCESSFUL_RETURN;
    int_t addIdx;
    BooleanType addBoundNotConstraint;
    SubjectToStatus addStatus;
    BooleanType exchangeHappened = BT_FALSE;

    int_t sModType = 0;
    int_t idxDeleted = -1;
    SubjectToStatus oldStatus;
    real_t oldDet, newDet;

    /* consistency checks */
    if ( bounds.getStatus( number ) == ST_INACTIVE )
        return THROWERROR( RET_BOUND_NOT_ACTIVE );

    if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
         ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
         ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
         ( getStatus( ) == QPS_SOLVED )           )
    {
        return THROWERROR( RET_UNKNOWN_BUG );
    }

    /* N) PERFORM ZERO CURVATURE TEST. */
    if (ensureNZC == BT_TRUE)
    {
        returnvalue = ensureNonzeroCurvature(BT_TRUE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);

        if (returnvalue != SUCCESSFUL_RETURN)
            return returnvalue;
    }

    /* save old bound status and determinant of old S for flipping strategy */
    oldStatus = bounds.getStatus( number );
    oldDet = detS;

    /* I) UPDATE INDICES */
    tabularOutput.idxRemB = number;
    if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
        return THROWERROR( RET_REMOVEBOUND_FAILED );

    /* Also update the Schur complement. */

    /* First check if this variable had been fixed before. In that
       case delete this variable from the Schur complement. */
    bool found = false;
    for ( int_t i=0; i<nS; i++ )
    {
        if ( schurUpdate[i] == SUT_VarFixed && number == schurUpdateIndex[i] )
        {
            if ( deleteFromSchurComplement( i, BT_TRUE ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_REMOVEBOUND_FAILED );
            found = true;
            idxDeleted = i;
            sModType = 2;
            break;
        }
    }
    if ( !found )
    {
        if ( nS < 0 || nS==nSmax )
        {
            /* The schur complement has become too large, reset. */
            /* Don't correct inertia here, may be corrected by flipping bounds! */
            returnValue retval = resetSchurComplement( BT_FALSE );
            if ( retval != SUCCESSFUL_RETURN )
            {
                if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH  )
                    MyPrintf( "In removeBound: KKT matrix singular when resetting Schur complement\n" );
                else if ( options.printLevel == PL_HIGH )
                    MyPrintf( "In removeBound, resetSchurComplement failed with retval = %d\n", retval);
                return THROWERROR( RET_REMOVEBOUND_FAILED );
            }
            found = true;
            sModType = 3;
        }
        else
        {
            /* If the variable was not yet in Schur complement, add it now. */
            int_t nFRStart = boundsFreeStart.getLength();
            int_t nACStart = constraintsActiveStart.getLength();
            int_t* FR_idxStart;
            boundsFreeStart.getNumberArray( &FR_idxStart );
            int_t* AC_idxStart;
            constraintsActiveStart.getNumberArray( &AC_idxStart );

            int_t numNonzerosM = 0;
            sparse_int_t* Mpos = new sparse_int_t[nFRStart+nACStart+nS]; // This is an overestimate
            real_t* Mvals = new real_t[nFRStart+nACStart+nS];
            int_t numNonzerosN = 0;
            sparse_int_t* Npos = new sparse_int_t[nFRStart+nACStart+nS]; // This is an overestimate
            real_t* Nvals = new real_t[nFRStart+nACStart+nS];
            real_t N_diag;

            int_t* irn = new int_t[nFRStart+nACStart+nS+1];
            int_t* jcn = new int_t[nFRStart+nACStart+nS+1];
            real_t* vals = new real_t[nFRStart+nACStart+nS+1];
            int_t iLength;
            int_t* iNumber = new int_t[nFRStart+nACStart+nS+1];
            int_t numNonzeros;
            int_t* iSIdx = new int_t[nS];

            /* First the Hessian part. */
            real_t regularisation = options.epsRegularisation;
            switch ( hessianType )
            {
                case HST_ZERO:
                    N_diag = regularisation;
                    break;

                case HST_IDENTITY:
                    N_diag = 1.0 + regularisation;
                    break;

                default:
                    N_diag = regularisation;
                    for ( int_t i=0; i<nFRStart; i++ )
                        iNumber[i] = FR_idxStart[i];
                    iLength = nFRStart;
                    for ( int_t i=0; i<nS; i++ )
                        if ( schurUpdate[i] == SUT_VarFreed )
                        {
                            iNumber[iLength] = schurUpdateIndex[i];
                            iSIdx[iLength-nFRStart] = i;
                            iLength++;
                        }
                    iNumber[iLength++] = number;

                    H->getSparseSubmatrix( iLength, iNumber, 1, &number, 0, 0, numNonzeros, irn, jcn, vals );

                    for ( int_t i=0; i<numNonzeros; i++ )
                    {
                        if ( irn[i] < nFRStart )
                        {
                            Mpos[numNonzerosM] = irn[i];
                            Mvals[numNonzerosM] = vals[i];
                            numNonzerosM++;
                        }
                        else if ( irn[i] != iLength-1 )
                        {
                            Npos[numNonzerosN] = iSIdx[irn[i] - nFRStart];
                            Nvals[numNonzerosN] = vals[i];
                            numNonzerosN++;
                        }
                        else
                            N_diag += vals[i];
                    }
                    break;
            }

            if ( constraintProduct != 0 )
            {
                MyPrintf( "In SQProblemSchur::removeBound, constraintProduct not yet implemented.\n");
                return THROWERROR(RET_NOT_YET_IMPLEMENTED);
            }

            for ( int_t i=0; i<nACStart; i++ )
                iNumber[i] = AC_idxStart[i];
            iLength = nACStart;
            for ( int_t i=0; i<nS; i++ )
                if ( schurUpdate[i] == SUT_ConAdded )
                {
                    iNumber[iLength] = schurUpdateIndex[i];
                    iSIdx[iLength-nACStart] = i;
                    iLength++;
                }

            A->getSparseSubmatrix( iLength, iNumber, 1, &number, 0, 0, numNonzeros, irn, jcn, vals );

            for ( int_t i=0; i<numNonzeros; i++ )
            {
                if ( irn[i] < nACStart )
                {
                    Mpos[numNonzerosM] = irn[i] + nFRStart;
                    Mvals[numNonzerosM] = vals[i];
                    numNonzerosM++;
                }
                else
                {
                    Npos[numNonzerosN] = iSIdx[irn[i] - nACStart];
                    Nvals[numNonzerosN] = vals[i];
                    numNonzerosN++;
                }
            }

            delete [] iSIdx;
            delete [] iNumber;
            delete [] vals;
            delete [] jcn;
            delete [] irn;

            returnvalue = addToSchurComplement( number, SUT_VarFreed, numNonzerosM, Mpos, Mvals, numNonzerosN, Npos, Nvals, N_diag );

            delete [] Mvals;
            delete [] Mpos;
            delete [] Nvals;
            delete [] Npos;

            if ( returnvalue != SUCCESSFUL_RETURN )
                return THROWERROR( RET_REMOVEBOUND_FAILED );

            found = true;
            sModType = 1;
        }
    }

    if ( !found )
        return THROWERROR( RET_REMOVEBOUND_FAILED );

    /* Now we have a new Schur complement (might be smaller, larger, or empty). Update QR factorization. */

    /* Flipping bounds strategy */
    if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
    {
        if ( sModType == 1 )
        {/* Case 1: We added a row and column to S. */

            /* Check if a direction of negative curvature showed up, i.e. determinants have THE SAME sign */
            newDet = calcDetSchur( idxDeleted );

            if ( oldDet * newDet > 0.0 )
            {
                hessianType = HST_SEMIDEF;

                /* Restore old S */
                nS--;

                /* Flip bounds */
                tabularOutput.idxAddB = number;
                tabularOutput.excAddB = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        bounds.moveFreeToFixed( number, ST_UPPER );
                        ub[number] = lb[number];
                        break;
                    case ST_UPPER:
                        bounds.moveFreeToFixed( number, ST_LOWER );
                        lb[number] = ub[number];
                        break;
                    default:
                        return THROWERROR( RET_MOVING_BOUND_FAILED );
                }
            }
            else
            {/* Determinants have the correct sign, compute QR of new (larger) S */
                updateSchurQR( idxDeleted );
            }
        }
        else if ( sModType == 2 )
        {/* Case 2: We deleted a row and column of S. */

            /* Check if a direction of negative curvature showed up, i.e. determinants have DIFFERENT signs */
            newDet = calcDetSchur( idxDeleted );

            if ( oldDet * newDet < 0.0 )
            {
                hessianType = HST_SEMIDEF;

                /* Restore old S */
                undoDeleteFromSchurComplement( idxDeleted );

                /* Flip bounds */
                tabularOutput.idxAddB = number;
                tabularOutput.excAddB = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        bounds.moveFreeToFixed( number, ST_UPPER );
                        ub[number] = lb[number];
                        break;
                    case ST_UPPER:
                        bounds.moveFreeToFixed( number, ST_LOWER );
                        lb[number] = ub[number];
                        break;
                    default:
                        return THROWERROR( RET_MOVING_BOUND_FAILED );
                }
            }
            else
            {/* Determinants have the correct sign, compute QR of new (smaller) S */
                updateSchurQR( idxDeleted );
            }
        }
        else if ( sModType == 3 )
        {/* Case 3: S was reset. */

            /* Check inertia of new factorization given by the sparse solver: must be ( nFR, nAC, 0 ) */
            int_t neig = sparseSolver->getNegativeEigenvalues( );
            if( neig > getNAC( ) ) // Wrong inertia, flip bounds!
            {
                /* Flip bounds and update Schur complement */
                tabularOutput.idxAddB = number;
                tabularOutput.excAddB = 2;
                switch ( oldStatus )
                {
                    case ST_LOWER:
                        ub[number] = lb[number];
                        addBound( number, ST_UPPER, BT_TRUE, BT_FALSE );
                        break;
                    case ST_UPPER:
                        lb[number] = ub[number];
                        addBound( number, ST_LOWER, BT_TRUE, BT_FALSE );
                        break;
                    default:
                        return THROWERROR( RET_MOVING_BOUND_FAILED );
                }
            }

            /* Check if flipping deleted the negative eigenvalue */
            if( correctInertia( ) )
                return THROWERROR( RET_REMOVEBOUND_FAILED );
        }
        else
        {/* None of the three cases happened */
            return THROWERROR( RET_REMOVEBOUND_FAILED );
        }
    }
    else
    {/* No flipping strategy, update QR factorization of S */
        updateSchurQR( idxDeleted );
    }

    /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
    if( rcondS < options.rcondSMin )
    {
        returnValue retval = resetSchurComplement( BT_TRUE );
        if ( retval != SUCCESSFUL_RETURN )
        {
            if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
                MyPrintf( "In removeBound: KKT matrix singular when resetting Schur complement\n" );
            else if ( options.printLevel == PL_HIGH )
                MyPrintf( "In removeBound, resetSchurComplement failed with retval = %d\n", retval);
            return THROWERROR( RET_ADDCONSTRAINT_FAILED );
        }
    }

    if ( exchangeHappened == BT_TRUE )
    {
        /* add bound or constraint */

        if ( addBoundNotConstraint )
        {
            addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
            tabularOutput.excAddB = 1;
        }
        else
        {
            addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
            tabularOutput.excAddC = 1;
        }
    }

    return SUCCESSFUL_RETURN;
}


/*
 *  s e t u p T Q f a c t o r i s a t i o n
 */
returnValue SQProblemSchur::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) const
{
    return THROWERROR( RET_UNKNOWN_BUG );
}


/*
 *  b a c k s o l v e R
 */
returnValue SQProblemSchur::backsolveR( const real_t* const b, BooleanType transposed, real_t* const a  ) const
{
    return THROWERROR( RET_UNKNOWN_BUG );
}


/*
 *  b a c k s o l v e R
 */
returnValue SQProblemSchur::backsolveR( const real_t* const b, BooleanType transposed, BooleanType removingBound, real_t* const a ) const
{
    return THROWERROR( RET_UNKNOWN_BUG );
}


/*
 *  c a l c D e t S c h u r
 */
real_t SQProblemSchur::calcDetSchur( int_t idxDel )
{
    if ( nS <= 0 )
        return 1.0;

    real_t newDet;
    int_t i, j;
    real_t c, s, nu;

    /* Case 1: S has been bordered by one row and column */
    if( idxDel < 0 )
    {
        /* Do a solve with the old S to check determinant of new (bordered) S */
        real_t *temp1 = new real_t[nS-1];
        real_t *temp2 = new real_t[nS-1];
        for( i=0; i<nS-1; i++ )
            temp1[i] = S[i + (nS-1)*nSmax];
        backsolveSchurQR( nS-1, temp1, 1, temp2 );

        newDet = S[(nS-1) + (nS-1)*nSmax];
        for( i=0; i<nS-1; i++ )
            newDet -= temp1[i]*temp2[i];
        newDet *= detS;
        delete [] temp1;
        delete [] temp2;
    }
    /* Case 2: row and column idxDel have been deleted from S */
    else
    {
        const int_t dim = nS+1;
        real_t *tempR = new real_t[dim*(dim-1)];
        real_t *tempColQ = new real_t[dim];

        /* Copy current R without column idxDel*/
        for( j=0; j<idxDel; j++ )
            for( i=0; i<dim; i++ )
                tempR[i+j*dim] = R_[i+j*nSmax];
        for( j=idxDel; j<dim-1; j++ )
            for( i=0; i<dim; i++ )
                tempR[i+j*dim] = R_[i+(j+1)*nSmax];
        /* Copy row idxDel of Q */
        for( j=0; j<dim; j++ )
            tempColQ[j] = Q_[idxDel+j*nSmax];

        /* Bring tempR to triangular form with nS-idxDel Givens rotations */
        for ( i=idxDel; i<nS; i++ )
        {
            computeGivens( tempR[i+i*dim], tempR[(i+1)+i*dim], tempR[i+i*dim], tempR[(i+1)+i*dim], c, s );
            nu = s/(1.0+c);
            /// \todo I think we do not need to transform all columns of R, i+3 or so should be sufficient
            for ( j=i+1; j<nS; j++ )
                applyGivens( c, s, nu, tempR[i+j*dim], tempR[(i+1)+j*dim], tempR[i+j*dim], tempR[(i+1)+j*dim] );

            /* Simultaneously transform relevant column of Q**T */
            applyGivens( c, s, nu, tempColQ[i], tempColQ[i+1], tempColQ[i], tempColQ[i+1] );
        }

        /* Delete row: nS Givens rotations to transform last column (and row!) of (old) Q**T to getAbs((nS+1)-th unity vector) */
        for ( i=nS; i>0; i-- )
        {
            computeGivens( tempColQ[nS], tempColQ[i-1], tempColQ[nS], tempColQ[i-1], c, s );
            nu = s/(1.0+c);

            /* Simultaneously transform diagonal elements of R (coldim is already one less than Q) */
            applyGivens( c, s, nu, tempR[nS+(i-1)*dim], tempR[(i-1)+(i-1)*dim], tempR[nS+(i-1)*dim], tempR[(i-1)+(i-1)*dim] );
        }

        /* Note that we implicitly did a row permutation of Q.
         * If we did an  odd permutation AND deleted a positive unity vector or
         * if we did an even permutation AND deleted a negative unity vector, then det(Q)=-1
         * ->Change signs of first column of Q and first row of R */
        if ( (( (nS - idxDel) % 2 == 1 ) && ( tempColQ[nS] > 0.0 )) ||
            (( (nS - idxDel) % 2 == 0 ) && ( tempColQ[nS] < 0.0 )) )
        {
            tempR[0] = -tempR[0];
        }

        newDet = 1.0;
        //for( i=0; i<nS; i++ )
            //newDet *= tempR[i+i*dim];
        for( i=0; i<nS; i++ )
            if( tempR[i+i*dim] < 0.0 ) newDet = -newDet;
        delete [] tempR;
        delete [] tempColQ;
    }

    return newDet;
}


/*
 *  u p d a t e S c h u r Q R
 */
returnValue SQProblemSchur::updateSchurQR( int_t idxDel )
{
    int_t i, j;
    real_t c, s, nu;

    if ( nS <= 0 )
    {
        detS = 1.0;
        rcondS = 1.0;
        return SUCCESSFUL_RETURN;
    }

    /* Case 1: S has been bordered by one row and column */
    if ( idxDel < 0 )
    {
        /* I: Augment Q**T by nS-th unity vector (row and column) */
        for ( i=0; i<nS; i++ )
        {
            Q_[i+(nS-1)*nSmax] = 0.0;
            Q_[(nS-1)+i*nSmax] = 0.0;
        }
        Q_[(nS-1)+(nS-1)*nSmax] = 1.0;

        /* IIa: Augment rows of R by last row of S */
        for ( i=0; i<nS; i++ )
            R_[(nS-1)+i*nSmax] = S[(nS-1)+i*nSmax];

        /* IIb: Augment columns of R by Q**T * S[nS,:] */
        for ( i=0; i<nS; i++ )
        {
            R_[i+(nS-1)*nSmax] = 0.0;
            for ( j=0; j<nS; j++ )
                R_[i+(nS-1)*nSmax] += Q_[j+i*nSmax] * S[j+(nS-1)*nSmax];
        }

        /* III: Restore triangular form of R by nS-1 Givens rotations */
        for ( i=0; i<nS-1; i++ )
        {
            computeGivens( R_[i+i*nSmax], R_[(nS-1)+i*nSmax], R_[i+i*nSmax], R_[(nS-1)+i*nSmax], c, s );
            nu = s/(1.0+c);
            for ( j=i+1; j<nS; j++ )
                applyGivens( c, s, nu, R_[i+j*nSmax], R_[(nS-1)+j*nSmax], R_[i+j*nSmax], R_[(nS-1)+j*nSmax] );

            /* Simultaneously transform Q**T */
            for ( j=0; j<nS; j++ )
                applyGivens( c, s, nu, Q_[j+i*nSmax], Q_[j+(nS-1)*nSmax], Q_[j+i*nSmax], Q_[j+(nS-1)*nSmax] );
        }
    }
    /* Case 2: row and column idxDel have been deleted from S */
    else
    {
        /* I: Delete column idxDel of R */
        for ( j=idxDel; j<nS; j++ )
            for ( i=0; i<nS+1; i++ )
                R_[i+j*nSmax] = R_[i+(j+1)*nSmax];

        /* II: Bring R back to triangular form with nS-idxDel Givens rotations */
        for ( i=idxDel; i<nS; i++ )
        {
            computeGivens( R_[i+i*nSmax], R_[(i+1)+i*nSmax], R_[i+i*nSmax], R_[(i+1)+i*nSmax], c, s );
            nu = s/(1.0+c);
            for ( j=i+1; j<nS; j++ )
                applyGivens( c, s, nu, R_[i+j*nSmax], R_[(i+1)+j*nSmax], R_[i+j*nSmax], R_[(i+1)+j*nSmax] );

            /* Simultaneously transform (old) Q**T (coldim is one larger)*/
            for ( j=0; j<nS+1; j++ )
                applyGivens( c, s, nu, Q_[j+i*nSmax], Q_[j+(i+1)*nSmax], Q_[j+i*nSmax], Q_[j+(i+1)*nSmax] );
        }

        /* III: Permute rows of Q: move row idxDel to position nS */
        real_t temp;
        for ( j=0; j<nS+1; j++ )
        {
            temp = Q_[idxDel+j*nSmax];
            for ( i=idxDel; i<nS; i++ )
                Q_[i+j*nSmax] = Q_[(i+1)+j*nSmax];
            Q_[nS+j*nSmax] = temp;
        }

        /* IV: Delete row: nS Givens rotations to transform last column (and row!) of (old) Q**T to getAbs((nS+1)-th unity vector) */
        for ( i=nS; i>0; i-- )
        {
            computeGivens( Q_[nS+nS*nSmax], Q_[nS+(i-1)*nSmax], Q_[nS+nS*nSmax], Q_[nS+(i-1)*nSmax], c, s );
            nu = s/(1.0+c);
            for ( j=0; j<nS; j++ )
                applyGivens( c, s, nu, Q_[j+nS*nSmax], Q_[j+(i-1)*nSmax], Q_[j+nS*nSmax], Q_[j+(i-1)*nSmax] );

            /* Simultaneously transform R (coldim is already one less than Q) */
            for ( j=i-1; j<nS; j++ )
                applyGivens( c, s, nu, R_[nS+j*nSmax], R_[(i-1)+j*nSmax], R_[nS+j*nSmax], R_[(i-1)+j*nSmax] );
        }

        /* If we did an  odd permutation AND deleted a positive unity vector or
         * if we did an even permutation AND deleted a negative unity vector, then det(Q)=-1
         * ->Change signs of first column of Q and first row of R s.t. we always maintain det(Q)=1 */
        if ( (( (nS - idxDel) % 2 == 1 ) && ( Q_[nS+nS*nSmax] > 0.0 )) ||
            (( (nS - idxDel) % 2 == 0 ) && ( Q_[nS+nS*nSmax] < 0.0 )) )
        {
            for ( i=0; i<nS+1; i++ )
                Q_[i] = -Q_[i];
            for ( i=0; i<nS; i++ )
                R_[i*nSmax] = -R_[i*nSmax];
        }
    }

    /* Compute determinant */
    detS = 1.0;
    //for ( i=0; i<nS; i++ )
        //detS *= R_[i+i*nSmax];
    for ( i=0; i<nS; i++ )
        if( R_[i+i*nSmax] < 0.0 ) detS = -detS;

    /* Estimate condition number of R (= condition number of S)*/
    real_t *WORK;
    unsigned long N = (unsigned long)nS;
    unsigned long LDA = (unsigned long)nSmax;
    unsigned long *IWORK;
    long INFO = 0;
    IWORK = new unsigned long[N];
    WORK = new real_t[3*N];
    TRCON( "1", "U", "N", &N, R_, &LDA, &rcondS, WORK, IWORK, &INFO );
    if ( INFO != 0 )
    {
        MyPrintf( "TRCON returns INFO = %d\n",(int)INFO );
    }

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "1/cond(S) = %23.16e.\n", rcondS );

    delete[] IWORK;
    delete[] WORK;

    return SUCCESSFUL_RETURN;
}


/*
 *  b a c k s o l v e S c h u r Q R
 */
returnValue SQProblemSchur::backsolveSchurQR( int_t dimS, const real_t* const rhs, int_t dimRhs, real_t* const sol )
{
    if( dimS < 1 || dimRhs < 1 )
        return SUCCESSFUL_RETURN;

    if( dimRhs > 1 )
    {
        MyPrintf("backsolve not implemented for dimRhs = %d\n", dimRhs);
        return RET_QR_FACTORISATION_FAILED;
    }

    int_t i, j;
    long INFO = 0;
    unsigned long NRHS = 1;
    unsigned long M = (unsigned long)dimS;
    unsigned long LDA = (unsigned long)nSmax;
    unsigned long LDC = (unsigned long)dimS;

    for( i=0; i<dimS; i++ )
        sol[i] = 0.0;

    /* Compute sol = Q**T * rhs */
    for( i=0; i<dimS; i++ )
        for( j=0; j<dimS; j++ )
            sol[i] += Q_[j+i*nSmax] * rhs[j];

    /* Solve Rx = sol */
    TRTRS( "U", "N", "N", &M, &NRHS, R_, &LDA, sol, &LDC, &INFO );
    if ( INFO != 0 )
    {
        MyPrintf("TRTRS returns INFO = %d\n", INFO);
        return RET_QR_FACTORISATION_FAILED;
    }

    return SUCCESSFUL_RETURN;
}


returnValue SQProblemSchur::stepCalcRhs(    int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, real_t& rhs_max,
                                            const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
                                            const real_t* const delta_lb, const real_t* const delta_ub,
                                            BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
                                            real_t* const delta_xFX, real_t* const delta_xFR,
                                            real_t* const delta_yAC, real_t* const delta_yFX
                                            )
{
    int_t i, ii;
    returnValue retval;

    if ( nS < 0 )
    {
        retval = resetSchurComplement( BT_FALSE );
        if (retval != SUCCESSFUL_RETURN)
        {
            MyPrintf( "In SQProblemSchur::stepCalcRhs, resetSchurComplement returns %d\n", retval);
            return THROWERROR( retval );
        }
    }

    /* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
     * delta_xFR, delta_yAC hold the steps that get refined */
    for ( i=0; i<nFR; ++i )
    {
        ii = FR_idx[i];
        tempA[i] = delta_g[ii];
        delta_xFR[i] = 0.0;
    }
    for ( i=0; i<nAC; ++i )
        delta_yAC[i] = 0.0;
    if ( Delta_bC_isZero == BT_FALSE )
    {
        for ( i=0; i<nAC; ++i )
        {
            ii = AC_idx[i];
            if ( constraints.getStatus( ii ) == ST_LOWER )
                tempB[i] = delta_lbA[ii];
            else
                tempB[i] = delta_ubA[ii];
        }
    }
    else
    {
        for ( i=0; i<nAC; ++i )
            tempB[i] = 0.0;
    }
    if ( ( hessianType != HST_IDENTITY ) && ( hessianType != HST_ZERO ) )
    {
        /* tempA becomes RHS for reduced augmented system, gFR+H_FX*delta_xFR */
        H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
    }
    /* tempB becomes RHS for reduced augmented system, bA-A_CX*delta_xFR */
    A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);

    /* If iterative refinement is requested, compute max-norm of RHS for termination test. */
    rhs_max = 0.0;
    if ( options.numRefinementSteps > 0 )
    {
        for ( i=0; i<nFR; i++ )
            rhs_max = getMax(rhs_max, getAbs(tempA[i]));
        for ( i=0; i<nAC; i++ )
            rhs_max = getMax(rhs_max, getAbs(tempB[i]));
    }
    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::stepCalcReorder(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart, int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart, int_t* AC_iSort, int_t* AC_iSortStart, real_t* rhs)
{
    int_t i, ii;
    /* Reorder information for the new to the old free variables. */
    i = 0;
    ii = 0;
    while ( ii < nFRStart )
    {
        if ( i == nFR )
            rhs[FR_iSortStart[ii++]] = 0.0;
        else
        {
            int_t idx = FR_idx[FR_iSort[i]];
            int_t idxStart = FR_idxStart[FR_iSortStart[ii]];

            if ( idx == idxStart )
                rhs[FR_iSortStart[ii++]] = -tempA[FR_iSort[i++]];
            else if ( idx < idxStart )
                i++;
            else
                rhs[FR_iSortStart[ii++]] = 0.0;
        }
    }
    /* Reorder information for the new to the old active constraints. */
    i = 0;
    ii = 0;
    while ( ii < nACStart )
    {
        if ( i == nAC )
            rhs[nFRStart+AC_iSortStart[ii++]] = 0.0;
        else
        {
            int_t idx = AC_idx[AC_iSort[i]];
            int_t idxStart = AC_idxStart[AC_iSortStart[ii]];

            if ( idx == idxStart )
                rhs[nFRStart+AC_iSortStart[ii++]] = tempB[AC_iSort[i++]];
            else if ( idx < idxStart )
                i++;
            else
                rhs[nFRStart+AC_iSortStart[ii++]] = 0.0;
        }
    }
    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::stepCalcBacksolveSchur( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, int_t dim, real_t* rhs, real_t* sol )
{
    returnValue retval;
    int_t i, ii;

    real_t* q = new real_t[nS];

    /* Compute extra compoments of the RHS */
    for ( ii=0; ii<nS; ii++ )
    {
        int_t idx = schurUpdateIndex[ii];
        switch ( schurUpdate[ii] ) // TODO: All the loops below could be done faster by binary search or so
        {
            case SUT_VarFixed:
                q[ii] = 0.0;
                break;

            case SUT_VarFreed:
                /* Find index of freed variable */
                for( i=0; i<nFR; ++i )
                    if ( FR_idx[i] == idx )
                    {
                        q[ii] = -tempA[i];
                        break;
                    }
                break;

            case SUT_ConAdded:
                /* Find index of added constraint */
                for( i=0; i<nAC; ++i )
                    if ( AC_idx[i] == idx )
                    {
                        q[ii] = tempB[i];
                        break;
                    }
                break;

            case SUT_ConRemoved:
                q[ii] = 0.0;
                break;

            default:
                return THROWERROR( RET_UNKNOWN_BUG );
        }
    }

    /* compute q = M^T K^{-1} r - q */
    computeMTransTimes(1.0, sol, -1.0, q);

    /* Solve linear system with Schur complement. */
    real_t* p = new real_t[nS];
    backsolveSchurQR( nS, q, 1, p );

    computeMTimes(-1.0, p, 1.0, rhs);

    retval = sparseSolver->solve(dim, rhs, sol);
    if (retval != SUCCESSFUL_RETURN)
    {
        MyPrintf( "sparseSolver->solve (second time) failed.\n");
        return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
    }

    /* Transfer extra compoments of the Schur complement solution to the correct place. */
    for ( ii=0; ii<nS; ii++ )
    {
        int_t idx = schurUpdateIndex[ii];
        switch ( schurUpdate[ii] ) // TODO: All the loops below could be done faster by binary search or so
        {
            case SUT_VarFixed:
                break;

            case SUT_VarFreed:
                /* Find index of freed variable */
                for( i=0; i<nFR; ++i )
                    if ( FR_idx[i] == idx )
                    {
                        delta_xFR_TMP[i] = p[ii];
                        break;
                    }
                break;

            case SUT_ConAdded:
                /* Find index of added constraint */
                for( i=0; i<nAC; ++i )
                    if ( AC_idx[i] == idx )
                    {
                        delta_yAC_TMP[i] = -p[ii];
                        break;
                    }
                break;

            case SUT_ConRemoved:
                break;

            default:
                return THROWERROR( RET_UNKNOWN_BUG );
        }
    }

    delete [] p;
    delete [] q;
    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::stepCalcReorder2(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart, int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart, int_t* AC_iSort, int_t* AC_iSortStart, real_t* sol, real_t* const delta_xFR, real_t* const delta_yAC)
{
    int_t i, ii;
            i = 0;
            ii = 0;
            while ( ii < nFRStart && i < nFR )
            {
                int_t idx = FR_idx[FR_iSort[i]];
                int_t idxStart = FR_idxStart[FR_iSortStart[ii]];

                if ( idx == idxStart )
                    delta_xFR_TMP[FR_iSort[i++]] = sol[FR_iSortStart[ii++]];
                else if ( idx < idxStart )
                    i++;
                else
                    ii++;
            }
            /* Transfer Schur complement solution for the active constraint multipliers to the correct places */
            i = 0;
            ii = 0;
            while ( ii < nACStart && i < nAC )
            {
                int_t idx = AC_idx[AC_iSort[i]];
                int_t idxStart = AC_idxStart[AC_iSortStart[ii]];

                if ( idx == idxStart )
                    delta_yAC_TMP[AC_iSort[i++]] = -sol[nFRStart+AC_iSortStart[ii++]];
                else if ( idx < idxStart )
                    i++;
                else
                    ii++;
            }

            /* refine the solution found so far */
            for ( i=0; i<nFR; ++i )
                delta_xFR[i] += delta_xFR_TMP[i];
            for ( i=0; i<nAC; ++i )
                delta_yAC[i] += delta_yAC_TMP[i];
    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::stepCalcResid(int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, BooleanType Delta_bC_isZero, real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA, real_t& rnrm)
{
    int_t i, ii;
                /* compute residuals in tempA and tempB, and max-norm */
                for ( i=0; i<nFR; ++i )
                {
                    ii = FR_idx[i];
                    tempA[i] = delta_g[ii];
                }

                switch ( hessianType )
                {
                    case HST_ZERO:
                        break;

                    case HST_IDENTITY:
                        for ( i=0; i<nFR; ++i )
                            tempA[i] += delta_xFR[i];
                        break;

                    default:
                        H->times(bounds.getFree(), bounds.getFree(),  1, 1.0, delta_xFR, nFR, 1.0, tempA, nFR);
                        H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
                        break;
                }

                for ( i=0; i<nFR; ++i )
                    tempA[i] += options.epsRegularisation*delta_xFR[i];

                A->transTimes(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_yAC, nAC, 1.0, tempA, nFR);
                rnrm = 0.0;
                for ( i=0; i<nFR; ++i )
                    if (rnrm < getAbs (tempA[i]))
                        rnrm = getAbs (tempA[i]);

                if (!Delta_bC_isZero)
                {
                    for ( i=0; i<nAC; ++i )
                    {
                        ii = AC_idx[i];
                        if ( constraints.getStatus( ii ) == ST_LOWER )
                            tempB[i] = delta_lbA[ii];
                        else
                            tempB[i] = delta_ubA[ii];
                    }
                }
                else
                {
                    for ( i=0; i<nAC; ++i )
                        tempB[i] = 0.0;
                }

                A->times(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_xFR, nFR, 1.0, tempB, nAC);

                A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
                for ( i=0; i<nAC; ++i )
                    if (rnrm < getAbs (tempB[i]))
                        rnrm = getAbs (tempB[i]);

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::stepCalcDeltayFx(int_t nFR, int_t nFX, int_t nAC, int_t* FX_idx, const real_t* const delta_g, real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, real_t* const delta_yFX)
{
    int_t i;
        for( i=0; i<nFX; ++i )
            delta_yFX[i] = delta_g[FX_idx[i]];

        A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);

        if ( hessianType == HST_ZERO )
        {
          // TODO: if ( usingRegularisation( ) == BT_TRUE )
                for( i=0; i<nFX; ++i )
                    delta_yFX[i] += options.epsRegularisation*delta_xFX[i];
        }
        else if ( hessianType == HST_IDENTITY )
        {
            for( i=0; i<nFX; ++i )
                delta_yFX[i] += delta_xFX[i];
        }
        else
        {
            H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
            H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
        }
    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::determineStepDirection( const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
                                                const real_t* const delta_lb, const real_t* const delta_ub,
                                                BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
                                                real_t* const delta_xFX, real_t* const delta_xFR,
                                                real_t* const delta_yAC, real_t* const delta_yFX
                                                )
{
    returnValue retval = determineStepDirection2(   delta_g, delta_lbA, delta_ubA, delta_lb, delta_ub,
                                                    Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
                                                    delta_yAC, delta_yFX
                                                    );

    if ( retval == RET_QR_FACTORISATION_FAILED )
    {
        retval = resetSchurComplement( BT_FALSE );
        if (retval != SUCCESSFUL_RETURN)
        {
            MyPrintf( "In SQProblem::determineStepDirection, resetSchurComplement returns %d\n", retval);
            return THROWERROR( retval );
        }
        retval = determineStepDirection2(   delta_g, delta_lbA, delta_ubA, delta_lb, delta_ub,
                                                    Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
                                                    delta_yAC, delta_yFX
                                                    );
    }
    return retval;
}

/*
 *  d e t e r m i n e S t e p D i r e c t i o n
 */
returnValue SQProblemSchur::determineStepDirection2(    const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
                                                const real_t* const delta_lb, const real_t* const delta_ub,
                                                BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
                                                real_t* const delta_xFX, real_t* const delta_xFR,
                                                real_t* const delta_yAC, real_t* const delta_yFX
                                                )
{
    /* The linear system to be solved here is this:

       / H_FF  H_FX  A_CF^T  0 \ /  delta_xFR \   / -delta_g_F \
       | H_XF  H_XX  A_CX^T  I | |  delta_xFX |   | -delta_g_X |
       | A_CF  A_CX    0     0 | | -delta_yAC | = |  delta_bA  |  <-- active entries of delta_lbA and delta_ubA with corresponding sign
       \  0     I      0     0 / \ -delta_yFX /   \  delta_bX  /  <-- fixed entries of delta_lb and delta_ub with corresponding sign

    */


    int_t i, ii, r;

    returnValue retval;

  //int_t nV  = getNV( );
    int_t nFR = getNFR( );
    int_t nFX = getNFX( );
    int_t nAC = getNAC( );

    int_t* FR_idx;
    int_t* FX_idx;
    int_t* AC_idx;

    bounds.getFree( )->getNumberArray( &FR_idx );
    bounds.getFixed( )->getNumberArray( &FX_idx );
    constraints.getActive( )->getNumberArray( &AC_idx );


    /* I) DETERMINE delta_xFX (this is exact, does not need refinement) */
    if ( Delta_bB_isZero == BT_FALSE )
    {
        for( i=0; i<nFX; ++i )
        {
            ii = FX_idx[i];

            if ( bounds.getStatus( ii ) == ST_LOWER )
                delta_xFX[i] = delta_lb[ii];
            else
                delta_xFX[i] = delta_ub[ii];
        }
    }
    else
    {
        for( i=0; i<nFX; ++i )
            delta_xFX[i] = 0.0;
    }

    if ( nFR+nAC>0 ) {
        real_t rhs_max = 0.0;
        retval = stepCalcRhs( nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, rhs_max, delta_g, delta_lbA, delta_ubA,
                              delta_lb, delta_ub, Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
                              delta_yAC, delta_yFX );

        if (retval != SUCCESSFUL_RETURN)
            return retval;
        int_t nFRStart = boundsFreeStart.getLength();
        int_t nACStart = constraintsActiveStart.getLength();

        int_t* FR_iSort;
        int_t* AC_iSort;
        bounds.getFree( )->getISortArray( &FR_iSort );
        constraints.getActive( )->getISortArray( &AC_iSort );

        int_t* FR_idxStart;
        int_t* AC_idxStart;
        boundsFreeStart.getNumberArray( &FR_idxStart );
        constraintsActiveStart.getNumberArray( &AC_idxStart );

        int_t* FR_iSortStart;
        int_t* AC_iSortStart;
        boundsFreeStart.getISortArray( &FR_iSortStart );
        constraintsActiveStart.getISortArray( &AC_iSortStart );

        int_t dim = nFRStart + nACStart;
        real_t* rhs = new real_t[dim];
        real_t* sol = new real_t[dim];

        /* Iterative refinement loop for delta_xFR, delta_yAC */
        for ( r=0; r<=options.numRefinementSteps; ++r )
        {
          retval = stepCalcReorder(nFR, nAC, FR_idx, AC_idx, nFRStart, nACStart, FR_idxStart, AC_idxStart, FR_iSort, FR_iSortStart, AC_iSort, AC_iSortStart, rhs);
            if (retval != SUCCESSFUL_RETURN)
                return retval;

            retval = sparseSolver->solve(dim, rhs, sol);

            if (retval != SUCCESSFUL_RETURN)
            {
                MyPrintf( "sparseSolver->solve (first time) failed.\n");
                return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
            }

            if ( nS > 0 )
            {
                retval = stepCalcBacksolveSchur( nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, dim, rhs, sol );
                if (retval != SUCCESSFUL_RETURN)
                    return retval;
            }

            /* Transfer Schur complement solution for the free variables to the correct places */
            retval = stepCalcReorder2(nFR, nAC, FR_idx, AC_idx, nFRStart, nACStart, FR_idxStart, AC_idxStart, FR_iSort, FR_iSortStart, AC_iSort, AC_iSortStart, sol, delta_xFR, delta_yAC);
            if (retval != SUCCESSFUL_RETURN)
                return retval;

            if ( r < options.numRefinementSteps ) // TODO: use "<" to avoid computation in last round
            {
                real_t rnrm;
                retval = stepCalcResid(nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, Delta_bC_isZero, delta_xFX, delta_xFR, delta_yAC, delta_g, delta_lbA, delta_ubA, rnrm);
                if (retval != SUCCESSFUL_RETURN)
                    return retval;

                /* early termination of residual norm small enough */
                if ( options.printLevel == PL_HIGH )
                    MyPrintf( "In iterative refinement (iter %d) rnrm = %e and epsIterRef*rhs_max = %e.\n", r, rnrm, options.epsIterRef*rhs_max);

                if ( rnrm <= options.epsIterRef*rhs_max )
                    break;
            }

        }

        delete [] sol;
        delete [] rhs;
    }

    /* IV) DETERMINE delta_yFX */
    if ( nFX > 0 )
    {
        retval = stepCalcDeltayFx(nFR, nFX, nAC, FX_idx, delta_g, delta_xFX, delta_xFR, delta_yAC, delta_yFX);
        if (retval != SUCCESSFUL_RETURN)
            return retval;
    }

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::resetSchurComplement( BooleanType allowInertiaCorrection )
{
    int_t j;
    int_t nFR = getNFR( );
    int_t nAC = getNAC( );

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "Resetting Schur complement.\n");

    nS = 0;
    detS = 1.0;
    rcondS = 1.0;
    boundsFreeStart = *bounds.getFree();
    constraintsActiveStart = *constraints.getActive();

    if ( nSmax > 0 )
        M_jc[0] = 0;

    int_t dim = nFR+nAC;
    // Count the number of nonzeros
    int_t numNonzeros;
    switch ( hessianType )
    {
        case HST_ZERO:
            numNonzeros = 0;
            break;

        case HST_IDENTITY:
            numNonzeros = nFR;
            break;

        default:
            H->getSparseSubmatrix( bounds.getFree(), bounds.getFree(), 1, 1, numNonzeros, 0, 0, 0, BT_TRUE);
            break;
    }
    // TODO: For now, we regularize every time
    if (options.epsRegularisation > 0.0)
        numNonzeros += nFR;

    int_t numNonzerosA;

    if ( constraintProduct != 0 )
    {
        MyPrintf( "In SQProblemSchur::determineStepDirection, constraintProduct not yet implemented.\n");
        return THROWERROR(RET_NOT_YET_IMPLEMENTED);
    }
    A->getSparseSubmatrix( constraints.getActive(), bounds.getFree(), nFR+1, 1, numNonzerosA, 0, 0, 0, BT_FALSE);
    numNonzeros += numNonzerosA;

    // Get the values
    real_t* avals = new real_t[numNonzeros];
    int_t* irn = new int_t[numNonzeros];
    int_t* jcn = new int_t[numNonzeros];
    numNonzeros = 0;
    switch ( hessianType )
    {
        case HST_ZERO:
            break;

        case HST_IDENTITY:
            numNonzeros += nFR;
            for (j = 0; j<nFR; j++)
            {
                irn[j] = j+1;
                jcn[j] = j+1;
                avals[j] = 1.0;
            }
            break;

        default:
            H->getSparseSubmatrix( bounds.getFree(), bounds.getFree(), 1, 1, numNonzeros, irn, jcn, avals, BT_TRUE);
            break;
    }

    // For now, we regularize every time
    if (options.epsRegularisation > 0.0)
    {
        for (j = 0; j<nFR; j++)
        {
            irn[numNonzeros] = j+1;
            jcn[numNonzeros] = j+1;
            avals[numNonzeros++] = options.epsRegularisation;
        }
    }

    A->getSparseSubmatrix( constraints.getActive(), bounds.getFree(), nFR+1, 1, numNonzerosA, irn+numNonzeros, jcn+numNonzeros, avals+numNonzeros, BT_FALSE);
    numNonzeros += numNonzerosA;

    // Call the linear solver
    sparseSolver->reset();
    returnValue retval = sparseSolver->setMatrixData(dim, numNonzeros, irn, jcn, avals);
    delete [] jcn;
    delete [] irn;
    delete [] avals;

    if (retval != SUCCESSFUL_RETURN)
        return THROWERROR(RET_NO_SPARSE_SOLVER);

    // Factorize the matrix for later backsolves
    retval = sparseSolver->factorize();
    numFactorizations++;

    // If matrix is singular, add bounds/remove constraints according to zero pivots
    if (retval == RET_KKT_MATRIX_SINGULAR)
    {
        if( repairSingularWorkingSet( ) == SUCCESSFUL_RETURN )
            return resetSchurComplement( allowInertiaCorrection );
        else
            return RET_KKT_MATRIX_SINGULAR;
    }

    // If matrix has wrong inertia, add bounds until inertia is correct
    if (retval == SUCCESSFUL_RETURN && allowInertiaCorrection)
    {
        int_t neig = sparseSolver->getNegativeEigenvalues( );
        if( neig > getNAC( ) )
        {
            if ( options.printLevel == PL_HIGH )
                MyPrintf( "WARNING: After new factorization, reduced Hessian has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );

            retval = correctInertia();
        }
    }

    if (retval != SUCCESSFUL_RETURN)
        return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);

    nS = 0;

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::computeMTimes( real_t alpha, const real_t* const x_, real_t beta, real_t* const y_ )
{
    if ( isEqual( alpha, -1.0 ) == BT_FALSE || isEqual( beta, 1.0 ) == BT_FALSE )
        return THROWERROR(RET_NOT_YET_IMPLEMENTED);

    int_t i, j;

    for ( j=0; j<nS; j++ )
    {
        const real_t xval = x_[j];
        for ( i=M_jc[j]; i<M_jc[j+1]; i++)
            y_[M_ir[i]] -= M_vals[i]*xval;
    }

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::computeMTransTimes( real_t alpha, const real_t* const x_, real_t beta, real_t* const y_ )
{
    if ( isEqual( alpha, 1.0 ) == BT_FALSE || ( isZero( beta ) == BT_FALSE && isEqual( beta, -1.0 ) == BT_FALSE ) )
        return THROWERROR(RET_NOT_YET_IMPLEMENTED);

    int_t i, j;

    if ( isZero( beta ) == BT_TRUE )
    {
        for ( j=0; j<nS; j++ )
        {
            y_[j] = 0.0;
            for ( i=M_jc[j]; i<M_jc[j+1]; i++)
                y_[j] += M_vals[i]*x_[M_ir[i]];
        }
    }
    else
    {
        for ( j=0; j<nS; j++ )
        {
            y_[j] = -y_[j];
            for ( i=M_jc[j]; i<M_jc[j+1]; i++)
                y_[j] += M_vals[i]*x_[M_ir[i]];
        }
    }

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::addToSchurComplement( int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t* Mpos, const real_t* const Mvals, int_t numNonzerosN, const sparse_int_t* Npos, const real_t* const Nvals, real_t N_diag )
{
    int_t i;

    int_t nFRStart = boundsFreeStart.getLength();
    int_t nACStart = constraintsActiveStart.getLength();

    real_t* new_Scol = new real_t[nS];

    int_t dim = nFRStart + nACStart;
    real_t* rhs = new real_t[dim];
    real_t* sol = new real_t[dim];

    for ( i=0; i<dim; i++ )
        rhs[i] = 0.0;

    for ( i=0; i<numNonzerosM; i++ )
        rhs[Mpos[i]] = Mvals[i];

    returnValue retval = sparseSolver->solve(dim, rhs, sol);
    if (retval != SUCCESSFUL_RETURN)
    {
        MyPrintf( "sparseSolver->solve in SQProblemSchur::addToSchurComplement failed.\n");
        return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
    }

    computeMTransTimes(1.0, sol, 0.0, new_Scol);

    /* Take care of off-diagonal elements in N. */
    for ( i=0; i<numNonzerosN; i++ )
        new_Scol[Npos[i]] -= Nvals[i];

    real_t sdiag = -N_diag;
    for ( i=0; i<numNonzerosM; i++ )
        sdiag += Mvals[i] * sol[Mpos[i]];

    /* Now augment S */
    for ( i=0; i<nS; i++)
        S[nS*nSmax + i] = new_Scol[i];
    for ( i=0; i<nS; i++)
        S[i*nSmax + nS] = new_Scol[i];
    S[nS*nSmax + nS] = sdiag;

    schurUpdateIndex[nS] = number;
    schurUpdate[nS] = update;

    /* Augment M matrix.  */
    if ( M_physicallength < M_jc[nS] + numNonzerosM )
    {
        /* If necessary, allocate more memory for M. */
        int_t M_physicallength_new = getMax(2*M_physicallength, M_physicallength + 2*numNonzerosM);
        real_t* M_vals_new = new real_t[M_physicallength_new];
        sparse_int_t* M_ir_new = new sparse_int_t[M_physicallength_new];
        memcpy( M_vals_new, M_vals, ((unsigned int)(M_jc[nS]))*sizeof(real_t) );
        memcpy( M_ir_new, M_ir, ((unsigned int)(M_jc[nS]))*sizeof(sparse_int_t) );
        M_physicallength = M_physicallength_new;
        delete [] M_vals;
        delete [] M_ir;
        M_vals = M_vals_new;
        M_ir = M_ir_new;
    }

    for ( i=0; i<numNonzerosM; i++ )
    {
        M_vals[M_jc[nS] + i] = Mvals[i];
        M_ir[M_jc[nS] + i] = Mpos[i];
    }
    M_jc[nS+1] = M_jc[nS] + numNonzerosM;

    nS++;

    delete [] sol;
    delete [] rhs;
    delete [] new_Scol;

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "added index %d with update type %d to Schur complement.  nS = %d\n", number, update, nS);

    return SUCCESSFUL_RETURN;
}


returnValue SQProblemSchur::deleteFromSchurComplement( int_t idx, BooleanType allowUndo )
{
    if ( options.printLevel == PL_HIGH )
        MyPrintf( "deleting entry %d with idx = %d and type %d from Schur complement.", idx, schurUpdateIndex[idx], schurUpdate[idx]);

    if ( idx != nS-1 )
    {
        real_t *temp_vals = NULL;
        int_t *temp_ir = NULL;
        int_t schurUpdateIndexTemp = -1;
        SchurUpdateType schurUpdateTemp = SUT_UNDEFINED;

        /* temporarily save the column of S to be deleted */
        if( allowUndo == BT_TRUE )
        {
            temp_vals = new real_t[nS];
            for ( int_t i=0; i<nS; i++ )
                temp_vals[i] = S[idx*nSmax + i];

            schurUpdateIndexTemp = schurUpdateIndex[idx];
            schurUpdateTemp = schurUpdate[idx];
        }

        /* Shift rows and columns >idx of S by one to the upper left */
        for ( int_t i=0; i<idx; i++ )
            for ( int_t j=idx+1; j<nS; j++ )
                S[i*nSmax + j-1] = S[i*nSmax + j];
        for ( int_t i=idx+1; i<nS; i++ )
        {
            for ( int_t j=0; j<idx; j++ )
                S[(i-1)*nSmax + j] = S[i*nSmax + j];
            for ( int_t j=idx+1; j<nS; j++ )
                S[(i-1)*nSmax + j-1] = S[i*nSmax + j];
        }
        for ( int_t i=idx+1; i<nS; i++ )
        {
            schurUpdateIndex[i-1] = schurUpdateIndex[i];
            schurUpdate[i-1] = schurUpdate[i];
        }

        /* Store deleted row/column in the last row/column of S, can retrieve it from there later */
        if( allowUndo == BT_TRUE )
        {
            for ( int_t i=0; i<nS; i++ )
            {
                S[(nS-1)*nSmax + i] = temp_vals[i];
                S[i*nSmax + (nS-1)] = temp_vals[i];
            }
            schurUpdateIndex[nS-1] = schurUpdateIndexTemp;
            schurUpdate[nS-1] = schurUpdateTemp;
            delete[] temp_vals;
        }

        /* temporarily save the (sparse) column of M to be deleted */
        int_t numEntries = M_jc[idx+1] - M_jc[idx];
        if( allowUndo == BT_TRUE )
        {
            temp_ir = new int_t[numEntries];
            temp_vals = new real_t[numEntries];

            for ( int_t i=M_jc[idx]; i<M_jc[idx+1]; i++ )
            {
                temp_ir[i-M_jc[idx]] = M_ir[i];
                temp_vals[i-M_jc[idx]] = M_vals[i];
            }
        }

        /* Shift all columns >idx one to the left */
        for ( int_t i=M_jc[idx+1]; i<M_jc[nS]; i++ )
        {
            M_ir[i-numEntries] = M_ir[i];
            M_vals[i-numEntries] = M_vals[i];
        }
        for ( int_t i=idx; i<nS; i++ )
            M_jc[i] = M_jc[i+1] - numEntries;

        /* Store deleted column of M in the last column, can retrieve it from there later */
        if( allowUndo == BT_TRUE )
        {
            for ( int_t i=M_jc[nS-1]; i<M_jc[nS]; i++ )
            {
                M_ir[i] = temp_ir[i-M_jc[nS-1]];
                M_vals[i] = temp_vals[i-M_jc[nS-1]];
            }

            delete[] temp_ir;
            delete[] temp_vals;
        }
    }

    nS--;

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "  nS = %d\n", nS);

    return SUCCESSFUL_RETURN;
}


returnValue SQProblemSchur::undoDeleteFromSchurComplement( int_t idx )
{
    if ( options.printLevel == PL_HIGH )
        MyPrintf( "undo deletion of entry %d with idx = %d and type %d from Schur complement. nS = %i\n", idx, schurUpdateIndex[nS-1], schurUpdate[nS-1],nS+1);

    if ( idx != nS )
    {
        real_t *temp_vals;
        int_t *temp_ir;
        int_t schurUpdateIndexTemp = -1;
        SchurUpdateType schurUpdateTemp = SUT_UNDEFINED;

        /* temporarily save the last column of S */
        temp_vals = new real_t[nS+1];
        for ( int_t i=0; i<nS+1; i++ )
            temp_vals[i] = S[i+nS*nSmax];

        schurUpdateIndexTemp = schurUpdateIndex[nS];
        schurUpdateTemp = schurUpdate[nS];

        /* Shift rows and columns =>idx of S by one to the lower right */
        for ( int_t i=idx-1; i>-1; i-- )
            for ( int_t j=nS-1; j>idx-1; j-- )
                S[(j+1)+i*nSmax] = S[j+i*nSmax];
        for ( int_t i=nS-1; i>idx-1; i-- )
        {
            for ( int_t j=idx-1; j>-1; j-- )
                S[j+(i+1)*nSmax] = S[j+i*nSmax];
            for ( int_t j=nS-1; j>idx-1; j-- )
                S[(j+1)+(i+1)*nSmax] = S[j+i*nSmax];
        }
        for ( int_t i=nS-1; i>idx-1; i-- )
        {
            schurUpdateIndex[i+1] = schurUpdateIndex[i];
            schurUpdate[i+1] = schurUpdate[i];
        }

        /* Insert stored row/column of S at position idx */
        for ( int_t i=0; i<nS+1; i++ )
        {
            S[idx*nSmax + i] = temp_vals[i];
            S[i*nSmax + idx] = temp_vals[i];
        }
        schurUpdateIndex[idx] = schurUpdateIndexTemp;
        schurUpdate[idx] = schurUpdateTemp;
        delete[] temp_vals;

        /* temporarily save the last (sparse) column of M */
        int_t numEntries = M_jc[nS+1] - M_jc[nS];
        temp_ir = new int_t[numEntries];
        temp_vals = new real_t[numEntries];
        for ( int_t i=M_jc[nS]; i<M_jc[nS+1]; i++ )
        {
            temp_ir[i-M_jc[nS]] = M_ir[i];
            temp_vals[i-M_jc[nS]] = M_vals[i];
        }

        /* Shift all columns =>idx one to the right */
        for ( int_t i=M_jc[nS]-1; i>M_jc[idx]-1; i-- )
        {
            M_ir[i+numEntries] = M_ir[i];
            M_vals[i+numEntries] = M_vals[i];
        }
        for ( int_t i=nS; i>idx-1; i-- )
            M_jc[i+1] = M_jc[i] + numEntries;

        /* Insert stored column of M at position idx */
        for ( int_t i=M_jc[idx]; i<M_jc[idx+1]; i++ )
        {
            M_ir[i] = temp_ir[i-M_jc[idx]];
            M_vals[i] = temp_vals[i-M_jc[idx]];
        }

        delete[] temp_ir;
        delete[] temp_vals;
    }

    nS++;

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "  nS = %d\n", nS);

    return SUCCESSFUL_RETURN;
}

returnValue SQProblemSchur::correctInertia( )
{
    SubjectToStatus B_status;
    real_t oldDetS;
    int_t nFR = getNFR( );
    int_t k, number, neig, nAdded;
    int_t *freeBoundIdx = new int_t[nFR];
    int_t *numberarray;

    /* method may only be called after refactorization or if one bound/constraint
     * has been added, i.e. when a bound has flipped after refactorization */
    if( nS != 0 && nS != 1 )
        return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
    neig = sparseSolver->getNegativeEigenvalues( );

    /* if a bound flipped, check if it did in fact remove a negative eigenvalue */
    if( nS == 1 && detS < 0 )
        neig--;

    /* if this method is triggered after flipping bounds, inertia is now probably correct */
    if( neig == getNAC( ) )
        return SUCCESSFUL_RETURN;

    /* get bound numbers in the order in which they are in the non-basis */
    bounds.getFree()->getNumberArray( &numberarray );
    for( k=0; k<nFR; k++ )
        freeBoundIdx[k] = numberarray[k];

    k = 0;
    nAdded = getNFR( );
    while ( neig > getNAC( ) && k < nFR )
    {
        oldDetS = detS;

        /* If it's linearly independent, fix the next free variable at the nearest bound */
        number = freeBoundIdx[k];
        if( addBound_checkLI( number ) == RET_LINEARLY_INDEPENDENT )
        {
            /* This is just heuristics: we need the bound which gives correct multiplier sign */
            if ( x[number] - lb[number] < ub[number] - x[number] )
                B_status = ST_LOWER;
            else
                B_status = ST_UPPER;

            /* Update Schur complement */
            if( addBound( number, B_status, BT_TRUE, BT_FALSE ) != SUCCESSFUL_RETURN )
            {
                if ( options.printLevel == PL_HIGH )
                    MyPrintf("In correctInertia: Adding bound[%i] = %i failed!\n", k, number );
                return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
            }

            /* Adjust bounds */
            if ( B_status == ST_LOWER )
                lb[number] = x[number];
            else
                ub[number] = x[number];
        }
        else
        {
            if ( options.printLevel == PL_HIGH )
                MyPrintf("bound[%i] = %i is linearly dependent. Do not add.\n", k, number );
            k++;
            continue;
        }

        /* Case 1: Schur complement has been reset, check inertia of new factorization */
        if( nS == 0 )
            neig = sparseSolver->getNegativeEigenvalues( );
        /* Case 2: Schur complement has grown, check if determinant changed sign */
        else if( oldDetS * detS < 0 )
            neig--;
        /* NB: Case 3: (Schur complement has shrunk) cannot happen here:
         * This method is called after a factorization reset or after ONE bound has been added */

        k++;
    }
    nAdded -= getNFR( );

    delete[] freeBoundIdx;

    /* if there are still too many negative eigenvalues, exit */
    if( neig > getNAC( ) )
    {
        if ( options.printLevel == PL_HIGH )
            MyPrintf( "Added %i bounds but KKT matrix still has %i negative eigenvalues, should be %i.\n", nAdded, neig, getNAC( ) );
        return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
    }
    else
    {
        if ( options.printLevel == PL_HIGH )
            MyPrintf( "After adding %i bounds, reduced Hessian has correct inertia.\n", nAdded, neig );
        return SUCCESSFUL_RETURN;
    }
}


returnValue SQProblemSchur::repairSingularWorkingSet( )
{
    int_t k, number;
    SubjectToStatus B_status;
    int_t rank = sparseSolver->getRank( );
    int_t nFR = getNFR( );
    int_t defect = nFR + getNAC( ) - rank;

    /* Rank detection not supported by linear solver */
    if ( rank < 0 )
        return RET_KKT_MATRIX_SINGULAR;

    /* Consistency check */
    if ( defect <= 0 )
        return RET_UNKNOWN_BUG;

    /* Determine zero pivots */
    int_t *zeroPivots = new int_t[defect];
    sparseSolver->getZeroPivots( zeroPivots );

    /* Determination of zero pivots not supported by linear solver */
    if ( zeroPivots == 0 )
        return RET_KKT_MATRIX_SINGULAR;

    /* We assume implicitly that pivots are sorted in ascending order */
    /// \todo make sure that this is so.
    /* Remove the one with the highest index first so not to mess up index lists */
    int_t bndsAdded = 0;
    for ( k=defect-1; k>-1; k-- )
    {
        /* Zero curvature in the Hessian: add a bound */
        if ( zeroPivots[k] < nFR )
        {
            number = bounds.getFree()->getNumber( zeroPivots[k] );

            if ( options.printLevel == PL_HIGH )
                MyPrintf( "WARNING: KKT matrix singular! Add bound %i before refactorization.\n", number);

            /* This is just heuristics: we need the bound which gives correct multiplier sign */
            if ( x[number] - lb[number] < ub[number] - x[number] )
                B_status = ST_LOWER;
            else
                B_status = ST_UPPER;

            /* Here we do not need to update the Schur complement because KKT matrix is factorized afterwards */
            if ( bounds.moveFreeToFixed( number, B_status ) != SUCCESSFUL_RETURN )
                return RET_ADDBOUND_FAILED;

            /* Adjust bounds */
            if ( B_status == ST_LOWER )
                lb[number] = x[number];
            else
                ub[number] = x[number];

            bndsAdded++;
        }
        /* Linearly dependent row in the Jacobian: remove a constraint */
        else
        {
            number = constraints.getActive()->getNumber( zeroPivots[k]-nFR );
            if ( options.printLevel == PL_HIGH )
                MyPrintf( "WARNING: KKT matrix singular! Removing constraint %i before refactorization.\n", number);

            if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
                return RET_REMOVECONSTRAINT_FAILED;

            // AW: If this is an equality constraint, it is now inactive and
            // will not be considered in the step computation which leads to
            // violation of that constraint in the future. Here, I try to
            // fix this by simply making this constraint no longer an
            // equality.
            // TODO: This is probably also necessary for bound constraints
            if ( constraints.getType(number) == ST_EQUALITY )
            {
                if ( options.printLevel == PL_HIGH )
                    MyPrintf( "WARNING: Making this constraint no longer an equality.\n");
                constraints.setType( number, ST_BOUNDED );
            }

            /* Adjust dual variable */
            y[number] = 0.0;
        }
    }

    if ( options.printLevel == PL_HIGH )
        MyPrintf( "WARNING: KKT matrix singular! Removed %i constraints and added %i bounds before refactorization.\n",
                    defect-bndsAdded, bndsAdded );

    delete[] zeroPivots;

    return SUCCESSFUL_RETURN;
}

END_NAMESPACE_QPOASES


/*
 *  end of file
 */
