int rien_feedbackStep(  )
{
int tmp;
acado_timer tmr;

acado_tic( &tmr );
acado_condenseFdb(  );
condenseTime += acado_toc( &tmr );

acado_tic( &tmr );
tmp = acado_solve( );
qpTime = acado_toc( &tmr );

acado_tic( &tmr );
acado_expand(  );
condenseTime += acado_toc( &tmr );
return tmp;
}
int rien_preparationStep(  )
{
int ret;
acado_timer tmr;

acado_tic( &tmr );
ret = acado_modelSimulation();
simTime = acado_toc( &tmr );

acado_evaluateObjective(  );

acado_tic( &tmr );
acado_regularizeHessian(  );
regularizeTime = acado_toc( &tmr );

acado_tic( &tmr );
acado_condensePrep(  );
condenseTime = acado_toc( &tmr );
return ret;
}
base::Vector6d NMPC::calcControl(const base::Vector6d &reference,
                                 const base::VectorXd &pose_samples,
                                 const base::Vector6d &efforts)
{
    /* Get the time before start of the loop */
    acado_timer t;
    acado_tic( &t );

    calcTrajectory(reference);
    updateStates(pose_samples, efforts);

    /* Prepare first step */
    acado_preparationStep();

    /* The "real-time iterations" loop */
    for(int iter = 0; iter < num_steps_; ++iter)
    {
        acado_feedbackStep( );

        // Optional: shift the initialization (look at acado_common.h)
        // shiftStates(2, 0, 0);
        // shiftControls( 0 );

        /* Prepare for the next step */
        acado_preparationStep();
    }

    base::Vector6d control = base::Vector6d::Zero();
    for (int i = 0; i < kNumControls; i++)
        control[i] = acadoVariables.u[i];

    control_derivative_ = control;

    /* Read the elapsed time */
    elapsed_time_ = acado_toc( &t );

    return control;
}
/** The MEX interface function. */
void mexFunction(	int nlhs,
					mxArray *plhs[],
					int nrhs,
					const mxArray *prhs[]
					)
{
	static unsigned initialized = 0;
	unsigned ctrl;
	int ctrlIndex, i, j;
	unsigned strategy;
	unsigned initType;
	real_t* xEnd = NULL;
	real_t* uEnd = NULL;
	const mxArray* src = prhs[ 0 ];
	
	const char *infoNames[ 9 ] = {"status", "cpuTime", "simTime", "qpTime", "condenseTime", "regularizeTime", "kktValue", "objValue", "nIterations"};
	mxArray* info;
	double status, cpuTime, kktValue, objValue;
	double tmp[ 1 ];
	mxArray* shPtr;
	acado_timer tmr;
	double nIterations = 0;
	
	const char *outNames[ NOO_4 ];
	outNames[ 0 ] = "info";
	outNames[ 1 ] = "x";
	outNames[ 2 ] = "u";
	outNames[ 3 ] = "mu";
#if ACADO_NXA
	outNames[ NOO ] = "z";
#endif	
		
#if ACADO_USE_ARRIVAL_COST == 1
	outNames[ NOO_2 ] = "xAC";
	outNames[NOO_2  + 1] = "SAC";
#endif
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
	outNames[ NOO_3 ] = "sigmaN";
#endif
	
	if (nrhs != 1)
		mexErrMsgTxt(
			"This function requires exactly one input: a structure with parameters.");
			
	if (nlhs != 1)
		mexErrMsgTxt(
			"This function returns one output.");
			
	if( !mxIsStruct( src ) )
		mexErrMsgTxt("The function argument must be a structure.");
	
	/* Get the control flag. */
	if (getArray(0, src, 0, "control", tmp, 1, 1) == 0)
		ctrl = (unsigned)tmp[ 0 ];
	else
		ctrl = 0;
		
	/* Get the initialization flag. */
	if (getArray(0, src, 0, "initialization", tmp, 1, 1) == 0)
		initType = (unsigned)tmp[ 0 ];
	else
		initType = 0;
		
	/* Copy MATLAB arrays to C arrays. */
	getArray(1, src, 0, "x",  acadoVariables.x, ACADO_N + 1, ACADO_NX);
	getArray(1, src, 0, "u",  acadoVariables.u, ACADO_N, ACADO_NU);
	getArray(1, src, 0, "mu", acadoVariables.mu, ACADO_N, ACADO_NX);
	
#if ACADO_NXA	
	getArray(1, src, 0, "z",  acadoVariables.z, ACADO_N, ACADO_NXA);
#endif	
	
#if ACADO_NOD
	getArray(1, src, 0, "od", acadoVariables.od, ACADO_N + 1, ACADO_NOD);
#endif

#if ACADO_INITIAL_STATE_FIXED
	getArray(1, src, 0, "x0", acadoVariables.x0, ACADO_NX, 1);
#endif /* ACADO_INITIAL_STATE_FIXED */

#if (ACADO_HARDCODED_CONSTRAINT_VALUES == 0) && ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )

	if (!initialized)
	{
		acado_initializeSolver();
	}
	
	/* Bounds */
#if ACADO_INITIAL_STATE_FIXED == 1
	getArray(1, src, 0, "lbValues", acadoVariables.lbValues, ACADO_N * ACADO_NU, 1);
	getArray(1, src, 0, "ubValues", acadoVariables.ubValues, ACADO_N * ACADO_NU, 1);
#else
	getArray(1, src, 0, "lbValues", acadoVariables.lbValues, ACADO_NX + ACADO_N * ACADO_NU, 1);
	getArray(1, src, 0, "ubValues", acadoVariables.ubValues, ACADO_NX + ACADO_N * ACADO_NU, 1);
#endif /* ACADO_INITIAL_STATE_FIXED == 0 */

#if QPOASES_NCMAX > 0
	/* Affine constraints */
	getArray(1, src, 0, "lbAValues", acadoVariables.lbAValues, QPOASES_NCMAX, 1);
	getArray(1, src, 0, "ubAValues", acadoVariables.ubAValues, QPOASES_NCMAX, 1);
#endif /* QPOASES_NCMAX > 0 */

#endif /* (ACADO_HARDCODED_CONSTRAINT_VALUES == 0) && ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */

#if (ACADO_QP_SOLVER == ACADO_QPDUNES)
	if (!initialized) {
		acado_initializeSolver();
	}
#endif

#if ACADO_USE_ARRIVAL_COST == 1
	getArray(1, src, 0, "xAC", acadoVariables.xAC, ACADO_NX, 1);
	getArray(1, src, 0, "SAC", acadoVariables.SAC, ACADO_NX, ACADO_NX);
    getArray(1, src, 0, "WL", acadoVariables.WL, ACADO_NX, ACADO_NX);
#endif

	/* Shifting strategy */
	shPtr = mxGetField(src, 0, "shifting");
	if (shPtr != NULL)
	{
		if( !mxIsStruct( shPtr ) )
			mexErrMsgTxt("Field \"shifting\" must be defined with a structure.");
		
		/* Get the shifting strategy flag */
		getArray(1, shPtr, 0, "strategy", tmp, 1, 1);
		strategy = (unsigned)tmp[ 0 ];
		
		if (strategy > 2)
			mexErrMsgTxt("Valid options for the shifting strategy are 1 or 2.");
	
		getArray(0, shPtr, 0, "xEnd", xEnd, ACADO_NX, 1);
		getArray(0, shPtr, 0, "uEnd", uEnd, ACADO_NU, 1);
	}
	else
		strategy = 0;
		
	acado_tic( &tmr );
	
	/* Call solver */
	switch ( ctrl )
	{
		case 0:
			/* Simple operational mode. Run one RTI with optional shifting. */
			
			if ( !initialized )
			{
				memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
			
#if ACADO_HARDCODED_CONSTRAINT_VALUES == 1
				acado_initializeSolver();
#endif /* ACADO_HARDCODED_CONSTRAINT_VALUES == 1 */
                
                /*for( i = 0; i < ACADO_N*ACADO_RK_NIS; i++ ) {
                    for( j = 0; j < ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA); j++ ) {
                        acadoWorkspace.rk_A_traj[i*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)+j*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)+j] = 1.0;
                    }
                }*/
				
				if (initType == 1)
				{
					acado_initializeNodesByForwardSimulation();
				}
				
#if ACADO_USE_ARRIVAL_COST == 1 
                	acado_updateArrivalCost( 1 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
				
				initialized = 1;
			}
			else if (strategy == 1 || strategy == 2)
			{
#if ACADO_USE_ARRIVAL_COST == 1 
                acado_updateArrivalCost( 0 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
				
				acado_shiftStates(strategy, xEnd, uEnd);
				acado_shiftControls(uEnd);
			}
			
			rien_preparationStep();
			
			status = (double)rien_feedbackStep();
			
			kktValue = acado_getKKT();
			objValue = acado_getObjective();

#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
			nIterations = (double)acado_getNWSR();
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
			
			break;
		
		case 1:
			/* Initialize */
			
			memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
			
			acado_initializeSolver();
				
			if (initType == 1)
			{
				acado_initializeNodesByForwardSimulation();
			}
			
#if ACADO_USE_ARRIVAL_COST == 1 
                acado_updateArrivalCost( 1 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */			
			
			break;
		
		case 2:
			/* Preparation step */
			
			rien_preparationStep();
			
			break;
		
		case 3:
			/* Feedback step */
			
			status = (double)rien_feedbackStep();
			
			kktValue = acado_getKKT();
			objValue = acado_getObjective();
			
#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
			nIterations = (double)acado_getNWSR();
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
			
			break;
		
		case 4:
			/* Shifting */
			
#if ACADO_USE_ARRIVAL_COST == 1 
                acado_updateArrivalCost( 0 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */			
			
			acado_shiftStates(strategy, xEnd, uEnd);
			acado_shiftControls( uEnd );
			
			break;
			
		default:
			/* Return an error */
			mexErrMsgTxt("Unknown control code.");
	}
	
	cpuTime = acado_toc( &tmr );
	
	/* Prepare return argument */
	
	plhs[ 0 ] = mxCreateStructMatrix(1, 1, NOO_4, outNames);
		
	setArray(plhs[ 0 ], 0, "x", acadoVariables.x, ACADO_N + 1, ACADO_NX);
	setArray(plhs[ 0 ], 0, "u", acadoVariables.u, ACADO_N, ACADO_NU);
	setArray(plhs[ 0 ], 0, "mu", acadoVariables.mu, ACADO_N, ACADO_NX);
#if ACADO_NXA > 0
	setArray(plhs[ 0 ], 0, "z", acadoVariables.z, ACADO_N, ACADO_NXA);
#endif	
		
#if ACADO_USE_ARRIVAL_COST == 1
	setArray(plhs[ 0 ], 0, "xAC", acadoVariables.xAC, ACADO_NX, 1);
	setArray(plhs[ 0 ], 0, "SAC", acadoVariables.SAC, ACADO_NX, ACADO_NX);
#endif /* ACADO_USE_ARRIVAL_COST */

#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
	setArray(plhs[ 0 ], 0, "sigmaN", acadoVariables.sigmaN, ACADO_NX, ACADO_NX);
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */

	/* Create the info structure. */
	info = mxCreateStructMatrix(1, 1, 9, infoNames);
		
	setArray(info, 0, "status", &status, 1, 1);
	setArray(info, 0, "cpuTime", &cpuTime, 1, 1);
	setArray(info, 0, "simTime", &simTime, 1, 1);
	setArray(info, 0, "qpTime", &qpTime, 1, 1);
	setArray(info, 0, "condenseTime", &condenseTime, 1, 1);
	setArray(info, 0, "regularizeTime", &regularizeTime, 1, 1);
	setArray(info, 0, "kktValue", &kktValue, 1, 1);
	setArray(info, 0, "objValue", &objValue, 1, 1);
	
#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
	setArray(info, 0, "nIterations", &nIterations, 1, 1);
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
		
	mxSetField(plhs[ 0 ], 0, "info", info);
	
	/* Cleanup of the allocated memory */
	FREE( xEnd );
	FREE( uEnd );
}