returnValue ExportGaussNewtonForces::setupEvaluation( )
{
	////////////////////////////////////////////////////////////////////////////
	//
	// Setup preparation phase
	//
	////////////////////////////////////////////////////////////////////////////
	preparation.setup("preparationStep");
	preparation.doc( "Preparation step of the RTI scheme." );

	ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
	retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
	preparation.setReturnValue(retSim, false);

	preparation	<< retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";

	preparation.addFunctionCall( evaluateObjective );
	preparation.addFunctionCall( evaluateConstraints );

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup feedback phase
	//
	////////////////////////////////////////////////////////////////////////////
	ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
	ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
	returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
	feedback.setup("feedbackStep" );
	feedback.doc( "Feedback/estimation step of the RTI scheme." );
	feedback.setReturnValue( returnValueFeedbackPhase );

	feedback.addStatement(
			//			cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
			cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose()
	);
	feedback.addLinebreak();

	// Calculate objective residuals
	feedback << (Dy -= y) << (DyN -= yN);
	feedback.addLinebreak();

	for (unsigned i = 0; i < N; ++i)
		feedback.addFunctionCall(setStagef, objGradients[ i ], ExportIndex( i ));
	feedback.addStatement( objGradients[ N ] == QN2 * DyN );
	feedback.addLinebreak();

	//
	// Configure output variables
	//
	std::vector< ExportVariable > vecQPVars;

	vecQPVars.clear();
	vecQPVars.resize(N + 1);
	for (unsigned i = 0; i < N; ++i)
		vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
	vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);

	//
	// In case warm starting is enabled, give an initial guess, based on the old solution
	//
	int hotstartQP;
	get(HOTSTART_QP, hotstartQP);

	if ( hotstartQP )
	{
		std::vector< ExportVariable > zInit;

		zInit.clear();
		zInit.resize(N + 1);
		for (unsigned i = 0; i < N; ++i)
		{
			string name = "z_init_";
			name = name + (i < 10 ? "0" : "") + toString( i );
			zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
		}
		string name = "z_init_";
		name = name + (N < 10 ? "0" : "") + toString( N );
		zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);

		// TODO This should be further investigated.

		//
		// 1) Just use the old solution
		//
		//		for (unsigned blk = 0; blk < N + 1; blk++)
		//			feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] );

		//
		// 2) Initialization by shifting
		//

		//		for (unsigned blk = 0; blk < N - 1; blk++)
		//			feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] );
		//		for (unsigned el = 0; el < NX; el++)
		//			feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) );
	}

	//
	// Call a QP solver
	// NOTE: we need two prefixes:
	// 1. module prefix
	// 2. structure instance prefix
	//
	ExportFunction solveQP;
	solveQP.setup("solve");

	feedback
	<< returnValueFeedbackPhase.getFullName() << " = "
	<< qpModuleName << "_" << solveQP.getName() << "( "
	<< "&" << qpObjPrefix << "_" << "params" << ", "
	<< "&" << qpObjPrefix << "_" << "output" << ", "
	<< "&" << qpObjPrefix << "_" << "info" << " );\n";
	feedback.addLinebreak();

	//
	// Here we have to add the differences....
	//

	ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
	ExportIndex index( "index" );
	acc.setup("accumulate", stageOut, index);

	acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) );
	acc.addLinebreak();
	acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) );
	acc.addLinebreak();

	for (unsigned i = 0; i < N; ++i)
		feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i ));
	feedback.addLinebreak();

	feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() );
	feedback.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// TODO Setup evaluation of KKT
	//
	////////////////////////////////////////////////////////////////////////////
	ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);

	getKKT.setup( "getKKT" );
	getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
	//	kkt.setDoc( "The KKT tolerance value." );
	kkt.setDoc( "1e-15." );
	getKKT.setReturnValue( kkt );

	getKKT.addStatement( kkt == 1e-15 );

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonBlockForces::setupEvaluation( )
{
	////////////////////////////////////////////////////////////////////////////
	//
	// Preparation phase
	//
	////////////////////////////////////////////////////////////////////////////

	preparation.setup( "preparationStep" );
	preparation.doc( "Preparation step of the RTI scheme." );
	ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
	retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
	preparation.setReturnValue(retSim, false);
	ExportIndex index("index");
	preparation.addIndex( index );

	preparation	<< retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";

	preparation.addFunctionCall( evaluateObjective );
	if( regularizeHessian.isDefined() ) preparation.addFunctionCall( regularizeHessian );
	preparation.addFunctionCall( evaluateConstraints );

	preparation.addLinebreak();
	preparation << (Dy -= y) << (DyN -= yN);
	preparation.addLinebreak();

	ExportForLoop condensePrepLoop( index, 0, getNumberOfBlocks() );
	condensePrepLoop.addFunctionCall( condensePrep, index );
	preparation.addStatement( condensePrepLoop );

	preparation.addFunctionCall( evaluateAffineConstraints );

	preparation.addStatement( objHessians[getNumberOfBlocks()] == QN1 );
	DMatrix mReg = eye<double>( getNX() );
	mReg *= levenbergMarquardt;
	preparation.addStatement( objHessians[getNumberOfBlocks()] += mReg );
	preparation.addLinebreak();

	preparation.addStatement( objGradients[ getNumberOfBlocks() ] == QN2 * DyN );
	int variableObjS;
	get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
	ExportVariable SlxCall =
				objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
	preparation.addStatement( objGradients[ getNumberOfBlocks() ] += SlxCall );
	preparation.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// Feedback phase
	//
	////////////////////////////////////////////////////////////////////////////

	ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
	tmp.setDoc( "Status code of the qpOASES QP solver." );

	feedback.setup("feedbackStep");
	feedback.doc( "Feedback/estimation step of the RTI scheme." );
	feedback.setReturnValue( tmp );
	feedback.addIndex( index );

	if (initialStateFixed() == true)
	{
		feedback.addStatement( cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose() );
	}
	feedback.addLinebreak();

	//
	// Configure output variables
	//
	std::vector< ExportVariable > vecQPVars;

	vecQPVars.clear();
	vecQPVars.resize(getNumberOfBlocks() + 1);
	for (unsigned i = 0; i < getNumberOfBlocks(); ++i)
		vecQPVars[ i ].setup(string("out") + toString(i + 1), getNumBlockVariables(), 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
	vecQPVars[ getNumberOfBlocks() ].setup(string("out") + toString(getNumberOfBlocks() + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);

	//
	// In case warm starting is enabled, give an initial guess, based on the old solution
	//
	int hotstartQP;
	get(HOTSTART_QP, hotstartQP);

	if ( hotstartQP )
	{
		return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
	}

	//
	// Call a QP solver
	// NOTE: we need two prefixes:
	// 1. module prefix
	// 2. structure instance prefix
	//
	ExportFunction solveQP;
	solveQP.setup("solve");
	solveQP.setName( "solve" );

	feedback
	<< tmp.getFullName() << " = "
	<< qpModuleName << "_" << solveQP.getName() << "( "
	<< "&" << qpObjPrefix << "_" << "params" << ", "
	<< "&" << qpObjPrefix << "_" << "output" << ", "
	<< "&" << qpObjPrefix << "_" << "info" << " , NULL);\n";
	feedback.addLinebreak();

	for (unsigned i = 0; i < getNumberOfBlocks(); ++i) {
		feedback.addFunctionCall( expand, vecQPVars[i], ExportIndex(i) );
	}

	feedback.addStatement( x.getRow( N ) += vecQPVars[ getNumberOfBlocks() ].getTranspose() );
	feedback.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup evaluation of the KKT tolerance
	//
	////////////////////////////////////////////////////////////////////////////

	ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);

	getKKT.setup( "getKKT" );
	getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
	//	kkt.setDoc( "The KKT tolerance value." );
	kkt.setDoc( "1e-15." );
	getKKT.setReturnValue( kkt );

	getKKT.addStatement( kkt == 1e-15 );

	return SUCCESSFUL_RETURN;
}