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


/**
 *	\file src/SQProblem.cpp
 *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
 *	\version 3.2
 *	\date 2007-2015
 *
 *	Implementation of the SQProblem class which is able to use the newly
 *	developed online active set strategy for parametric quadratic programming
 *	with varying matrices.
 */


#include <qpOASES/SQProblem.hpp>


BEGIN_NAMESPACE_QPOASES


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


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


/*
 *	S Q P r o b l e m
 */
SQProblem::SQProblem( int_t _nV, int_t _nC, HessianType _hessianType ) : QProblem( _nV,_nC,_hessianType )
{
}


/*
 *	S Q P r o b l e m
 */
SQProblem::SQProblem( const SQProblem& rhs ) : QProblem( rhs )
{
}


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


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

	return *this;
}



/*
 * h o t s t a r t
 */
returnValue SQProblem::hotstart(	SymmetricMatrix *H_new, const real_t* const g_new, Matrix *A_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_t& nWSR, real_t* const cputime,
									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
									)
{
	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
	{
		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
	}


	real_t starttime = 0.0;
	real_t auxTime = 0.0;

    if ( cputime != 0 )
        starttime = getCPUtime( );


	/* I) UPDATE QP MATRICES AND VECTORS */
	if ( setupNewAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );


	/* II) PERFORM USUAL HOMOTOPY */

	/* Allow only remaining CPU time for usual hotstart. */
	if ( cputime != 0 )
	{
		auxTime = getCPUtime( ) - starttime;
		*cputime -= auxTime;
	}

	returnValue returnvalue = QProblem::hotstart(	g_new,lb_new,ub_new,lbA_new,ubA_new,
													nWSR,cputime,
													guessedBounds,guessedConstraints
													);
	
	if ( cputime != 0 )
		*cputime += auxTime;

	return returnvalue;
}


/*
 *	h o t s t a r t
 */
returnValue SQProblem::hotstart(	const real_t* const H_new, const real_t* const g_new, const real_t* const A_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_t& nWSR, real_t* const cputime,
									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
									)
{
	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) UPDATE QP MATRICES AND VECTORS */
	if ( setupNewAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );


	/* II) PERFORM USUAL HOMOTOPY */

	/* Allow only remaining CPU time for usual hotstart. */
	if ( cputime != 0 )
		*cputime -= getCPUtime( ) - starttime;

	returnValue returnvalue = QProblem::hotstart(	g_new,lb_new,ub_new,lbA_new,ubA_new,
													nWSR,cputime,
													guessedBounds,guessedConstraints
													);


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

	return returnvalue;
}


/*
 *	h o t s t a r t
 */
returnValue SQProblem::hotstart(	const char* const H_file, const char* const g_file, const char* const A_file,
									const char* const lb_file, const char* const ub_file,
									const char* const lbA_file, const char* const ubA_file,
									int_t& nWSR, real_t* const cputime,
									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
									)
{
	int_t nV = getNV( );
	int_t nC = getNC( );

	returnValue returnvalue;

	/* consistency checks */
	if ( ( H_file == 0 ) || ( g_file == 0 ) )
		return THROWERROR( RET_INVALID_ARGUMENTS );

	if ( ( nC > 0 ) && ( A_file == 0 ) )
		return THROWERROR( RET_INVALID_ARGUMENTS );


	/* 1) Load new QP matrices from files. */
	real_t* H_new  = new real_t[nV*nV];
	real_t* A_new  = new real_t[nC*nV];

	if ( readFromFile( H_new, nV,nV, H_file ) != SUCCESSFUL_RETURN )
	{
		delete[] A_new;
		delete[] H_new;
		return THROWERROR( RET_UNABLE_TO_READ_FILE );
	}

	if ( readFromFile( A_new, nC,nV, A_file ) != SUCCESSFUL_RETURN )
	{
		delete[] A_new;
		delete[] H_new;
		return THROWERROR( RET_UNABLE_TO_READ_FILE );
	}

	/* 2) Load new QP vectors from files. */
	real_t* g_new   = new real_t[nV];
	real_t* lb_new  = ( lb_file != 0 )  ? new real_t[nV] : 0;
	real_t* ub_new  = ( ub_file != 0 )  ? new real_t[nV] : 0;
	real_t* lbA_new = ( lbA_file != 0 ) ? new real_t[nC] : 0;
	real_t* ubA_new = ( ubA_file != 0 ) ? new real_t[nC] : 0;

	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
											g_new,lb_new,ub_new,lbA_new,ubA_new
											);
	if ( returnvalue != SUCCESSFUL_RETURN )
	{
		if ( ubA_file != 0 )
			delete[] ubA_new;
		if ( lbA_file != 0 )
			delete[] lbA_new;
		if ( ub_file != 0 )
			delete[] ub_new;
		if ( lb_file != 0 )
			delete[] lb_new;
		delete[] g_new;
		delete[] A_new;
		delete[] H_new;

		return THROWERROR( RET_UNABLE_TO_READ_FILE );
	}

	/* 3) Actually perform hotstart. */
	returnvalue = hotstart(	H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new,
							nWSR,cputime,
							guessedBounds,guessedConstraints
							);

	if ( ubA_file != 0 )
		delete[] ubA_new;
	if ( lbA_file != 0 )
		delete[] lbA_new;
	if ( ub_file != 0 )
		delete[] ub_new;
	if ( lb_file != 0 )
		delete[] lb_new;
	delete[] g_new;
	delete[] A_new;
	delete[] H_new;

	return returnvalue;
}


/*
 *	h o t s t a r t
 */
returnValue SQProblem::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_t& nWSR, real_t* const cputime,
									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
									)
{
	/* Call to hotstart function for fixed QP matrices. */
	return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints );
}


/*
 *	h o t s t a r t
 */
returnValue SQProblem::hotstart(	const char* const g_file,
									const char* const lb_file, const char* const ub_file,
									const char* const lbA_file, const char* const ubA_file,
									int_t& nWSR, real_t* const cputime,
									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
									)
{
	/* Call to hotstart function for fixed QP matrices. */
	return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime, guessedBounds,guessedConstraints );
}



#ifdef __MATLAB__
returnValue SQProblem::resetMatrixPointers( )
{
	H = 0;
	A = 0;

	return SUCCESSFUL_RETURN;
}
#endif



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

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

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

	status = QPS_PREPARINGAUXILIARYQP;


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

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

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

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

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

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

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

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

			if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
				return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
		}
	}
	else
	{
		/* if no Hessian is specified, keep previous trivial Hessian (HST_ZERO or HST_IDENTITY),
		   otherwise abort */
		if ( H != 0 )
			return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
	}

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


	/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
	/* 1) Make a copy of current bounds/constraints ... */
	Bounds      oldBounds      = bounds;
	Constraints oldConstraints = constraints;

    /* we're trying to find an active set with positive definite null
     * space Hessian twice:
     * - first for the current active set including all equalities
     * - second after moving all inactive variables to a bound
     *   (depending on Options). This creates an empty null space and
     *   is guaranteed to succeed. Thus this loop will exit after n_try=1.
     */
    int_t n_try;
    for (n_try = 0; n_try < 2; ++n_try) {

        if (n_try > 0) {
            // the current active set leaves an indefinite null space Hessian
            // move all inactive variables to a bound, creating an empty null space
            for (int_t ii = 0; ii < nV; ++ii)
                if (oldBounds.getStatus (ii) == ST_INACTIVE)
                    oldBounds.setStatus (ii, options.initialStatusBounds);
        }

        /*    ... reset them ... */
        bounds.init( nV );
        constraints.init( nC );

        /*    ... and set them up afresh. */
        if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

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

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

        /* 2) Setup TQ factorisation. */
        if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

		// check for equalities that have become bounds ...
		for (int_t ii = 0; ii < nC; ++ii) {
			if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
				if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
					oldConstraints.setStatus (ii, ST_UPPER);
				else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
					oldConstraints.setStatus (ii, ST_LOWER);
			}
		}

		// ... and do the same also for the bounds!
		for (int_t ii = 0; ii < nV; ++ii) {
			if (oldBounds.getType(ii) == ST_EQUALITY
					&& bounds.getType(ii) == ST_BOUNDED) {
				if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
					oldBounds.setStatus(ii, ST_UPPER);
				else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
					oldBounds.setStatus(ii, ST_LOWER);
			}
		}

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

        /* Factorise projected Hessian
        * this now handles all special cases (no active bounds/constraints, no nullspace) */
        returnvalue = computeProjectedCholesky( );

        /* leave the loop if decomposition was successful, i.e. we have
         * found an active set with positive definite null space Hessian */
        if ( returnvalue == SUCCESSFUL_RETURN )
            break;
    }

    /* adjust lb/ub if we changed the old active set in the second try
     */
    if (n_try > 0) {
		// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
		for (int_t ii = 0; ii < nC; ++ii)
			Ax_l[ii] = Ax_u[ii] = Ax[ii];
        setupAuxiliaryQPbounds( &bounds, &constraints, BT_FALSE );
	}

	status = QPS_AUXILIARYQPSOLVED;

	return SUCCESSFUL_RETURN;
}


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

	DenseMatrix *dA = 0;
	SymDenseMat *sH = 0;

	if ( A_new != 0 )
	{
		dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
	}
	else
	{
		if ( nC > 0 )
			return THROWERROR( RET_INVALID_ARGUMENTS );
	}

	if ( H_new != 0 )
		sH = new SymDenseMat(nV, nV, nV, (real_t*) H_new);

	returnValue returnvalue = setupNewAuxiliaryQP( sH,dA, lb_new,ub_new,lbA_new,ubA_new );

	if ( H_new != 0 )
		freeHessian = BT_TRUE;
	freeConstraintMatrix = BT_TRUE;

	return returnvalue;
}


END_NAMESPACE_QPOASES


/*
 *	end of file
 */