QProblem.cpp

Go to the documentation of this file.

/*
 *      This file is part of qpOASES.
 *
 *      qpOASES -- An Implementation of the Online Active Set Strategy.
 *      Copyright (C) 2007-2008 by Hans Joachim Ferreau 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
 *
 */


#include <QProblem.hpp>

#include <stdio.h>

void printmatrix2(char *name, double *A, int m, int n) {
  int i, j;

  printf("%s = [...\n", name);
  for (i = 0; i < m; i++) {
    for (j = 0; j < n; j++)
        printf("  % 9.4f", A[i*n+j]);
    printf(",\n");
  }
  printf("];\n");
}

//#define __PERFORM_KKT_TEST__


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


/*
 *      Q P r o b l e m
 */
QProblem::QProblem( ) : QProblemB( )
{
        constraints.init( 0 );

        sizeT = 0;

        cyclingManager.init( 0,0 );
}


/*
 *      Q P r o b l e m
 */
QProblem::QProblem( int _nV, int _nC ) : QProblemB( _nV )
{
        /* consistency checks */
        if ( _nV <= 0 )
                _nV = 1;

        if ( _nC < 0 )
        {
                _nC = 0;
                THROWERROR( RET_INVALID_ARGUMENTS );
        }

        constraints.init( _nC );


        sizeT = _nC;
        if ( _nC > _nV )
                sizeT = _nV;

        cyclingManager.init( _nV,_nC );
}


/*
 *      Q P r o b l e m
 */
QProblem::QProblem( const QProblem& rhs ) :     QProblemB( rhs )
{
        int i, j;

        int _nV = rhs.bounds.getNV( );
        int _nC = rhs.constraints.getNC( );

        for( i=0; i<_nC; ++i )
                for( j=0; j<_nV; ++j )
                        A[i*NVMAX + j] = rhs.A[i*NVMAX + j];

        for( i=0; i<_nC; ++i )
                lbA[i] = rhs.lbA[i];

        for( i=0; i<_nC; ++i )
                        ubA[i] = rhs.ubA[i];

        constraints = rhs.constraints;

        for( i=0; i<(_nV+_nC); ++i )
                y[i] = rhs.y[i];


        sizeT = rhs.sizeT;

        for( i=0; i<sizeT; ++i )
                for( j=0; j<sizeT; ++j )
                        T[i*NVMAX + j] = rhs.T[i*NVMAX + j];

        for( i=0; i<_nV; ++i )
                for( j=0; j<_nV; ++j )
                        Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];

        for( i=0; i<_nC; ++i )
                Ax[i] = rhs.Ax[i];

        cyclingManager = rhs.cyclingManager;
}


/*
 *      ~ Q P r o b l e m
 */
QProblem::~QProblem( )
{
}


/*
 *      o p e r a t o r =
 */
QProblem& QProblem::operator=( const QProblem& rhs )
{
        int i, j;

        if ( this != &rhs )
        {
                QProblemB::operator=( rhs );


                int _nV = rhs.bounds.getNV( );
                int _nC = rhs.constraints.getNC( );

                for( i=0; i<_nC; ++i )
                        for( j=0; j<_nV; ++j )
                                A[i*NVMAX + j] = rhs.A[i*NVMAX + j];

                for( i=0; i<_nC; ++i )
                        lbA[i] = rhs.lbA[i];

                for( i=0; i<_nC; ++i )
                        ubA[i] = rhs.ubA[i];

                constraints = rhs.constraints;

                for( i=0; i<(_nV+_nC); ++i )
                        y[i] = rhs.y[i];


                sizeT = rhs.sizeT;

                for( i=0; i<sizeT; ++i )
                        for( j=0; j<sizeT; ++j )
                                T[i*NVMAX + j] = rhs.T[i*NVMAX + j];

                for( i=0; i<_nV; ++i )
                        for( j=0; j<_nV; ++j )
                                Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];

                for( i=0; i<_nC; ++i )
                        Ax[i] = rhs.Ax[i];

                cyclingManager = rhs.cyclingManager;
        }

        return *this;
}


/*
 *      r e s e t
 */
returnValue QProblem::reset( )
{
        int i, j;
        int nV = getNV( );
        int nC = getNC( );


        /* 1) Reset bounds, Cholesky decomposition and status flags. */
        if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_RESET_FAILED );

        /* 2) Reset constraints. */
        constraints.init( nC );

        /* 3) Reset TQ factorisation. */
        for( i=0; i<sizeT; ++i )
                for( j=0; j<sizeT; ++j )
                        T[i*NVMAX + j] = 0.0;

        for( i=0; i<nV; ++i )
                for( j=0; j<nV; ++j )
                        Q[i*NVMAX + j] = 0.0;

        /* 4) Reset cycling manager. */
        if ( cyclingManager.clearCyclingData( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_RESET_FAILED );

        return SUCCESSFUL_RETURN;
}


/*
 *      i n i t
 */
returnValue QProblem::init(     const real_t* const _H, const real_t* const _g, const real_t* const _A,
                                                        const real_t* const _lb, const real_t* const _ub,
                                                        const real_t* const _lbA, const real_t* const _ubA,
                                                        int& nWSR, const real_t* const yOpt, real_t* const cputime
                                                        )
{
        /* 1) Setup QP data. */
        if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        /* 2) Call to main initialisation routine (without any additional information). */
        return solveInitialQP( 0,yOpt,0,0, nWSR,cputime );
}


/*
 *      h o t s t a r t
 */
returnValue QProblem::hotstart( const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
                                                                const real_t* const lbA_new, const real_t* const ubA_new,
                                                                int& nWSR, real_t* const cputime
                                                                )
{
        int l;

        /* consistency check */
        if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
                 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
                 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
        {
                return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
        }

        /* start runtime measurement */
        real_t starttime = 0.0;
        if ( cputime != 0 )
                starttime = getCPUtime( );


        /* I) PREPARATIONS */
        /* 1) Reset cycling and status flags and increase QP counter. */
        cyclingManager.clearCyclingData( );

        infeasible = BT_FALSE;
        unbounded = BT_FALSE;

        ++count;

        /* 2) Allocate delta vectors of gradient and (constraints') bounds. */
        returnValue returnvalue;
        BooleanType Delta_bC_isZero, Delta_bB_isZero;

        int FR_idx[NVMAX];
        int FX_idx[NVMAX];
        int AC_idx[NCMAX_ALLOC];
        int IAC_idx[NCMAX_ALLOC];

        real_t delta_g[NVMAX];
        real_t delta_lb[NVMAX];
        real_t delta_ub[NVMAX];
        real_t delta_lbA[NCMAX_ALLOC];
        real_t delta_ubA[NCMAX_ALLOC];

        real_t delta_xFR[NVMAX];
        real_t delta_xFX[NVMAX];
        real_t delta_yAC[NCMAX_ALLOC];
        real_t delta_yFX[NVMAX];
        real_t delta_Ax[NCMAX_ALLOC];

        int BC_idx;
        SubjectToStatus BC_status;
        BooleanType BC_isBound;

        #ifdef PC_DEBUG
        char messageString[80];
        #endif


        /* II) MAIN HOMOTOPY LOOP */
        for( l=0; l<nWSR; ++l )
        {
                status = QPS_PERFORMINGHOMOTOPY;

                if ( printlevel == PL_HIGH )
                {
                        #ifdef PC_DEBUG
                        sprintf( messageString,"%d ...",l );
                        getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
                        #endif
                }

                /* 1) Setup index arrays. */
                if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_HOTSTART_FAILED );

                if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_HOTSTART_FAILED );

                if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_HOTSTART_FAILED );

                if ( constraints.getInactive( )->getNumberArray( IAC_idx ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_HOTSTART_FAILED );

                /* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */
                returnvalue = hotstart_determineDataShift(  FX_idx, AC_idx,
                                                                                                        g_new,lbA_new,ubA_new,lb_new,ub_new,
                                                                                                        delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
                                                                                                        Delta_bC_isZero, Delta_bB_isZero );
                if ( returnvalue != SUCCESSFUL_RETURN )
                {
                        nWSR = l;
                        THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
                        return returnvalue;
                }

                /* 3) Determination of step direction of X and Y. */
                returnvalue = hotstart_determineStepDirection(  FR_idx,FX_idx,AC_idx,
                                                                                                                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 ( returnvalue != SUCCESSFUL_RETURN )
                {
                        nWSR = l;
                        THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
                        return returnvalue;
                }

                /* 4) Determination of step length TAU. */
                returnvalue = hotstart_determineStepLength(     FR_idx,FX_idx,AC_idx,IAC_idx,
                                                                                                        delta_lbA,delta_ubA,delta_lb,delta_ub,
                                                                                                        delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
                                                                                                        BC_idx,BC_status,BC_isBound
                                                                                                        );
                if ( returnvalue != SUCCESSFUL_RETURN )
                {
                        nWSR = l;
                        THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
                        return returnvalue;
                }

                /* 5) Realisation of the homotopy step. */
                returnvalue = hotstart_performStep(     FR_idx,FX_idx,AC_idx,IAC_idx,
                                                                                        delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
                                                                                        delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
                                                                                        BC_idx,BC_status,BC_isBound
                                                                                        );

                if ( returnvalue != SUCCESSFUL_RETURN )
                {
                        nWSR = l;

                        /* stop runtime measurement */
                        if ( cputime != 0 )
                                        *cputime = getCPUtime( ) - starttime;

                        /* optimal solution found? */
                        if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
                        {
                                status = QPS_SOLVED;

                                if ( printlevel == PL_HIGH )
                                        THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );

                                #ifdef PC_DEBUG
                                if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
                                        THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
                                #endif

                                /* check KKT optimality conditions */
                                return checkKKTconditions( );
                        }
                        else
                        {
                                /* checks for infeasibility... */
                                if ( isInfeasible( ) == BT_TRUE )
                                {
                                        status = QPS_HOMOTOPYQPSOLVED;
                                        return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
                                }

                                /* ...unboundedness... */
                                if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
                                        return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );

                                /* ... and throw unspecific error otherwise */
                                THROWERROR( RET_HOMOTOPY_STEP_FAILED );
                                return returnvalue;
                        }
                }

                /* 6) Output information of successful QP iteration. */
                status = QPS_HOMOTOPYQPSOLVED;

                #ifdef PC_DEBUG
                if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
                        THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
                #endif
        }


        /* stop runtime measurement */
        if ( cputime != 0 )
                *cputime = getCPUtime( ) - starttime;


        /* if programm gets to here, output information that QP could not be solved
         * within the given maximum numbers of working set changes */
        if ( printlevel == PL_HIGH )
        {
                #ifdef PC_DEBUG
                sprintf( messageString,"(nWSR = %d)",nWSR );
                return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
                #endif
        }

        /* Finally check KKT optimality conditions. */
        returnValue returnvalueKKTcheck = checkKKTconditions( );

        if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
                return returnvalueKKTcheck;
        else
                return RET_MAX_NWSR_REACHED;
}


/*
 *      g e t N Z
 */
int QProblem::getNZ( )
{
        /* nZ = nFR - nAC */
        return bounds.getFree( )->getLength( ) - constraints.getActive( )->getLength( );
}


/*
 *      g e t D u a l S o l u t i o n
 */
returnValue QProblem::getDualSolution( real_t* const yOpt ) const
{
        int i;

        /* return optimal dual solution vector
         * only if current QP has been solved */
        if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
                 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
                 ( getStatus( ) == QPS_SOLVED ) )
        {
                for( i=0; i<getNV( )+getNC( ); ++i )
                        yOpt[i] = y[i];

                return SUCCESSFUL_RETURN;
        }
        else
        {
                return RET_QP_NOT_SOLVED;
        }
}



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

/*
 *      s e t u p S u b j e c t T o T y p e
 */
returnValue QProblem::setupSubjectToType( )
{
        int i;
        int nC = getNC( );


        /* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
        if ( QProblemB::setupSubjectToType( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );


        /* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
        /* 1) Check if lower constraints' bounds are present. */
        constraints.setNoLower( BT_TRUE );
        for( i=0; i<nC; ++i )
        {
                if ( lbA[i] > -INFTY )
                {
                        constraints.setNoLower( BT_FALSE );
                        break;
                }
        }

        /* 2) Check if upper constraints' bounds are present. */
        constraints.setNoUpper( BT_TRUE );
        for( i=0; i<nC; ++i )
        {
                if ( ubA[i] < INFTY )
                {
                        constraints.setNoUpper( BT_FALSE );
                        break;
                }
        }

        /* 3) Determine implicit equality constraints and unbounded constraints. */
        int nEC = 0;
        int nUC = 0;

        for( i=0; i<nC; ++i )
        {
                if ( ( lbA[i] < -INFTY + BOUNDTOL ) && ( ubA[i] > INFTY - BOUNDTOL ) )
                {
                        constraints.setType( i,ST_UNBOUNDED );
                        ++nUC;
                }
                else
                {
                        if ( lbA[i] > ubA[i] - BOUNDTOL )
                        {
                                constraints.setType( i,ST_EQUALITY );
                                ++nEC;
                        }
                        else
                        {
                                constraints.setType( i,ST_BOUNDED );
                        }
                }
        }

        /* 4) Set dimensions of constraints structure. */
        constraints.setNEC( nEC );
        constraints.setNUC( nUC );
        constraints.setNIC( nC - nEC - nUC );

        return SUCCESSFUL_RETURN;
}


/*
 *      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 QProblem::setupCholeskyDecompositionProjected( )
{
        int i, j, k, ii, kk;
        int nV  = getNV( );
        int nFR = getNFR( );
        int nZ  = getNZ( );

        /* 1) Initialises R with all zeros. */
        for( i=0; i<nV; ++i )
                for( j=0; j<nV; ++j )
                        R[i*NVMAX + j] = 0.0;

        /* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
        if ( hessianType == HST_IDENTITY )
        {
                /* if Hessian is identity, so is its Cholesky factor. */
                for( i=0; i<nV; ++i )
                        R[i*NVMAX + i] = 1.0;
        }
        else
        {
                if ( nZ > 0 )
                {
                        int FR_idx[NVMAX];
                        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_INDEXLIST_CORRUPTED );

                        int AC_idx[NCMAX_ALLOC];
                        if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_INDEXLIST_CORRUPTED );


                        real_t HZ[NVMAX*NVMAX];
                        real_t ZHZ[NVMAX*NVMAX];

                        /* calculate H*Z */
                        for ( i=0; i<nFR; ++i )
                        {
                                ii = FR_idx[i];

                                for ( j=0; j<nZ; ++j )
                                {
                                        HZ[i*NVMAX + j] = 0.0;
                                        for ( k=0; k<nFR; ++k )
                                        {
                                                kk = FR_idx[k];
                                                HZ[i*NVMAX + j] += H[ii*NVMAX + kk] * Q[kk*NVMAX + j];
                                        }
                                }
                        }

                        /* calculate Z'*H*Z */
                        for ( i=0; i<nZ; ++i )
                                for ( j=0; j<nZ; ++j )
                                {
                                        ZHZ[i*NVMAX + j] = 0.0;
                                        for ( k=0; k<nFR; ++k )
                                        {
                                                kk = FR_idx[k];
                                                ZHZ[i*NVMAX + j] += Q[kk*NVMAX + i] * HZ[k*NVMAX + j];
                                        }
                                }

                        /* R'*R = Z'*H*Z */
                        real_t sum;

                        for( i=0; i<nZ; ++i )
                        {
                                /* j == i */
                                sum = ZHZ[i*NVMAX + i];

                                for( k=(i-1); k>=0; --k )
                                        sum -= R[k*NVMAX + i] * R[k*NVMAX + i];

                                if ( sum > 0.0 )
                                        R[i*NVMAX + i] = sqrt( sum );
                                else
                                {
                                        hessianType = HST_SEMIDEF;
                                        return THROWERROR( RET_HESSIAN_NOT_SPD );
                                }

                                for( j=(i+1); j<nZ; ++j )
                                {
                                        sum = ZHZ[j*NVMAX + i];

                                        for( k=(i-1); k>=0; --k )
                                                sum -= R[k*NVMAX + i] * R[k*NVMAX + j];

                                        R[i*NVMAX + j] = sum / R[i*NVMAX + i];
                                }
                        }
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      s e t u p T Q f a c t o r i s a t i o n
 */
returnValue QProblem::setupTQfactorisation( )
{
        int i, j, ii;
        int nV  = getNV( );
        int nFR = getNFR( );

        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INDEXLIST_CORRUPTED );

        /* 1) Set Q to unity matrix. */
        for( i=0; i<nV; ++i )
                for( j=0; j<nV; ++j )
                        Q[i*NVMAX + j] = 0.0;

        for( i=0; i<nFR; ++i )
        {
                ii = FR_idx[i];
                Q[ii*NVMAX + i] = 1.0;
        }

        /* 2) Set T to zero matrix. */
        for( i=0; i<sizeT; ++i )
                for( j=0; j<sizeT; ++j )
                        T[i*NVMAX + j] = 0.0;

        return SUCCESSFUL_RETURN;
}


/*
 *      s o l v e I n i t i a l Q P
 */
returnValue QProblem::solveInitialQP(   const real_t* const xOpt, const real_t* const yOpt,
                                                                                const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
                                                                                int& nWSR, real_t* const cputime
                                                                                )
{
        int i;

        /* some definitions */
        int nV = getNV( );
        int nC = getNC( );


        /* start runtime measurement */
        real_t starttime = 0.0;
        if ( cputime != 0 )
                starttime = getCPUtime( );


        status = QPS_NOTINITIALISED;

        /* I) ANALYSE QP DATA: */
        /* 1) Check if Hessian happens to be the identity matrix. */
        if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        /* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
        if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        /* 3) Initialise cycling manager. */
        cyclingManager.clearCyclingData( );

        status = QPS_PREPARINGAUXILIARYQP;


        /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
        /* 1) Setup bounds and constraints data structure. */
        if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

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

        /* 2) Setup optimal primal/dual solution for auxiliary QP. */
        if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        /* 3) Obtain linear independent working set for auxiliary QP. */

        static Bounds auxiliaryBounds;

        auxiliaryBounds.init( nV );

        static Constraints auxiliaryConstraints;

        auxiliaryConstraints.init( nC );

        if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds,guessedConstraints,
                                                                        &auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        /* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
        if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED_TQ );

        if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        if ( ( getNAC( ) + getNFX( ) ) == 0 )
        {
                /* Factorise full Hessian if no bounds/constraints are active. */
                if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_INIT_FAILED_CHOLESKY );
        }
        else
        {
                /* Factorise projected Hessian if there active bounds/constraints. */
                if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_INIT_FAILED_CHOLESKY );
        }

        /* 5) Store original QP formulation... */
        real_t g_original[NVMAX];
        real_t lb_original[NVMAX];
        real_t ub_original[NVMAX];
        real_t lbA_original[NCMAX_ALLOC];
        real_t ubA_original[NCMAX_ALLOC];

        for( i=0; i<nV; ++i )
        {
                g_original[i] = g[i];
                lb_original[i] = lb[i];
                ub_original[i] = ub[i];
        }

        for( i=0; i<nC; ++i )
        {
                lbA_original[i] = lbA[i];
                ubA_original[i] = ubA[i];
        }

        /* ... and setup QP data of an auxiliary QP having an optimal solution
         * as specified by the user (or xOpt = yOpt = 0, by default). */
        if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INIT_FAILED );

        status = QPS_AUXILIARYQPSOLVED;


        /* III) SOLVE ACTUAL INITIAL QP: */
        /* Use hotstart method to find the solution of the original initial QP,... */
        returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,0 );


        /* ... check for infeasibility and unboundedness... */
        if ( isInfeasible( ) == BT_TRUE )
                return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );

        if ( isUnbounded( ) == BT_TRUE )
                return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );

        /* ... and internal errors. */
        if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED )  &&
             ( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
                return THROWERROR( RET_INIT_FAILED_HOTSTART );


        /* stop runtime measurement */
        if ( cputime != 0 )
                *cputime = getCPUtime( ) - starttime;

        if ( printlevel == PL_HIGH )
                THROWINFO( RET_INIT_SUCCESSFUL );

        return returnvalue;
}


/*
 *      o b t a i n A u x i l i a r y W o r k i n g S e t
 */
returnValue QProblem::obtainAuxiliaryWorkingSet(        const real_t* const xOpt, const real_t* const yOpt,
                                                                                                        const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
                                                                                                        Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
                                                                                                        ) const
{
        int i = 0;
        int nV = getNV( );
        int nC = getNC( );


        /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
        if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
                return THROWERROR( RET_INVALID_ARGUMENTS );


        SubjectToStatus guessedStatus;

        /* 2) Setup working set of bounds for auxiliary initial QP. */
        if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );

        /* 3) Setup working set of constraints for auxiliary initial QP. */
        if ( guessedConstraints != 0 )
        {
                /* If an initial working set is specific, use it!
                 * Moreover, add all equality constraints if specified. */
                for( i=0; i<nC; ++i )
                {
                        guessedStatus = guessedConstraints->getStatus( i );

                        if ( constraints.getType( i ) == ST_EQUALITY )
                        {
                                if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                        }
                        else
                        {
                                if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                        }
                }
        }
        else    /* No initial working set specified. */
        {
                /* Obtain initial working set by "clipping". */
                if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
                {
                        for( i=0; i<nC; ++i )
                        {
                                if ( Ax[i] <= lbA[i] + BOUNDTOL )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                        continue;
                                }

                                if ( Ax[i] >= ubA[i] - BOUNDTOL )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
                                                        return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                        continue;
                                }

                                /* Moreover, add all equality constraints if specified. */
                                if ( constraints.getType( i ) == ST_EQUALITY )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                                else
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                        }
                }

                /* Obtain initial working set in accordance to sign of dual solution vector. */
                if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
                {
                        for( i=0; i<nC; ++i )
                        {
                                if ( yOpt[nV+i] > ZERO )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                        continue;
                                }

                                if ( yOpt[nV+i] < -ZERO )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                        continue;
                                }

                                /* Moreover, add all equality constraints if specified. */
                                if ( constraints.getType( i ) == ST_EQUALITY )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                                else
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                        }
                }

                /* If xOpt and yOpt are null pointer and no initial working is specified,
                 * start with empty working set (or implicitly fixed bounds and equality constraints only)
                 * for auxiliary QP. */
                if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
                {
                        for( i=0; i<nC; ++i )
                        {
                                /* Only add all equality constraints if specified. */
                                if ( constraints.getType( i ) == ST_EQUALITY )
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                                else
                                {
                                        if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
                                }
                        }
                }
        }

        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 QProblem::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
                                                                                                const Constraints* const auxiliaryConstraints,
                                                                                                BooleanType setupAfresh
                                                                                                )
{
        int i;
        int nV = getNV( );
        int 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) SETUP CHOLESKY FLAG:
         *    Cholesky decomposition shall only be updated if working set
         *    shall be updated (i.e. NOT setup afresh!) */
        BooleanType updateCholesky;
        if ( setupAfresh == BT_TRUE )
                updateCholesky = BT_FALSE;
        else
                updateCholesky = BT_TRUE;


        /* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
        if ( setupAfresh == BT_FALSE )
        {
                /* 1) Remove all active constraints that shall be inactive AND
                *    all active constraints that are active at the wrong bound. */
                for( i=0; i<nC; ++i )
                {
                        if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
                                if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_SETUP_WORKINGSET_FAILED );

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

                /* 2) Remove all active bounds that shall be inactive AND
                *    all active bounds that are active at the wrong bound. */
                for( i=0; i<nV; ++i )
                {
                        if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
                                if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_SETUP_WORKINGSET_FAILED );

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


        /* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
        /* 1) Add all inactive bounds that shall be active AND
         *    all formerly active bounds that have been active at the wrong bound. */
        for( i=0; i<nV; ++i )
        {
                if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
                {
                        /* Add bound only if it is linearly independent from the current working set. */
                        if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
                        {
                                if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
                        }
                }
        }

        /* 2) Add all inactive constraints that shall be active AND
         *    all formerly active constraints that have been active at the wrong bound. */
        for( i=0; i<nC; ++i )
        {
                if ( ( auxiliaryConstraints->getStatus( i ) == ST_LOWER ) || ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) )
                {
                        /* formerly inactive */
                        if ( constraints.getStatus( i ) == ST_INACTIVE )
                        {
                                /* Add constraint only if it is linearly independent from the current working set. */
                                if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
                                {
                                        if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
                                                return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
                                }
                        }
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      s e t u p A u x i l i a r y Q P s o l u t i o n
 */
returnValue QProblem::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt
                                                                                                )
{
        int i, j;
        int nV = getNV( );
        int nC = getNC( );


        /* Setup primal/dual solution vector for auxiliary initial QP:
         * if a null pointer is passed, a zero vector is assigned;
         *  old solution vector is kept if pointer to internal solution vevtor is passed. */
        if ( xOpt != 0 )
        {
                if ( xOpt != x )
                        for( i=0; i<nV; ++i )
                                x[i] = xOpt[i];

                for ( j=0; j<nC; ++j )
                {
                        Ax[j] = 0.0;

                        for( i=0; i<nV; ++i )
                                Ax[j] += A[j*NVMAX + i] * x[i];
                }
        }
        else
        {
                for( i=0; i<nV; ++i )
                        x[i] = 0.0;

                for ( j=0; j<nC; ++j )
                        Ax[j] = 0.0;
        }

        if ( yOpt != 0 )
        {
                if ( yOpt != y )
                        for( i=0; i<nV+nC; ++i )
                                y[i] = yOpt[i];
        }
        else
        {
                for( i=0; i<nV+nC; ++i )
                        y[i] = 0.0;
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      s e t u p A u x i l i a r y Q P g r a d i e n t
 */
returnValue QProblem::setupAuxiliaryQPgradient( )
{
        int i, j;
        int nV = getNV( );
        int nC = getNC( );


        /* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]. */
        for ( i=0; i<nV; ++i )
        {
                /* Id'*yB */
                g[i] = y[i];

                /* A'*yC */
                for ( j=0; j<nC; ++j )
                        g[i] += A[j*NVMAX + i] * y[nV+j];

                /* -H*x */
                for ( j=0; j<nV; ++j )
                        g[i] -= H[i*NVMAX + j] * x[j];
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      s e t u p A u x i l i a r y Q P b o u n d s
 */
returnValue QProblem::setupAuxiliaryQPbounds(   const Bounds* const auxiliaryBounds,
                                                                                                const Constraints* const auxiliaryConstraints,
                                                                                                BooleanType useRelaxation
                                                                                                )
{
        int i;
        int nV = getNV( );
        int nC = getNC( );


        /* 1) Setup bound vectors. */
        for ( i=0; i<nV; ++i )
        {
                switch ( bounds.getStatus( i ) )
                {
                        case ST_INACTIVE:
                                if ( useRelaxation == BT_TRUE )
                                {
                                        if ( bounds.getType( i ) == ST_EQUALITY )
                                        {
                                                lb[i] = x[i];
                                                ub[i] = x[i];
                                        }
                                        else
                                        {
                                                /* If a bound is inactive although it was supposed to be
                                                * active by the auxiliaryBounds, it could not be added
                                                * due to linear dependence. Thus set it "strongly inactive". */
                                                if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
                                                        lb[i] = x[i];
                                                else
                                                        lb[i] = x[i] - BOUNDRELAXATION;

                                                if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
                                                        ub[i] = x[i];
                                                else
                                                        ub[i] = x[i] + BOUNDRELAXATION;
                                        }
                                }
                                break;

                        case ST_LOWER:
                                lb[i] = x[i];
                                if ( bounds.getType( i ) == ST_EQUALITY )
                                {
                                        ub[i] = x[i];
                                }
                                else
                                {
                                        if ( useRelaxation == BT_TRUE )
                                                ub[i] = x[i] + BOUNDRELAXATION;
                                }
                                break;

                        case ST_UPPER:
                                ub[i] = x[i];
                                if ( bounds.getType( i ) == ST_EQUALITY )
                                {
                                        lb[i] = x[i];
                                }
                                else
                                {
                                        if ( useRelaxation == BT_TRUE )
                                                lb[i] = x[i] - BOUNDRELAXATION;
                                }
                                break;

                        default:
                                return THROWERROR( RET_UNKNOWN_BUG );
                }
        }

        /* 2) Setup constraints vectors. */
        for ( i=0; i<nC; ++i )
        {
                switch ( constraints.getStatus( i ) )
                {
                        case ST_INACTIVE:
                                if ( useRelaxation == BT_TRUE )
                                {
                                        if ( constraints.getType( i ) == ST_EQUALITY )
                                        {
                                                lbA[i] = Ax[i];
                                                ubA[i] = Ax[i];
                                        }
                                        else
                                        {
                                                /* If a constraint is inactive although it was supposed to be
                                                * active by the auxiliaryConstraints, it could not be added
                                                * due to linear dependence. Thus set it "strongly inactive". */
                                                if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
                                                        lbA[i] = Ax[i];
                                                else
                                                        lbA[i] = Ax[i] - BOUNDRELAXATION;

                                                if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
                                                        ubA[i] = Ax[i];
                                                else
                                                        ubA[i] = Ax[i] + BOUNDRELAXATION;
                                        }
                                }
                                break;

                        case ST_LOWER:
                                lbA[i] = Ax[i];
                                if ( constraints.getType( i ) == ST_EQUALITY )
                                {
                                        ubA[i] = Ax[i];
                                }
                                else
                                {
                                        if ( useRelaxation == BT_TRUE )
                                                ubA[i] = Ax[i] + BOUNDRELAXATION;
                                }
                                break;

                        case ST_UPPER:
                                ubA[i] = Ax[i];
                                if ( constraints.getType( i ) == ST_EQUALITY )
                                {
                                        lbA[i] = Ax[i];
                                }
                                else
                                {
                                        if ( useRelaxation == BT_TRUE )
                                                lbA[i] = Ax[i] - BOUNDRELAXATION;
                                }
                                break;

                        default:
                                return THROWERROR( RET_UNKNOWN_BUG );
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      a d d C o n s t r a i n t
 */
returnValue QProblem::addConstraint(    int number, SubjectToStatus C_status,
                                                                                BooleanType updateCholesky
                                                                                )
{
        int i, j, ii;

        /* 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. */
        /* check for LI only if Cholesky decomposition shall be updated! */
        if ( updateCholesky == 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 THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );

                        case RET_ENSURELI_FAILED_CYCLING:
                                return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );

                        default:
                                return THROWERROR( RET_ENSURELI_FAILED );
                }
        }

        /* some definitions */
        int nFR = getNFR( );
        int nAC = getNAC( );
        int nZ  = getNZ( );

        int tcol = sizeT - nAC;


        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ADDCONSTRAINT_FAILED );

        real_t aFR[NVMAX];
        real_t wZ[NVMAX];
        for( i=0; i<nZ; ++i )
                wZ[i] = 0.0;


        /* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
        /* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
        for( i=0; i<nFR; ++i )
        {
                ii = FR_idx[i];
                aFR[i] = A[number*NVMAX + ii];
        }

        /* calculate wZ */
        for( i=0; i<nFR; ++i )
        {
                ii = FR_idx[i];
                for( j=0; j<nZ; ++j )
                        wZ[j] += aFR[i] * Q[ii*NVMAX + j];
        }

        /* 2) Calculate wY and store it directly into T. */
        if ( nAC > 0 )
        {
                for( j=0; j<nAC; ++j )
                        T[nAC*NVMAX + tcol+j] = 0.0;
                for( i=0; i<nFR; ++i )
                {
                        ii = FR_idx[i];
                        for( j=0; j<nAC; ++j )
                                T[nAC*NVMAX + tcol+j] += aFR[i] * Q[ii*NVMAX + nZ+j];
                }
        }


        real_t c, s;

        if ( nZ > 0 )
        {
                /* II) RESTORE TRIANGULAR FORM OF T: */
                /*     Use column-wise Givens rotations to restore reverse triangular form
                *      of T, simultanenous change of Q (i.e. Z) and R. */
                for( j=0; j<nZ-1; ++j )
                {
                        computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );

                        for( i=0; i<nFR; ++i )
                        {
                                ii = FR_idx[i];
                                applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
                        }

                        if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
                        {
                                for( i=0; i<=j+1; ++i )
                                        applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
                        }
                }

                T[nAC*NVMAX + tcol-1] = wZ[nZ-1];


                if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
                {
                        /* III) RESTORE TRIANGULAR FORM OF R:
                         *      Use row-wise Givens rotations to restore upper triangular form of R. */
                        for( i=0; i<nZ-1; ++i )
                        {
                                computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );

                                for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
                                        applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
                        }
                        /* last column of R is thrown away */
                        for( i=0; i<nZ; ++i )
                                R[i*NVMAX + nZ-1] = 0.0;
                }
        }


        /* IV) UPDATE INDICES */
        if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
                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 QProblem::addConstraint_checkLI( int number )
{
        int i, j, jj;
        int nFR = getNFR( );
        int nZ  = getNZ( );

        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INDEXLIST_CORRUPTED );

        /* Check if constraint <number> is linearly independent from the
           the active ones (<=> is element of null space of Afr). */
        real_t sum;

        for( i=0; i<nZ; ++i )
        {
                sum = 0.0;
                for( j=0; j<nFR; ++j )
                {
                        jj = FR_idx[j];
                        sum += Q[jj*NVMAX + i] * A[number*NVMAX + jj];
                }

                if ( getAbs( sum ) > 10.0*EPS )
                        return RET_LINEARLY_INDEPENDENT;
        }

        return RET_LINEARLY_DEPENDENT;
}


/*
 *      a d d C o n s t r a i n t _ e n s u r e L I
 */
returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
{
        int i, j, ii, jj;
        int nV  = getNV( );
        int nFR = getNFR( );
        int nFX = getNFX( );
        int nAC = getNAC( );
        int nZ  = getNZ( );


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

        if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
                return THROWERROR( RET_ENSURELI_FAILED );

        if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
                return SUCCESSFUL_RETURN;


        /* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
        /* 1) Determine coefficients of linear combination,
         *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
         *    An Algorithm for the Solution of the Parametric Quadratic Programming
         *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        int FX_idx[NVMAX];
        if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        real_t xiC[NCMAX_ALLOC];
        real_t xiC_TMP[NCMAX_ALLOC];
        real_t xiB[NVMAX];

        /* 2) Calculate xiC */
        if ( nAC > 0 )
        {
                if ( C_status == ST_LOWER )
                {
                        for( i=0; i<nAC; ++i )
                        {
                                xiC_TMP[i] = 0.0;
                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        xiC_TMP[i] += Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
                                }
                        }
                }
                else
                {
                        for( i=0; i<nAC; ++i )
                        {
                                xiC_TMP[i] = 0.0;
                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        xiC_TMP[i] -= Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
                                }
                        }
                }

                if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_ENSURELI_FAILED_TQ );
        }

        /* 3) Calculate xiB. */
        int AC_idx[NCMAX_ALLOC];
        if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        if ( C_status == ST_LOWER )
        {
                for( i=0; i<nFX; ++i )
                {
                        ii = FX_idx[i];
                        xiB[i] = A[number*NVMAX + ii];

                        for( j=0; j<nAC; ++j )
                        {
                                jj = AC_idx[j];
                                xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
                        }
                }
        }
        else
        {
                for( i=0; i<nFX; ++i )
                {
                        ii = FX_idx[i];
                        xiB[i] = -A[number*NVMAX + ii];

                        for( j=0; j<nAC; ++j )
                        {
                                jj = AC_idx[j];
                                xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
                        }
                }
        }


        /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
        real_t y_min = INFTY * INFTY;
        int y_min_number = -1;
        BooleanType y_min_isBound = BT_FALSE;

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

                if ( constraints.getStatus( ii ) == ST_LOWER )
                {
                        if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
                        {
                                if ( y[nV+ii]/xiC[i] < y_min )
                                {
                                        y_min = y[nV+ii]/xiC[i];
                                        y_min_number = ii;
                                }
                        }
                }
                else
                {
                        if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
                        {
                                if ( y[nV+ii]/xiC[i] < y_min )
                                {
                                        y_min = y[nV+ii]/xiC[i];
                                        y_min_number = ii;
                                }
                        }
                }
        }

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

                if ( bounds.getStatus( ii ) == ST_LOWER )
                {
                        if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
                        {
                                if ( y[ii]/xiB[i] < y_min )
                                {
                                        y_min = y[ii]/xiB[i];
                                        y_min_number = ii;
                                        y_min_isBound = BT_TRUE;
                                }
                        }
                }
                else
                {
                        if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
                        {
                                if ( y[ii]/xiB[i] < y_min )
                                {
                                        y_min = y[ii]/xiB[i];
                                        y_min_number = ii;
                                        y_min_isBound = BT_TRUE;
                                }
                        }
                }
        }

        /* setup output preferences */
        #ifdef PC_DEBUG
        char messageString[80];
        VisibilityStatus visibilityStatus;

        if ( printlevel == PL_HIGH )
                visibilityStatus = VS_VISIBLE;
        else
                visibilityStatus = VS_HIDDEN;
        #endif


        /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
        if ( y_min_number >= 0 )
        {
                /* 1) Check for cycling due to infeasibility. */
                if ( ( cyclingManager.getCyclingStatus( number,BT_FALSE ) == CYC_PREV_REMOVED ) &&
                         ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
                {
                        infeasible = BT_TRUE;

                        return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
                }
                else
                {
                        /* set cycling data */
                        cyclingManager.clearCyclingData( );
                        cyclingManager.setCyclingStatus( number,BT_FALSE, CYC_PREV_ADDED );
                        cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
                }

                /* 2) 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 )
                {
                        #ifdef PC_DEBUG
                        sprintf( messageString,"bound no. %d.",y_min_number );
                        getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                        #endif

                        if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                        y[y_min_number] = 0.0;
                }
                else
                {
                        #ifdef PC_DEBUG
                        sprintf( messageString,"constraint no. %d.",y_min_number );
                        getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                        #endif

                        if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                        y[nV+y_min_number] = 0.0;
                }
        }
        else
        {
                /* no constraint/bound can be removed => QP is infeasible! */
                infeasible = BT_TRUE;

                return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
        }

        return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
}



/*
 *      a d d B o u n d
 */
returnValue QProblem::addBound( int number, SubjectToStatus B_status,
                                                                BooleanType updateCholesky
                                                                )
{
        int i, j, ii;

        /* 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. */
        /* check for LI only if Cholesky decomposition shall be updated! */
        if ( updateCholesky == 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 THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );

                        case RET_ENSURELI_FAILED_CYCLING:
                                return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );

                        default:
                                return THROWERROR( RET_ENSURELI_FAILED );
                }
        }


        /* some definitions */
        int nFR = getNFR( );
        int nAC = getNAC( );
        int nZ  = getNZ( );

        int tcol = sizeT - nAC;


        /* I) SWAP INDEXLIST OF FREE VARIABLES:
         *    move the variable to be fixed to the end of the list of free variables. */
        int lastfreenumber = bounds.getFree( )->getLastNumber( );
        if ( lastfreenumber != number )
                if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
                        THROWERROR( RET_ADDBOUND_FAILED );


        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ADDBOUND_FAILED );

        real_t w[NVMAX];


        /* II) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
        /* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
        for( i=0; i<nFR; ++i )
                w[i] = Q[FR_idx[nFR-1]*NVMAX + i];


        /* 2) Use column-wise Givens rotations to restore reverse triangular form
         *    of the first row of T, simultanenous change of Q (i.e. Z) and R. */
        real_t c, s;

        for( j=0; j<nZ-1; ++j )
        {
                computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );

                for( i=0; i<nFR; ++i )
                {
                        ii = FR_idx[i];
                        applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
                }

                if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
                {
                        for( i=0; i<=j+1; ++i )
                                applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
                }
        }


        if ( nAC > 0 )    /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
        {
                /* store new column a in a temporary vector instead of shifting T one column to the left */
                real_t tmp[NCMAX_ALLOC];
                for( i=0; i<nAC; ++i )
                        tmp[i] = 0.0;

                {
                        j = nZ-1;

                        computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );

                        for( i=0; i<nFR; ++i )
                        {
                                ii = FR_idx[i];
                                applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
                        }

                        applyGivens( c,s,T[(nAC-1)*NVMAX + tcol],tmp[nAC-1], tmp[nAC-1],T[(nAC-1)*NVMAX + tcol] );
                }

                for( j=nZ; j<nFR-1; ++j )
                {
                        computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );

                        for( i=0; i<nFR; ++i )
                        {
                                ii = FR_idx[i];
                                applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
                        }

                        for( i=(nFR-2-j); i<nAC; ++i )
                                applyGivens( c,s,T[i*NVMAX + 1+tcol-nZ+j],tmp[i], tmp[i],T[i*NVMAX + 1+tcol-nZ+j] );
                }

        }


        if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
        {
                /* III) RESTORE TRIANGULAR FORM OF R:
                 *      use row-wise Givens rotations to restore upper triangular form of R */
                for( i=0; i<nZ-1; ++i )
                {
                        computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );

                        for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
                                applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
                }
                /* last column of R is thrown away */
                for( i=0; i<nZ; ++i )
                        R[i*NVMAX + nZ-1] = 0.0;
        }


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

        return SUCCESSFUL_RETURN;
}


/*
 *      a d d B o u n d _ c h e c k L I
 */
returnValue QProblem::addBound_checkLI( int number )
{
        int i;

        /* some definitions */
        int nZ  = getNZ( );

        /* Check if constraint <number> is linearly independent from the
           the active ones (<=> is element of null space of Afr). */
        for( i=0; i<nZ; ++i )
        {
                if ( getAbs( Q[number*NVMAX + i] ) > EPS )
                        return RET_LINEARLY_INDEPENDENT;
        }

        return RET_LINEARLY_DEPENDENT;
}


/*
 *      a d d B o u n d _ e n s u r e L I
 */
returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
{
        int i, j, ii, jj;
        int nV  = getNV( );
        int nFX = getNFX( );
        int nAC = getNAC( );
        int nZ  = getNZ( );


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

        if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
                return SUCCESSFUL_RETURN;


        /* II) NEW BOUND IS LINEARLY DEPENDENT: */
        /* 1) Determine coefficients of linear combination,
         *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
         *    An Algorithm for the Solution of the Parametric Quadratic Programming
         *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        int FX_idx[NVMAX];
        if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        int AC_idx[NCMAX_ALLOC];
        if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_ENSURELI_FAILED );

        real_t xiC[NCMAX_ALLOC];
        real_t xiC_TMP[NCMAX_ALLOC];
        real_t xiB[NVMAX];

        /* 2) Calculate xiC. */
        if ( nAC > 0 )
        {
                if ( B_status == ST_LOWER )
                {
                        for( i=0; i<nAC; ++i )
                                xiC_TMP[i] = Q[number*NVMAX + nZ+i];
                }
                else
                {
                        for( i=0; i<nAC; ++i )
                                xiC_TMP[i] = -Q[number*NVMAX + nZ+i];
                }

                if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_ENSURELI_FAILED_TQ );
        }

        /* 3) Calculate xiB. */
        for( i=0; i<nFX; ++i )
        {
                ii = FX_idx[i];

                xiB[i] = 0.0;
                for( j=0; j<nAC; ++j )
                {
                        jj = AC_idx[j];
                        xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
                }
        }


        /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
        real_t y_min = INFTY * INFTY;
        int y_min_number = -1;
        BooleanType y_min_isBound = BT_FALSE;

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

                if ( constraints.getStatus( ii ) == ST_LOWER )
                {
                        if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
                        {
                                if ( y[nV+ii]/xiC[i] < y_min )
                                {
                                        y_min = y[nV+ii]/xiC[i];
                                        y_min_number = ii;
                                }
                        }
                }
                else
                {
                        if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
                        {
                                if ( y[nV+ii]/xiC[i] < y_min )
                                {
                                        y_min = y[nV+ii]/xiC[i];
                                        y_min_number = ii;
                                }
                        }
                }
        }

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

                if ( bounds.getStatus( ii ) == ST_LOWER )
                {
                        if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
                        {
                                if ( y[ii]/xiB[i] < y_min )
                                {
                                        y_min = y[ii]/xiB[i];
                                        y_min_number = ii;
                                        y_min_isBound = BT_TRUE;
                                }
                        }
                }
                else
                {
                        if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
                        {
                                if ( y[ii]/xiB[i] < y_min )
                                {
                                        y_min = y[ii]/xiB[i];
                                        y_min_number = ii;
                                        y_min_isBound = BT_TRUE;
                                }
                        }
                }
        }

        /* setup output preferences */
        #ifdef PC_DEBUG
        char messageString[80];
        VisibilityStatus visibilityStatus;

        if ( printlevel == PL_HIGH )
                visibilityStatus = VS_VISIBLE;
        else
                visibilityStatus = VS_HIDDEN;
        #endif


        /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
        if ( y_min_number >= 0 )
        {
                /* 1) Check for cycling due to infeasibility. */
                if ( ( cyclingManager.getCyclingStatus( number,BT_TRUE ) == CYC_PREV_REMOVED ) &&
                         ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
                {
                        infeasible = BT_TRUE;

                        return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
                }
                else
                {
                        /* set cycling data */
                        cyclingManager.clearCyclingData( );
                        cyclingManager.setCyclingStatus( number,BT_TRUE, CYC_PREV_ADDED );
                        cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
                }


                /* 2) 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 )
                {
                        #ifdef PC_DEBUG
                        sprintf( messageString,"bound no. %d.",y_min_number );
                        getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                        #endif

                        if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                        y[y_min_number] = 0.0;
                }
                else
                {
                        #ifdef PC_DEBUG
                        sprintf( messageString,"constraint no. %d.",y_min_number );
                        getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                        #endif

                        if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                        y[nV+y_min_number] = 0.0;
                }
        }
        else
        {
                /* no constraint/bound can be removed => QP is infeasible! */
                infeasible = BT_TRUE;

                return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
        }

        return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
}



/*
 *      r e m o v e C o n s t r a i n t
 */
returnValue QProblem::removeConstraint( int number,
                                                                                BooleanType updateCholesky
                                                                                )
{
        int i, j, ii, jj;

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

        /* some definitions */
        int nFR = getNFR( );
        int nAC = getNAC( );
        int nZ  = getNZ( );

        int tcol = sizeT - nAC;
        int number_idx = constraints.getActive( )->getIndex( number );


        /* 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 );


        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_REMOVECONSTRAINT_FAILED );


        /* I) REMOVE <number>th ROW FROM T,
         *    i.e. shift rows number+1 through nAC  upwards (instead of the actual
         *    constraint number its corresponding index within matrix A is used). */
        if ( number_idx < nAC-1 )
        {
                for( i=(number_idx+1); i<nAC; ++i )
                        for( j=(nAC-i-1); j<nAC; ++j )
                                T[(i-1)*NVMAX + tcol+j] = T[i*NVMAX + tcol+j];
                /* gimmick: write zeros into the last row of T */
                for( j=0; j<nAC; ++j )
                        T[(nAC-1)*NVMAX + tcol+j] = 0.0;


                /* II) RESTORE TRIANGULAR FORM OF T,
                 *     use column-wise Givens rotations to restore reverse triangular form
                 *     of T simultanenous change of Q (i.e. Y). */
                real_t c, s;

                for( j=(nAC-2-number_idx); j>=0; --j )
                {
                        computeGivens( T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j], T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j],c,s );

                        for( i=(nAC-j-1); i<(nAC-1); ++i )
                                applyGivens( c,s,T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j], T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j] );

                        for( i=0; i<nFR; ++i )
                        {
                                ii = FR_idx[i];
                                applyGivens( c,s,Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j], Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j] );
                        }
                }
        }
        else
        {
                /* gimmick: write zeros into the last row of T */
                for( j=0; j<nAC; ++j )
                        T[(nAC-1)*NVMAX + tcol+j] = 0.0;
        }


        if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
        {
                /* III) UPDATE CHOLESKY DECOMPOSITION,
                 *      calculate new additional column (i.e. [r sqrt(rho2)]')
                 *      of the Cholesky factor R. */
                real_t Hz[NVMAX];
                for ( i=0; i<nFR; ++i )
                        Hz[i] = 0.0;
                real_t rho2 = 0.0;

                /* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
                 *    (i.e. the old leftmost column of Y).  */
                for( j=0; j<nFR; ++j )
                {
                        jj = FR_idx[j];
                        for( i=0; i<nFR; ++i )
                                Hz[i] += H[jj*NVMAX + FR_idx[i]] * Q[jj*NVMAX + nZ];
                }

                if ( nZ > 0 )
                {
                        real_t ZHz[NVMAX];
                        for ( i=0; i<nZ; ++i )
                                ZHz[i] = 0.0;
                        real_t r[NVMAX];

                        /* 2) Calculate ZHz = Z'*Hz (old Z). */
                        for( j=0; j<nFR; ++j )
                        {
                                jj = FR_idx[j];

                                for( i=0; i<nZ; ++i )
                                        ZHz[i] += Q[jj*NVMAX + i] * Hz[j];
                        }

                        /* 3) Calculate r = R^-T * ZHz. */
                        if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
                                return THROWERROR( RET_REMOVECONSTRAINT_FAILED );

                        /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
                         *    and store r into R. */
                        for( i=0; i<nZ; ++i )
                        {
                                rho2 -= r[i]*r[i];
                                R[i*NVMAX + nZ] = r[i];
                        }
                }

                for( j=0; j<nFR; ++j )
                        rho2 += Q[FR_idx[j]*NVMAX + nZ] * Hz[j];

                /* 5) Store rho into R. */
                if ( rho2 > 0.0 )
                        R[nZ*NVMAX + nZ] = sqrt( rho2 );
                else
                {
                        hessianType = HST_SEMIDEF;
                        return THROWERROR( RET_HESSIAN_NOT_SPD );
                }
        }

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


        return SUCCESSFUL_RETURN;
}


/*
 *      r e m o v e B o u n d
 */
returnValue QProblem::removeBound(      int number,
                                                                        BooleanType updateCholesky
                                                                        )
{
        int i, j, ii, jj;

        /* 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 );
        }

        /* some definitions */
        int nFR = getNFR( );
        int nAC = getNAC( );
        int nZ  = getNZ( );

        int tcol = sizeT - nAC;


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


        int FR_idx[NVMAX];
        if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_REMOVEBOUND_FAILED );

        /* I) APPEND <nFR+1>th UNITY VECOTR TO Q. */
        int nnFRp1 = FR_idx[nFR];
        for( i=0; i<nFR; ++i )
        {
                ii = FR_idx[i];
                Q[ii*NVMAX + nFR] = 0.0;
                Q[nnFRp1*NVMAX + i] = 0.0;
        }
        Q[nnFRp1*NVMAX + nFR] = 1.0;

        if ( nAC > 0 )
        {
                /* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
                int AC_idx[NCMAX_ALLOC];
                if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_REMOVEBOUND_FAILED );

                real_t tmp[NCMAX_ALLOC];
                for( i=0; i<nAC; ++i )
                {
                        ii = AC_idx[i];
                        tmp[i] =  A[ii*NVMAX + number];
                }


                /* II) RESTORE TRIANGULAR FORM OF T,
                 *     use column-wise Givens rotations to restore reverse triangular form
                 *     of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
                real_t c, s;

                for( j=(nAC-1); j>=0; --j )
                {
                        computeGivens( tmp[nAC-1-j],T[(nAC-1-j)*NVMAX + tcol+j],T[(nAC-1-j)*NVMAX + tcol+j],tmp[nAC-1-j],c,s );

                        for( i=(nAC-j); i<nAC; ++i )
                                applyGivens( c,s,tmp[i],T[i*NVMAX + tcol+j],T[i*NVMAX + tcol+j],tmp[i] );

                        for( i=0; i<=nFR; ++i )
                        {
                                ii = FR_idx[i];
                                /* nZ+1+nAC = nFR+1  /  nZ+(1) = nZ+1 */
                                applyGivens( c,s,Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j],Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j] );
                        }
                }
        }


        if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
        {
                /* III) UPDATE CHOLESKY DECOMPOSITION,
                 *      calculate new additional column (i.e. [r sqrt(rho2)]')
                 *      of the Cholesky factor R: */
                real_t z2 = Q[nnFRp1*NVMAX + nZ];
                real_t rho2 = H[nnFRp1*NVMAX + nnFRp1]*z2*z2; /* rho2 = h2*z2*z2 */

                if ( nFR > 0 )
                {
                        real_t Hz[NVMAX];
                        for( i=0; i<nFR; ++i )
                                Hz[i] = 0.0;
                        /* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
                         *    rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
                        for( j=0; j<nFR; ++j )
                        {
                                jj = FR_idx[j];
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        /*                         H * z1 */
                                        Hz[i] += H[jj*NVMAX + ii] * Q[jj*NVMAX + nZ];
                                }
                        }

                        if ( nZ > 0 )
                        {
                                real_t r[NVMAX];
                                real_t rhs[NVMAX];
                                for( i=0; i<nZ; ++i )
                                        rhs[i] = 0.0;

                                /* 2) Calculate rhs. */
                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        for( i=0; i<nZ; ++i )
                                                                                /* Zfr' * ( Hz + z2*h1 ) */
                                                rhs[i] += Q[jj*NVMAX + i] * ( Hz[j] + z2 * H[nnFRp1*NVMAX + jj] );
                                }

                                /* 3) Calculate r = R^-T * rhs. */
                                if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_REMOVEBOUND_FAILED );

                                /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
                                 *    and store r into R. */
                                for( i=0; i<nZ; ++i )
                                {
                                        rho2 -= r[i]*r[i];
                                        R[i*NVMAX + nZ] = r[i];
                                }
                        }

                        for( j=0; j<nFR; ++j )
                        {
                                jj = FR_idx[j];
                                                        /* z1' * ( Hz + 2*z2*h1 ) */
                                rho2 += Q[jj*NVMAX + nZ] * ( Hz[j] + 2.0*z2*H[nnFRp1*NVMAX + jj] );
                        }
                }


                /* 5) Store rho into R. */
                if ( rho2 > 0.0 )
                        R[nZ*NVMAX + nZ] = sqrt( rho2 );
                else
                {
                        hessianType = HST_SEMIDEF;
                        return THROWERROR( RET_HESSIAN_NOT_SPD );
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      b a c k s o l v e R  (CODE DUPLICATE OF QProblemB CLASS!!!)
 */
returnValue QProblem::backsolveR(       const real_t* const b, BooleanType transposed,
                                                                        real_t* const a
                                                                        )
{
        /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
        return backsolveR( b,transposed,BT_FALSE,a );
}


/*
 *      b a c k s o l v e R  (CODE DUPLICATE OF QProblemB CLASS!!!)
 */
returnValue QProblem::backsolveR(       const real_t* const b, BooleanType transposed,
                                                                        BooleanType removingBound,
                                                                        real_t* const a
                                                                        )
{
        int i, j;
        int nR = getNZ( );

        real_t sum;

        /* if backsolve is called while removing a bound, reduce nZ by one. */
        if ( removingBound == BT_TRUE )
                --nR;

        /* nothing to do */
        if ( nR <= 0 )
                return SUCCESSFUL_RETURN;


        /* Solve Ra = b, where R might be transposed. */
        if ( transposed == BT_FALSE )
        {
                /* solve Ra = b */
                for( i=(nR-1); i>=0; --i )
                {
                        sum = b[i];
                        for( j=(i+1); j<nR; ++j )
                                sum -= R[i*NVMAX + j] * a[j];

                        if ( getAbs( R[i*NVMAX + i] ) > ZERO )
                                a[i] = sum / R[i*NVMAX + i];
                        else
                                return THROWERROR( RET_DIV_BY_ZERO );
                }
        }
        else
        {
                /* solve R^T*a = b */
                for( i=0; i<nR; ++i )
                {
                        sum = b[i];

                        for( j=0; j<i; ++j )
                                sum -= R[j*NVMAX + i] * a[j];

                        if ( getAbs( R[i*NVMAX + i] ) > ZERO )
                                a[i] = sum / R[i*NVMAX + i];
                        else
                                return THROWERROR( RET_DIV_BY_ZERO );
                }
        }

        return SUCCESSFUL_RETURN;
}



/*
 *      b a c k s o l v e T
 */
returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a )
{
        int i, j;
        int nT = getNAC( );
        int tcol = sizeT - nT;

        real_t sum;

        /* nothing to do */
        if ( nT <= 0 )
                return SUCCESSFUL_RETURN;


        /* Solve Ta = b, where T might be transposed. */
        if ( transposed == BT_FALSE )
        {
                /* solve Ta = b */
                for( i=0; i<nT; ++i )
                {
                        sum = b[i];
                        for( j=0; j<i; ++j )
                                sum -= T[i*NVMAX + sizeT-1-j] * a[nT-1-j];

                        if ( getAbs( T[i*NVMAX + sizeT-1-i] ) > ZERO )
                                a[nT-1-i] = sum / T[i*NVMAX + sizeT-1-i];
                        else
                                return THROWERROR( RET_DIV_BY_ZERO );
                }
        }
        else
        {
                /* solve T^T*a = b */
                for( i=0; i<nT; ++i )
                {
                        sum = b[i];
                        for( j=0; j<i; ++j )
                                sum -= T[(nT-1-j)*NVMAX + tcol+i] * a[nT-1-j];

                        if ( getAbs( T[(nT-1-i)*NVMAX + tcol+i] ) > ZERO )
                                a[nT-1-i] = sum / T[(nT-1-i)*NVMAX + tcol+i];
                        else
                                return THROWERROR( RET_DIV_BY_ZERO );
                }
        }


        return SUCCESSFUL_RETURN;
}


/*
 *      h o t s t a r t _ d e t e r m i n e D a t a S h i f t
 */
returnValue QProblem::hotstart_determineDataShift(  const int* const FX_idx, const int* const AC_idx,
                                                                                                        const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
                                                                                                        const real_t* const lb_new, const real_t* const ub_new,
                                                                                                        real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
                                                                                                        real_t* const delta_lb, real_t* const delta_ub,
                                                                                                        BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
                                                                                                        )
{
        int i, ii;
        int nC  = getNC( );
        int nAC = getNAC( );


        /* I) DETERMINE DATA SHIFT FOR BOUNDS */
        QProblemB::hotstart_determineDataShift( FX_idx,g_new,lb_new,ub_new, delta_g,delta_lb,delta_ub, Delta_bB_isZero );


        /* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
        /* 1) Calculate shift directions. */
        for( i=0; i<nC; ++i )
        {
                /* if lower constraints' bounds do not exist, shift them to -infinity */
                if ( lbA_new != 0 )
                        delta_lbA[i] = lbA_new[i] - lbA[i];
                else
                        delta_lbA[i] = -INFTY - lbA[i];
        }

        for( i=0; i<nC; ++i )
        {
                /* if upper constraints' bounds do not exist, shift them to infinity */
                if ( ubA_new != 0 )
                        delta_ubA[i] = ubA_new[i] - ubA[i];
                else
                        delta_ubA[i] = INFTY - ubA[i];
        }

        /* 2) Determine if active constraints' bounds are to be shifted. */
        Delta_bC_isZero = BT_TRUE;

        for ( i=0; i<nAC; ++i )
        {
                ii = AC_idx[i];

                if ( ( getAbs( delta_lbA[ii] ) > EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )
                {
                        Delta_bC_isZero = BT_FALSE;
                        break;
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n
 */
returnValue QProblem::hotstart_determineStepDirection(  const int* const FR_idx, const int* const FX_idx, const int* const AC_idx,
                                                                                                                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 i, j, ii, jj;
        int nFR = getNFR( );
        int nFX = getNFX( );
        int nAC = getNAC( );
        int nZ  = getNZ( );

        /* initialise auxiliary vectors */
        real_t HMX_delta_xFX[NVMAX];
        real_t YFR_delta_xFRy[NVMAX];
        real_t ZFR_delta_xFRz[NVMAX];
        real_t HFR_YFR_delta_xFRy[NVMAX];
        for( i=0; i<nFR; ++i )
        {
                delta_xFR[i] = 0.0;
                HMX_delta_xFX[i] = 0.0;
                YFR_delta_xFRy[i] = 0.0;
                ZFR_delta_xFRz[i] = 0.0;
                HFR_YFR_delta_xFRy[i] = 0.0;
        }

        real_t delta_xFRy[NCMAX_ALLOC];
        real_t delta_xFRz[NVMAX];
        for( i=0; i<nZ; ++i )
                delta_xFRz[i] = 0.0;


        /* I) DETERMINE delta_xFX */
        if ( nFX > 0 )
        {
                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];
                }
        }

        /* II) DETERMINE delta_xFR */
        if ( nFR > 0 )
        {
                /* 1) Determine delta_xFRy. */
                if ( nAC > 0 )
                {
                        if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
                        {
                                for( i=0; i<nAC; ++i )
                                        delta_xFRy[i] = 0.0;

                                for( i=0; i<nFR; ++i )
                                        delta_xFR[i] = 0.0;
                        }
                        else
                        {
                                /* auxillary variable */
                                real_t delta_xFRy_TMP[NCMAX_ALLOC];

                                for( i=0; i<nAC; ++i )
                                {
                                        ii = AC_idx[i];

                                        if ( constraints.getStatus( ii ) == ST_LOWER )
                                                delta_xFRy_TMP[i] = delta_lbA[ii];
                                        else
                                                delta_xFRy_TMP[i] = delta_ubA[ii];

                                        if ( Delta_bB_isZero == BT_FALSE )
                                        {
                                                for( j=0; j<nFX; ++j )
                                                {
                                                        jj = FX_idx[j];
                                                        delta_xFRy_TMP[i] -= A[ii*NVMAX + jj] * delta_xFX[j];
                                                }
                                        }
                                }

                                if ( backsolveT( delta_xFRy_TMP, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );

                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        for( j=0; j<nAC; ++j )
                                                YFR_delta_xFRy[i] += Q[ii*NVMAX + nZ+j] * delta_xFRy[j];

                                        /* delta_xFR = YFR*delta_xFRy (+ ZFR*delta_xFRz) */
                                        delta_xFR[i] = YFR_delta_xFRy[i];
                                }
                        }
                }

                /* 2) Determine delta_xFRz. */
                if ( hessianType == HST_IDENTITY )
                {
                        for( j=0; j<nFR; ++j )
                        {
                                jj = FR_idx[j];
                                for( i=0; i<nZ; ++i )
                                        delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_g[jj];
                        }

                        if ( nZ > 0 )
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        for( j=0; j<nZ; ++j )
                                                ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];

                                        delta_xFR[i] += ZFR_delta_xFRz[i];
                                }
                        }
                }
                else
                {
                        if ( Delta_bB_isZero == BT_FALSE )
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        for( j=0; j<nFX; ++j )
                                        {
                                                jj = FX_idx[j];
                                                HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
                                        }
                                }
                        }

                        if ( nAC > 0 )
                        {
                                if ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) )
                                {
                                        for( i=0; i<nFR; ++i )
                                        {
                                                ii = FR_idx[i];
                                                for( j=0; j<nFR; ++j )
                                                {
                                                        jj = FR_idx[j];
                                                        HFR_YFR_delta_xFRy[i] += H[ii*NVMAX + jj] * YFR_delta_xFRy[j];
                                                }
                                        }
                                }
                        }


                        if ( nZ > 0 )
                        {
                                /* auxiliary variable */
                                real_t delta_xFRz_TMP[NVMAX];
                                real_t delta_xFRz_RHS[NVMAX];


                                if ( ( nAC > 0 ) && ( nFX > 0 ) && ( Delta_bB_isZero == BT_FALSE ) )
                                {
                                        for( j=0; j<nFR; ++j )
                                        {
                                                jj = FR_idx[j];
                                                delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j] + HMX_delta_xFX[j];
                                        }
                                }
                                else
                                {
                                        if ( ( nAC == 0 ) && ( Delta_bB_isZero == BT_TRUE ) )
                                        {
                                                for( j=0; j<nFR; ++j )
                                                {
                                                        jj = FR_idx[j];
                                                        delta_xFRz_RHS[j] = delta_g[jj];
                                                }
                                        }
                                        else
                                        {
                                                if ( nAC > 0 ) /* => Delta_bB_isZero == BT_TRUE, as BT_FALSE would imply nFX>0 */
                                                {
                                                        for( j=0; j<nFR; ++j )
                                                        {
                                                                jj = FR_idx[j];
                                                                delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j];
                                                        }
                                                }
                                                else /* Delta_bB_isZero == BT_FALSE, as nAC==0 */
                                                {
                                                        for( j=0; j<nFR; ++j )
                                                        {
                                                                jj = FR_idx[j];
                                                                delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j];
                                                        }
                                                }
                                        }
                                }

                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        for( i=0; i<nZ; ++i )
                                                delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_xFRz_RHS[j];
                                }


                                if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );

                                if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );


                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        for( j=0; j<nZ; ++j )
                                                ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];

                                        delta_xFR[i] += ZFR_delta_xFRz[i];
                                }
                        }
                }
        }

        /* III) DETERMINE delta_yAC */
        if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
        {
                /* auxiliary variables */
                real_t delta_yAC_TMP[NCMAX_ALLOC];
                for( i=0; i<nAC; ++i )
                        delta_yAC_TMP[i] = 0.0;
                real_t delta_yAC_RHS[NVMAX];
                for( i=0; i<nFR; ++i )
                        delta_yAC_RHS[i] = 0.0;

                if ( hessianType == HST_IDENTITY )
                {
                        /* delta_yAC = (T')^-1 * ( Yfr*delta_gFR + delta_xFRy ) */
                        for( j=0; j<nAC; ++j )
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        delta_yAC_TMP[j] += Q[ii*NVMAX + nZ+j] * delta_g[ii];
                                }

                                delta_yAC_TMP[j] += delta_xFRy[j];
                        }
                }
                else
                {
                        if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        delta_yAC_RHS[i] = delta_g[ii];
                                }
                        }
                        else
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        delta_yAC_RHS[i] = HFR_YFR_delta_xFRy[i] + delta_g[ii];
                                }
                        }

                        if ( nZ > 0 )
                        {
                                for( i=0; i<nFR; ++i )
                                {
                                        ii = FR_idx[i];
                                        for( j=0; j<nFR; ++j )
                                        {
                                                jj = FR_idx[j];
                                                delta_yAC_RHS[i] += H[ii*NVMAX + jj] * ZFR_delta_xFRz[j];
                                        }
                                }
                        }

                        if ( nFX > 0 )
                        {
                                if ( Delta_bB_isZero == BT_FALSE )
                                {
                                        for( i=0; i<nFR; ++i )
                                                delta_yAC_RHS[i] += HMX_delta_xFX[i];
                                }
                        }

                        for( i=0; i<nAC; ++i)
                        {
                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        delta_yAC_TMP[i] += Q[jj*NVMAX + nZ+i] * delta_yAC_RHS[j];
                                }
                        }
                }

                if ( backsolveT( delta_yAC_TMP,BT_TRUE,delta_yAC ) != SUCCESSFUL_RETURN )
                        return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
        }


        /* IV) DETERMINE delta_yFX */
        if ( nFX > 0 )
        {
                for( i=0; i<nFX; ++i )
                {
                        ii = FX_idx[i];

                        delta_yFX[i] = delta_g[ii];

                        for( j=0; j<nAC; ++j )
                        {
                                jj = AC_idx[j];
                                delta_yFX[i] -= A[jj*NVMAX + ii] * delta_yAC[j];
                        }

                        if ( hessianType == HST_IDENTITY )
                        {
                                delta_yFX[i] += delta_xFX[i];
                        }
                        else
                        {
                                for( j=0; j<nFR; ++j )
                                {
                                        jj = FR_idx[j];
                                        delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
                                }

                                if ( Delta_bB_isZero == BT_FALSE )
                                {
                                        for( j=0; j<nFX; ++j )
                                        {
                                                jj = FX_idx[j];
                                                delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
                                        }
                                }
                        }
                }
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      h o t s t a r t _ d e t e r m i n e S t e p L e n g t h
 */
returnValue QProblem::hotstart_determineStepLength(     const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
                                                                                                        const real_t* const delta_lbA, const real_t* const delta_ubA,
                                                                                                        const real_t* const delta_lb, const real_t* const delta_ub,
                                                                                                        const real_t* const delta_xFX, const real_t* const delta_xFR,
                                                                                                        const real_t* const delta_yAC, const real_t* const delta_yFX,
                                                                                                        real_t* const delta_Ax, int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
                                                                                                        )
{
        int i, j, ii, jj;
        int nV  = getNV( );
        int nC  = getNC( );
        int nFR = getNFR( );
        int nFX = getNFX( );
        int nAC = getNAC( );
        int nIAC = getNIAC( );

        /* initialise maximum steplength array */
        real_t maxStepLength[2*(NVMAX+NCMAX_ALLOC)];
        for ( i=0; i<2*(nV+nC); ++i )
                maxStepLength[i] = 1.0;


        /* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
        /* 1) Ensure that active dual constraints' bounds remain valid
         *    (ignoring inequality constraints).  */
        for( i=0; i<nAC; ++i )
        {
                ii = AC_idx[i];

                if ( constraints.getType( ii ) != ST_EQUALITY )
                {
                        if ( constraints.getStatus( ii ) == ST_LOWER )
                        {
                                /* active lower constraints' bounds */
                                if ( delta_yAC[i] < -ZERO )
                                {
                                        if ( y[nV+ii] > 0.0 )
                                                maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
                                        else
                                                maxStepLength[nV+ii] = 0.0;
                                }
                        }
                        else
                        {
                                /* active upper constraints' bounds */
                                if ( delta_yAC[i] > ZERO )
                                {
                                        if ( y[nV+ii] < 0.0 )
                                                maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
                                        else
                                                maxStepLength[nV+ii] = 0.0;
                                }
                        }
                }
        }

        /* 2) Ensure that active dual bounds remain valid
         *    (ignoring implicitly fixed variables). */
        for( i=0; i<nFX; ++i )
        {
                ii = FX_idx[i];

                if ( bounds.getType( ii ) != ST_EQUALITY )
                {
                        if ( bounds.getStatus( ii ) == ST_LOWER )
                        {
                                /* active lower bounds */
                                if ( delta_yFX[i] < -ZERO )
                                {
                                        if ( y[ii] > 0.0 )
                                                maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
                                        else
                                                maxStepLength[ii] = 0.0;
                                }
                        }
                        else
                        {
                                /* active upper bounds */
                                if ( delta_yFX[i] > ZERO )
                                {
                                        if ( y[ii] < 0.0 )
                                                maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
                                        else
                                                maxStepLength[ii] = 0.0;
                                }
                        }
                }
        }


        /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
        /* 1) Ensure that inactive constraints' bounds remain valid
         *    (ignoring unbounded constraints). */
        real_t delta_x[NVMAX];
        for( j=0; j<nFR; ++j )
        {
                jj = FR_idx[j];
                delta_x[jj] = delta_xFR[j];
        }
        for( j=0; j<nFX; ++j )
        {
                jj = FX_idx[j];
                delta_x[jj] = delta_xFX[j];
        }

        for( i=0; i<nIAC; ++i )
        {
                ii = IAC_idx[i];

                if ( constraints.getType( ii ) != ST_UNBOUNDED )
                {
                        delta_Ax[ii] = 0.0;
                        for( j=0; j<nV; ++j )
                                delta_Ax[ii] += A[ii*NVMAX + j] * delta_x[j]; // POSSIBLE SPEEDUP!

                        /* inactive lower constraints' bounds */
                        if ( constraints.isNoLower( ) == BT_FALSE )
                        {
                                if ( delta_lbA[ii] > delta_Ax[ii] )
                                {
                                        if ( Ax[ii] > lbA[ii] )
                                                maxStepLength[nV+ii] = ( Ax[ii] - lbA[ii] ) / ( delta_lbA[ii] - delta_Ax[ii] );
                                        else
                                                maxStepLength[nV+ii] = 0.0;
                                }
                        }

                        /* inactive upper constraints' bounds */
                        if ( constraints.isNoUpper( ) == BT_FALSE )
                        {
                                if ( delta_ubA[ii] < delta_Ax[ii] )
                                {
                                        if ( Ax[ii] < ubA[ii] )
                                                maxStepLength[nV+nC+nV+ii] = ( Ax[ii] - ubA[ii] ) / ( delta_ubA[ii] - delta_Ax[ii] );
                                        else
                                                maxStepLength[nV+nC+nV+ii] = 0.0;
                                }
                        }
                }
        }


        /* 2) Ensure that inactive bounds remain valid
         *    (ignoring unbounded variables). */
        /* inactive lower bounds */
        if ( bounds.isNoLower( ) == BT_FALSE )
        {
                for( i=0; i<nFR; ++i )
                {
                        ii = FR_idx[i];
                        if ( bounds.getType( ii ) != ST_UNBOUNDED )
                                if ( delta_lb[ii] > delta_xFR[i] )
                                {
                                        if ( x[ii] > lb[ii] )
                                                maxStepLength[ii] = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
                                        else
                                                maxStepLength[ii] = 0.0;
                                }
                }
        }

        /* inactive upper bounds */
        if ( bounds.isNoUpper( ) == BT_FALSE )
        {
                for( i=0; i<nFR; ++i )
                {
                        ii = FR_idx[i];
                        if ( bounds.getType( ii ) != ST_UNBOUNDED )
                                if ( delta_ub[ii] < delta_xFR[i] )
                                {
                                        if ( x[ii] < ub[ii] )
                                                maxStepLength[nV+nC+ii] = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
                                        else
                                                maxStepLength[nV+nC+ii] = 0.0;
                                }
                }
        }


        /* III) DETERMINE MAXIMUM HOMOTOPY STEPLENGTH */
        real_t tau_new = 1.0;

        BC_idx = 0;
        BC_status = ST_UNDEFINED;
        BC_isBound = BT_FALSE;

        for ( i=0; i<nV; ++i )
        {
                /* 1) Consider lower/dual blocking bounds. */
                if ( maxStepLength[i] < tau_new )
                {
                        tau_new = maxStepLength[i];
                        BC_idx = i;
                        BC_isBound = BT_TRUE;
                        if ( bounds.getStatus( i ) == ST_INACTIVE ) /* inactive? */
                                BC_status = ST_LOWER;
                        else
                                BC_status = ST_INACTIVE;
                }

                /* 2) Consider upper blocking bounds. */
                if ( maxStepLength[nV+nC+i] < tau_new )
                {
                        tau_new = maxStepLength[nV+nC+i];
                        BC_idx = i;
                        BC_isBound = BT_TRUE;
                        BC_status = ST_UPPER;
                }
        }

        for ( i=nV; i<nV+nC; ++i )
        {
                /* 3) Consider lower/dual blocking constraints. */
                if ( maxStepLength[i] < tau_new )
                {
                        tau_new = maxStepLength[i];
                        BC_idx = i-nV;
                        BC_isBound = BT_FALSE;
                        if ( constraints.getStatus( i-nV ) == ST_INACTIVE ) /* inactive? */
                                BC_status = ST_LOWER;
                        else
                                BC_status = ST_INACTIVE;
                }

                /* 4) Consider upper blocking constraints. */
                if ( maxStepLength[nV+nC+i] < tau_new )
                {
                        tau_new = maxStepLength[nV+nC+i];
                        BC_idx = i-nV;
                        BC_isBound = BT_FALSE;
                        BC_status = ST_UPPER;
                }
        }


        /* IV) CLEAR CYCLING DATA
         *     if a positive step can be taken */
        if ( tau_new > EPS )
                cyclingManager.clearCyclingData( );


        /* V) SET MAXIMUM HOMOTOPY STEPLENGTH */
        tau = tau_new;

        #ifdef PC_DEBUG
        if ( printlevel == PL_HIGH )
        {

                char messageString[80];

                if ( BC_status == ST_UNDEFINED )
                        sprintf( messageString,"Stepsize is %.6e!",tau );
                else
                        sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)",tau,BC_idx,BC_isBound,BC_status );

                getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
        }
        #endif

        return SUCCESSFUL_RETURN;
}


/*
 *      h o t s t a r t _ p e r f o r m S t e p
 */
returnValue QProblem::hotstart_performStep(     const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
                                                                                        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,
                                                                                        const real_t* const delta_xFX, const real_t* const delta_xFR,
                                                                                        const real_t* const delta_yAC, const real_t* const delta_yFX,
                                                                                        const real_t* const delta_Ax, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
                                                                                        )
{
        int i, j, ii;
        int nV  = getNV( );
        int nC  = getNC( );
        int nFR = getNFR( );
        int nFX = getNFX( );
        int nAC = getNAC( );
        int nIAC = getNIAC( );


        /* I) CHECK (CONSTRAINTS') BOUNDS' CONSISTENCY */
        if ( areBoundsConsistent( delta_lb,delta_ub,delta_lbA,delta_ubA ) == BT_FALSE )
        {
                infeasible = BT_TRUE;
                tau = 0.0;

                return THROWERROR( RET_QP_INFEASIBLE );
        }


        /* II) GO TO ACTIVE SET CHANGE */
        if ( tau > ZERO )
        {
                /* 1) Perform step in primal und dual space... */
                for( i=0; i<nFR; ++i )
                {
                        ii = FR_idx[i];
                        x[ii] += tau*delta_xFR[i];
                }

                for( i=0; i<nFX; ++i )
                {
                        ii = FX_idx[i];
                        x[ii] += tau*delta_xFX[i];
                        y[ii] += tau*delta_yFX[i];
                }

                for( i=0; i<nAC; ++i )
                {
                        ii = AC_idx[i];
                        y[nV+ii] += tau*delta_yAC[i];
                }

                /* ... also for Ax. */
                for( i=0; i<nIAC; ++i )
                {
                        ii = IAC_idx[i];
                        if ( constraints.getType( ii ) != ST_UNBOUNDED )
                                Ax[ii] += tau*delta_Ax[ii];
                }
                for( i=0; i<nAC; ++i )
                {
                        ii = AC_idx[i];

                        Ax[ii] = 0.0;
                        for( j=0; j<nV; ++j )
                                Ax[ii] += A[ii*NVMAX + j] * x[j];
                }

                /* 2) Shift QP data. */
                for( i=0; i<nV; ++i )
                {
                        g[i]  += tau*delta_g[i];
                        lb[i] += tau*delta_lb[i];
                        ub[i] += tau*delta_ub[i];
                }

                for( i=0; i<nC; ++i )
                {
                        lbA[i] += tau*delta_lbA[i];
                        ubA[i] += tau*delta_ubA[i];
                }
        }
        else
        {
                /* print a stepsize warning if stepsize is zero */
                #ifdef PC_DEBUG
                char messageString[80];
                sprintf( messageString,"Stepsize is %.6e",tau );
                getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
                #endif
        }


        /* setup output preferences */
        #ifdef PC_DEBUG
        char messageString[80];
        VisibilityStatus visibilityStatus;

        if ( printlevel == PL_HIGH )
                visibilityStatus = VS_VISIBLE;
        else
                visibilityStatus = VS_HIDDEN;
        #endif

        
        /* III) UPDATE ACTIVE SET */
        switch ( BC_status )
        {
                /* Optimal solution found as no working set change detected. */
                case ST_UNDEFINED:
                        return RET_OPTIMAL_SOLUTION_FOUND;


                /* Remove one variable from active set. */
                case ST_INACTIVE:
                        if ( BC_isBound == BT_TRUE )
                        {
                                #ifdef PC_DEBUG
                                sprintf( messageString,"bound no. %d.", BC_idx );
                                getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                                #endif

                                if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                                y[BC_idx] = 0.0;
                        }
                        else
                        {
                                #ifdef PC_DEBUG
                                sprintf( messageString,"constraint no. %d.", BC_idx );
                                getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                                #endif

                                if ( removeConstraint( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );

                                y[nV+BC_idx] = 0.0;
                        }
                        break;


                /* Add one variable to active set. */
                default:
                        if ( BC_isBound == BT_TRUE )
                        {
                                #ifdef PC_DEBUG
                                if ( BC_status == ST_LOWER )
                                        sprintf( messageString,"lower bound no. %d.", BC_idx );
                                else
                                        sprintf( messageString,"upper bound no. %d.", BC_idx );
                                getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                                #endif

                                if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
                        }
                        else
                        {
                                #ifdef PC_DEBUG
                                if ( BC_status == ST_LOWER )
                                        sprintf( messageString,"lower constraint's bound no. %d.", BC_idx );
                                else
                                        sprintf( messageString,"upper constraint's bound no. %d.", BC_idx );
                                getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
                                #endif

                                if ( addConstraint( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
                                        return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
                        }
                        break;
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      a r e B o u n d s C o n s i s t e n t
 */
BooleanType QProblem::areBoundsConsistent(      const real_t* const delta_lb, const real_t* const delta_ub,
                                                                                        const real_t* const delta_lbA, const real_t* const delta_ubA
                                                                                        ) const
{
        int i;

        /* 1) Check bounds' consistency. */
        if ( QProblemB::areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
                return BT_FALSE;

        /* 2) Check constraints' consistency, i.e.
         *    check if delta_lb[i] is greater than delta_ub[i]
         *    for a component i whose bounds are already (numerically) equal. */
        for( i=0; i<getNC( ); ++i )
                if ( ( lbA[i] > ubA[i] - BOUNDTOL ) && ( delta_lbA[i] > delta_ubA[i] + EPS ) )
                        return BT_FALSE;

        return BT_TRUE;
}


/*
 *      s e t u p Q P d a t a
 */
returnValue QProblem::setupQPdata(      const real_t* const _H, const real_t* const _g, const real_t* const _A,
                                                                        const real_t* const _lb, const real_t* const _ub,
                                                                        const real_t* const _lbA, const real_t* const _ubA
                                                                        )
{
        int i, j;
        int nV = getNV( );
        int nC = getNC( );


        /* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
        if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        /* 2) Load constraint matrix. */
        if ( ( nC > 0 ) && ( _A == 0 ) )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        if ( nC > 0 )
        {
                for( i=0; i<nC; ++i )
                        for( j=0; j<nV; ++j )
                                A[i*NVMAX + j] = _A[i*nV + j];

                /* 3) Setup lower constraints' bounds vector. */
                if ( _lbA != 0 )
                {
                        for( i=0; i<nC; ++i )
                                lbA[i] = _lbA[i];
                }
                else
                {
                        /* if no lower constraints' bounds are specified, set them to -infinity */
                        for( i=0; i<nC; ++i )
                                lbA[i] = -INFTY;
                }

                /* 4) Setup upper constraints' bounds vector. */
                if ( _ubA != 0 )
                {
                        for( i=0; i<nC; ++i )
                                ubA[i] = _ubA[i];
                }
                else
                {
                        /* if no upper constraints' bounds are specified, set them to infinity */
                        for( i=0; i<nC; ++i )
                                ubA[i] = INFTY;
                }
        }

//      printmatrix2( "A",A,10,20 );
        
//      printmatrix2( "lbA",lbA,1,nC );
//      printmatrix2( "ubA",ubA,1,nC );

        return SUCCESSFUL_RETURN;
}



#ifdef PC_DEBUG  /* Define print functions only for debugging! */

/*
 *      p r i n t I t e r a t i o n
 */
returnValue QProblem::printIteration(   int iteration,
                                                                                int BC_idx,     SubjectToStatus BC_status, BooleanType BC_isBound
                                                                                )
{
        char myPrintfString[160];

        /* consistency check */
        if ( iteration < 0 )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        /* nothing to do */
        if ( printlevel != PL_MEDIUM )
                return SUCCESSFUL_RETURN;


        /* 1) Print header at first iteration. */
        if ( iteration == 0 )
        {
                sprintf( myPrintfString,"\n##############  qpOASES  --  QP NO.%4.1d  ###############\n", count );
                myPrintf( myPrintfString );

                sprintf( myPrintfString,"  Iter  |  StepLength   |     Info      |  nFX  |  nAC  \n" );
                myPrintf( myPrintfString );
        }

        /* 2) Print iteration line. */
        if ( BC_status == ST_UNDEFINED )
        {
                sprintf( myPrintfString,"  %4.1d  |  %1.5e  |   QP SOLVED   | %4.1d  | %4.1d  \n", iteration,tau,getNFX( ),getNAC( ) );
                myPrintf( myPrintfString );
        }
        else
        {
                char info[8];

                if ( BC_status == ST_INACTIVE )
                        sprintf( info,"REM " );
                else
                        sprintf( info,"ADD " );

                if ( BC_isBound == BT_TRUE )
                        sprintf( &(info[4]),"BND" );
                else
                        sprintf( &(info[4]),"CON" );

                sprintf( myPrintfString,"  %4.1d  |  %1.5e  |  %s%4.1d  | %4.1d  | %4.1d  \n", iteration,tau,info,BC_idx,getNFX( ),getNAC( ) );
                myPrintf( myPrintfString );
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      p r i n t I t e r a t i o n
 */
returnValue QProblem::printIteration(   int iteration,
                                                                                int BC_idx,     SubjectToStatus BC_status
                                                                                )
{
        return printIteration( iteration,BC_idx,BC_status,BT_TRUE );
}

#endif  /* PC_DEBUG */



/*
 *      c h e c k K K T c o n d i t i o n s
 */
returnValue QProblem::checkKKTconditions( )
{
        #ifdef __PERFORM_KKT_TEST__

        int i, j, jj;
        int nV  = getNV( );
        int nC  = getNC( );
        int nAC = getNAC( );

        real_t tmp;
        real_t maxKKTviolation = 0.0;

        int AC_idx[NCMAX_ALLOC];
        constraints.getActive( )->getNumberArray( AC_idx );

        /* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
        for( i=0; i<nV; ++i )
        {
                tmp = g[i];

                for( j=0; j<nV; ++j )
                        tmp += H[i*NVMAX + j] * x[j];

                tmp -= y[i];

                /* Only sum over active constraints as y is zero for all inactive ones. */
                for( j=0; j<nAC; ++j )
                {
                        jj = AC_idx[j];
                        tmp -= A[jj*NVMAX + i] * y[nV+jj];
                }

                if ( getAbs( tmp ) > maxKKTviolation )
                        maxKKTviolation = getAbs( tmp );
        }

        /* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
        /* lbA <= Ax <= ubA */
        for( i=0; i<nC; ++i )
        {
                if ( lbA[i] - Ax[i] > maxKKTviolation )
                        maxKKTviolation = lbA[i] - Ax[i];

                if ( Ax[i] - ubA[i] > maxKKTviolation )
                        maxKKTviolation = Ax[i] - ubA[i];
        }

        /* lb <= x <= ub */
        for( i=0; i<nV; ++i )
        {
                if ( lb[i] - x[i] > maxKKTviolation )
                        maxKKTviolation = lb[i] - x[i];

                if ( x[i] - ub[i] > maxKKTviolation )
                        maxKKTviolation = x[i] - ub[i];
        }

        /* 3) Check for correct sign of y and for complementary slackness. */
        /* bounds */
        for( i=0; i<nV; ++i )
        {
                switch ( bounds.getStatus( i ) )
                {
                        case ST_LOWER:
                                if ( -y[i] > maxKKTviolation )
                                        maxKKTviolation = -y[i];
                                if ( getAbs( x[i] - lb[i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( x[i] - lb[i] );
                                break;

                        case ST_UPPER:
                                if ( y[i] > maxKKTviolation )
                                        maxKKTviolation = y[i];
                                if ( getAbs( ub[i] - x[i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( ub[i] - x[i] );
                                break;

                        default: /* inactive */
                        if ( getAbs( y[i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( y[i] );
                                break;
                }
        }

        /* constraints */
        for( i=0; i<nC; ++i )
        {
                switch ( constraints.getStatus( i ) )
                {
                        case ST_LOWER:
                                if ( -y[nV+i] > maxKKTviolation )
                                        maxKKTviolation = -y[nV+i];
                                if ( getAbs( Ax[i] - lbA[i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( Ax[i] - lbA[i] );
                                break;

                        case ST_UPPER:
                                if ( y[nV+i] > maxKKTviolation )
                                        maxKKTviolation = y[nV+i];
                                if ( getAbs( ubA[i] - Ax[i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( ubA[i] - Ax[i] );
                                break;

                        default: /* inactive */
                        if ( getAbs( y[nV+i] ) > maxKKTviolation )
                                        maxKKTviolation = getAbs( y[nV+i] );
                                break;
                }
        }

        if ( maxKKTviolation > CRITICALACCURACY )
                return RET_NO_SOLUTION;

        if ( maxKKTviolation > DESIREDACCURACY )
                return RET_INACCURATE_SOLUTION;

        #endif /* __PERFORM_KKT_TEST__ */

        return SUCCESSFUL_RETURN;
}


/*
 *      end of file
 */