/*
 *	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 examples/example5.cpp
 *	\author Andreas Potschka, Christian Kirches
 *	\version 3.2
 *	\date 2011-2015
 *
 *	Very simple example for testing qpOASES (using the possibility to
 *  compute the local linear feedback law)
 */



#include <stdlib.h>

#include <qpOASES.hpp>
#include "example4CP.cpp"


/**	Example for qpOASES main function using the possibility to specify
 *	user-defined constraint product function. */
int main( )
{
	USING_NAMESPACE_QPOASES

	int_t i,j,jj;
	real_t d = 0.0;

	/* Setup data of first QP... */
	real_t H[7*7];
	real_t A[50*7];
	real_t g[7];
	real_t lbA[50];

	/*	    ( 1.0 0.5 |                    )
	 *	    ( 0.5 2.0 |                    )
	 *	    ( --------+------------------- )
	 *	H = (         | 1e-6               )
	 *	    (         |      1e-6          )
	 *	    (         |           ...      )
	 *	    (         |               1e-6 ) */
	for( i=0; i<7*7; ++i )
		H[i] = 0.0;
	for( i=2; i<7; ++i )
		H[i*7+i] = 1.0e-6;
	H[0] = 1.0;
	H[1] = 0.5;
	H[7] = 0.5;
	H[8] = 2.0;

	/*	    ( x.x x.x | 1.0             )
	 *	    ( x.x x.x | ...             )
	 *	    ( x.x x.x | 1.0             )
	 *	    ( x.x x.x |     1.0         )
	 *	A = ( x.x x.x |     ...         )
	 *	    ( x.x x.x |     1.0         )
	 *	    ( x.x x.x |         ...     )
	 *	    ( x.x x.x |             1.0 )
	 *	    ( x.x x.x |             ... )
	 *	    ( x.x x.x |             1.0 ) */
	for( i=0; i<50*7; ++i )
		A[i] = 0.0;
	for( i=0; i<50; ++i )
	{
		for( j=0; j<2; ++j )
			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;

		A[i*7 + (i/10)+2] = 1.0;
	}

	/*	    ( -1.0 )
	 *	    ( -0.5 )
	 *	    ( ---- )
	 *	g = (      )
	 *	    (      )
	 *	    (      )
	 *	    (      ) */
	for( i=0; i<7; ++i )
		g[i] = 0.0;
	g[0] = -1.0;
	g[1] = -0.5;

	for( i=0; i<50; ++i )
		lbA[i] = 1.0;

	/* ... and setting up user-defined constraint product function. */
	MyConstraintProduct myCP( 7,50,A );


	/* Setting up QProblem object and set construct product function. */
	QProblem example( 7,50 );
	example.setConstraintProduct( &myCP );


	/* Solve first QP. */
	real_t cputime = 1.0;
	int_t nWSR = 100;
	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );

	/* Get and print solution of QP. */
	real_t xOpt[7], yOpt[7+50];
	example.getPrimalSolution( xOpt );
	example.getDualSolution( yOpt );


	/* Compute local linear feedback law */
	const int_t n_rhs = 7+7+50;
	real_t g_in[7*n_rhs];
	real_t b_in[7*n_rhs];
	real_t bA_in[50*n_rhs];
	real_t x_out[7*n_rhs];
	real_t y_out[(7+50)*n_rhs];

	int_t ii;
	memset (g_in, 0, sizeof (g_in));
	memset (b_in, 0, sizeof (b_in));
	memset (bA_in, 0, sizeof (bA_in));

	for ( ii = 0; ii < 7; ++ii )
		g_in[ii*7 + ii] = 1.0;
	for ( ii = 0; ii < 7; ++ii )
		b_in[(ii+7)*7 + ii] = 1.0;
	for ( ii = 0; ii < 50; ++ii )
		bA_in[(ii+14)*50 + ii] = 1.0;

	example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );

	/* Verify validity of local feedback law by perturbation and hot starts */
	real_t perturb = 1.0e-6;
	real_t nrm = 0.0;
	for ( ii = 0; ii < n_rhs; ++ii )
	{
		for ( jj = 0; jj < 7; ++jj )
			g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
		for ( jj = 0; jj < 50; ++jj )
			bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;

		nWSR = 100;
		example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );

		real_t xPer[7], yPer[7+50];
		example.getPrimalSolution( xPer );
		example.getDualSolution( yPer );

		for ( jj = 0; jj < 7; ++jj )
		{
			d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
			if (nrm < d) nrm=d;
		}
		for ( jj = 0; jj < 7+50; ++jj )
		{
			d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
			if (nrm < d) nrm=d;
		}
	}
	printf ("Maximum perturbation over all directions: %e\n", nrm);

	/* // print feedback matrix
	for (ii = 0; ii < n_rhs; ++ii)
	{
		printf ("x: ");
		for (jj = 0; jj < 7; ++jj )
			printf ("%8.2e ", x_out[ii*7+jj]);
		printf (" y: ");
		for (jj = 0; jj < 7+50; ++jj )
			printf ("%8.2e ", y_out[ii*(7+50)+jj]);
		printf("\n");
	}
*/

	return 0;
}


/*
 *	end of file
 */