/*
 *	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/SubjectTo.cpp
 *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
 *	\version 3.2
 *	\date 2007-2015
 *
 *	Implementation of the SubjectTo class designed to manage working sets of
 *	constraints and bounds within a QProblem.
 */


#include <qpOASES/SubjectTo.hpp>


BEGIN_NAMESPACE_QPOASES


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


/*
 *	S u b j e c t T o
 */
SubjectTo::SubjectTo( )
{
	type   = 0;
	status = 0;

	init( );
}


/*
 *	S u b j e c t T o
 */
SubjectTo::SubjectTo( int_t _n )
{
	type   = 0;
	status = 0;

	init( _n );
}


/*
 *	S u b j e c t T o
 */
SubjectTo::SubjectTo( const SubjectTo& rhs )
{
	copy( rhs );
}


/*
 *	~ S u b j e c t T o
 */
SubjectTo::~SubjectTo( )
{
	clear( );
}


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

	return *this;
}


/*
 *	i n i t
 */
returnValue SubjectTo::init(	int_t _n
								)
{
	int_t i;

	if ( _n < 0 )
		return THROWERROR( RET_INVALID_ARGUMENTS );

	clear( );

	n = _n;
	noLower = BT_TRUE;
	noUpper = BT_TRUE;

	if ( n > 0 )
	{
		type   = new SubjectToType[n];
		status = new SubjectToStatus[n];

		for( i=0; i<n; ++i )
		{
			type[i]   = ST_UNKNOWN;
			status[i] = ST_UNDEFINED;
		}
	}

	return SUCCESSFUL_RETURN;
}



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

/*
 *	c l e a r
 */
returnValue SubjectTo::clear( )
{
	if ( type != 0 )
	{
		delete[] type;
		type = 0;
	}

	if ( status != 0 )
	{
		delete[] status;
		status = 0;
	}

	return SUCCESSFUL_RETURN;
}


/*
 *	c o p y
 */
returnValue SubjectTo::copy(	const SubjectTo& rhs
								)
{
	int_t i;

	n = rhs.n;
	noLower = rhs.noLower;
	noUpper = rhs.noUpper;

	if ( rhs.n != 0 )
	{
		type   = new SubjectToType[n];
		status = new SubjectToStatus[n];

		for( i=0; i<n; ++i )
		{
			type[i]   = rhs.type[i];
			status[i] = rhs.status[i];
		}
	}
	else
	{
		type   = 0;
		status = 0;
	}

	return SUCCESSFUL_RETURN;
}


/*
 *	a d d I n d e x
 */
returnValue SubjectTo::addIndex(	Indexlist* const indexlist,
									int_t newnumber, SubjectToStatus newstatus
									)
{
	if ( status != 0 )
	{
		/* consistency check */
		if ( status[newnumber] == newstatus )
			return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );

		status[newnumber] = newstatus;
	}
	else
		return THROWERROR( RET_ADDINDEX_FAILED );

	if ( indexlist != 0 )
	{
		if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
			return THROWERROR( RET_ADDINDEX_FAILED );
	}
	else
		return THROWERROR( RET_INVALID_ARGUMENTS );

	return SUCCESSFUL_RETURN;
}


/*
 *	r e m o v e I n d e x
 */
returnValue SubjectTo::removeIndex(	Indexlist* const indexlist,
									int_t removenumber
									)
{
	if ( status != 0 )
		status[removenumber] = ST_UNDEFINED;
	else
		return THROWERROR( RET_REMOVEINDEX_FAILED );

	if ( indexlist != 0 )
	{
		if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
			return THROWERROR( RET_REMOVEINDEX_FAILED );
	}
	else
		return THROWERROR( RET_INVALID_ARGUMENTS );

	return SUCCESSFUL_RETURN;
}


/*
 *	s w a p I n d e x
 */
returnValue SubjectTo::swapIndex(	Indexlist* const indexlist,
									int_t number1, int_t number2
									)
{
	/* consistency checks */
	if ( status != 0 )
	{
		if ( status[number1] != status[number2] )
			return THROWERROR( RET_SWAPINDEX_FAILED );
	}
	else
		return THROWERROR( RET_SWAPINDEX_FAILED );

	if ( number1 == number2 )
	{
		THROWWARNING( RET_NOTHING_TO_DO );
		return SUCCESSFUL_RETURN;
	}

	if ( indexlist != 0 )
	{
		if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
			return THROWERROR( RET_SWAPINDEX_FAILED );
	}
	else
		return THROWERROR( RET_INVALID_ARGUMENTS );

	return SUCCESSFUL_RETURN;
}


END_NAMESPACE_QPOASES


/*
 *	end of file
 */