void simu_controller_loop(MBSdataStruct *MBSdata)
{
    // controller variables
    ControllerStruct *cvs;

    #ifndef YARP
   		cvs = MBSdata->user_IO->cvs;

   		// controller
   		controller_inputs(MBSdata);
   		controller_loop(cvs);
   		controller_outputs(MBSdata);

   		// simulation outputs
   		simu_outputs(MBSdata);

   	#else //if def YARP

   		// TODO : ifdef YARP, then change argument of simu_controller_loop to remove MBSdata

   		getControllerInput_Yarp();
   		//controller_loop(cvs);  // uncomment when getControllerInput_Yarp will be working
   		writeControllerOutput_Yarp();
   	#endif
}
void simu_controller_loop(MBSdataStruct *MBSdata)
{
    double tsim;

    ControllerStruct *cvs;
    UserIOStruct *uvs;

    uvs = MBSdata->user_IO;
	cvs = uvs->cvs;

    tsim = MBSdata->tsim;
    
    // controller
    if (tsim >= uvs->last_t_ctrl + PERIOD_CTRL - TIME_EPSILON)
    {
        uvs->last_t_ctrl = tsim;
        
        controller_inputs(MBSdata);
        controller_loop(cvs);
        controller_outputs(MBSdata);
    }

    // simulation outputs
    simu_outputs(MBSdata);
    
    // objective function
    fitness(MBSdata);

	// stopping the simulation if needed
	stop_simu(MBSdata);
}
// Returns 0 if no problem
int user_initialization(MBSdataStruct *MBSdata, LocalDataStruct *lds)
#endif
{

	// inputs of the controller
	controller_inputs(MBSdata);



	//  controller initialization
    controller_init(MBSdata->user_IO->cvs);

    //init_SEAStruct(1);

    #ifdef CMEX
    return 0;
    #endif
}