/*! \fn updateContinuousSystem
 *
 *  Function to update the whole system with EventIteration.
 *  Evaluate the functionDAE()
 *
 *  \param [ref] [data]
 */
static void prefixedName_updateContinuousSystem(DATA *data, threadData_t *threadData)
{
  TRACE_PUSH

  externalInputUpdate(data);
  data->callback->input_function(data, threadData);
  data->callback->functionODE(data, threadData);
  data->callback->functionAlgebraics(data, threadData);
  data->callback->output_function(data, threadData);
  data->callback->function_storeDelayed(data, threadData);
  storePreValues(data);

  TRACE_POP
}
static modelica_boolean LIQSS2_calculateState(DATA* data, threadData_t *threadData){
	externalInputUpdate(data);
	data->callback->input_function(data, threadData);
	data->callback->functionODE(data, threadData);
	data->callback->functionAlgebraics(data, threadData);
	data->callback->output_function(data, threadData);
	data->callback->function_storeDelayed(data, threadData);
	data->callback->functionDAE(data, threadData);

	if(data->simulationInfo->needToIterate){
		//printf("needToIterate is een\n");
		//printf("nTI:  %d\n", data->simulationInfo->needToIterate);
		return true;
	}
	else
		return false;
}
/*! \fn updateContinuousSystem
 *
 *  Function to update the whole system with EventIteration.
 *  Evaluate the functionDAE()
 *
 *  \param [ref] [data]
 */
static void prefixedName_updateContinuousSystem(DATA *data, threadData_t *threadData)
{
  TRACE_PUSH

  externalInputUpdate(data);
  data->callback->input_function(data, threadData);

  if (compiledInDAEMode) /* dae mode */
  {
    data->simulationInfo->daeModeData->evaluateDAEResiduals(data, threadData, EVAL_ALGEBRAIC);
  }
  else /* ode mode */
  {
    data->callback->functionODE(data, threadData);
    data->callback->functionAlgebraics(data, threadData);
  }
  data->callback->output_function(data, threadData);
  data->callback->setc_function(data, threadData);
  data->callback->function_storeDelayed(data, threadData);
  storePreValues(data);

  TRACE_POP
}
/*! performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo)
 *
 *  \param [ref] [data]
 *  \param [ref] [solverInfo]
 *
 *  This function performs the simulation controlled by solverInfo.
 */
modelica_integer prefixedName_performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo)
{
  TRACE_PUSH

  SIMULATION_INFO *simInfo = &(data->simulationInfo);
  MODEL_DATA *mData = &(data->modelData);
  uinteger currStepNo = 0;
  modelica_integer retValIntegrator = 0;
  modelica_integer retValue = 0;
  uinteger ind = 0;

  solverInfo->currentTime = simInfo->startTime;

  warningStreamPrint(LOG_STDOUT, 0, "This QSS method is under development and should not be used yet.");

  if (data->callback->initialAnalyticJacobianA(data))
  {
    infoStreamPrint(LOG_STDOUT, 0, "Jacobian or sparse pattern is not generated or failed to initialize.");
    return UNKNOWN;
  }
  printSparseStructure(data, LOG_SOLVER);

/* *********************************************************************************** */
  /* Initialization */
  uinteger i = 0; /* loop var */
  SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
  modelica_real* state = sData->realVars;
  modelica_real* stateDer = sData->realVars + data->modelData.nStates;
  const SPARSE_PATTERN* pattern = &(data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sparsePattern);
  const uinteger ROWS = data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sizeRows;
  const uinteger STATES = data->modelData.nStates;
  uinteger numDer = 0;  /* number of derivatives influenced by state k */

  modelica_boolean fail = 0;
  modelica_real* qik = NULL;  /* Approximation of states */
  modelica_real* xik = NULL;  /* states */
  modelica_real* derXik = NULL;  /* Derivative of states */
  modelica_real* tq = NULL;    /* Time of approximation, because not all approximations are calculated at a specific time, each approx. has its own timestamp */
  modelica_real* tx = NULL;    /* Time of the states, because not all states are calculated at a specific time, each state has its own timestamp */
  modelica_real* tqp = NULL;    /* Time of the next change in state */
  modelica_real* nQh = NULL;    /* next value of the state */
  modelica_real* dQ = NULL;    /* change in quantity of every state, default = nominal*10^-4 */

  /* allocate memory*/
  qik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (qik == NULL) ? 1 : ( 0 | fail);
  xik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (xik == NULL) ? 1 : ( 0 | fail);
  derXik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (derXik == NULL) ? 1 : ( 0 | fail);
  tq = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (tq == NULL) ? 1 : ( 0 | fail);
  tx = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (tx == NULL) ? 1 : ( 0 | fail);
  tqp = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (tqp == NULL) ? 1 : ( 0 | fail);
  nQh = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (nQh == NULL) ? 1 : ( 0 | fail);
  dQ = (modelica_real*)calloc(STATES, sizeof(modelica_real));
  fail = (dQ == NULL) ? 1 : ( 0 | fail);


  if (fail)
    return OO_MEMORY;
  /* end - allocate memory */

  /* further initialization of local variables */

  modelica_real diffQ = 0.0, dTnextQ = 0.0, nextQ = 0.0;
  for (i = 0; i < STATES; i++)
  {
    dQ[i] = 0.0001 * data->modelData.realVarsData[i].attribute.nominal;
    tx[i] = tq[i] = simInfo->startTime;
    qik[i] = state[i];
    xik[i] = state[i];
    derXik[i] = stateDer[i];
    retValue = deltaQ(data, dQ[i], i, &dTnextQ, &nextQ, &diffQ);
    if (OK != retValue)
      return retValue;
    tqp[i] = tq[i] + dTnextQ;
    nQh[i] = nextQ;
  }

/* Transform the sparsity pattern into a data structure for an index based access. */
  modelica_integer* der = (modelica_integer*)calloc(ROWS, sizeof(modelica_integer));
  if (NULL==der)
    return OO_MEMORY;
  for (i = 0; i < ROWS; i++)
    der[i] = -1;

  /* how many states are involved in each derivative */
  /* **** This is needed if we have QSS2 or higher **** */
 /* uinteger* numStatesInDer = calloc(ROWS,sizeof(uinteger));
  if (NULL==numStatesInDer) return OO_MEMORY; */
  /*
   *   dx1/dt = x2
   *   dx2/dt = x1 + x2
   *   lead to
   *  StatesInDer[0]-{ 2 }
   *  StatesInDer[1]-{ 1, 2 }
  */
/*  uinteger** StatesInDer = calloc(ROWS,sizeof(uinteger*));
  if (NULL==StatesInDer) return OO_MEMORY; */

  /* count number of states in each derivative*/
/*  for (i = 0; i < pattern->leadindex[ROWS-1]; i++) numStatesInDer[ pattern->index[i] ]++; */
  /* collect memory for all stateindices */
/*  for (i = 0; i < ROWS; i++)
  {
    *(StatesInDer + i) = calloc(numStatesInDer[i], sizeof(uinteger));
    if (NULL==*(StatesInDer + i)) return OO_MEMORY;
  }

  retValue = getStatesInDer(pattern->index, pattern->leadindex, ROWS, STATES, StatesInDer);
  if (OK != retValue) return retValue; */

/*  retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, 0);
  if (OK != retValue) return retValue; */
/* End of transformation */

#ifdef D
  FILE* fid=NULL;
  fid = fopen("log_qss.txt","w");
#endif

/* *********************************************************************************** */

/***** Start main simulation loop *****/
  while(solverInfo->currentTime < simInfo->stopTime)
  {
    modelica_integer success = 0;
    threadData->currentErrorStage = ERROR_SIMULATION;
    omc_alloc_interface.collect_a_little();

#if !defined(OMC_EMCC)
    /* try */
    MMC_TRY_INTERNAL(simulationJumpBuffer)
    {
#endif

#ifdef USE_DEBUG_TRACE
    if (useStream[LOG_TRACE])
      printf("TRACE: push loop step=%u, time=%.12g\n", currStepNo, solverInfo->currentTime);
#endif

#ifdef D
    fprintf(fid,"t = %.8f\n",solverInfo->currentTime);
    fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp");
    for (i = 0; i < STATES; i++)
    {
      fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]);
    }
#endif

    currStepNo++;

    ind = minStep(tqp, STATES);

    if (isnan(tqp[ind]))
    {
#ifdef D
      fprintf(fid,"Exit caused by #QNAN!\tind=%d",ind);
#endif
      return ISNAN;
    }
    if (isinf(tqp[ind]))
    {
      /* If all derivatives are zero, the states stay constant and only the
       * time propagates till stop->time.
       */
      warningStreamPrint(LOG_STDOUT, 0, "All derivatives are zero at time %f!.\n", sData->timeValue);
      solverInfo->currentTime = simInfo->stopTime;
      sData->timeValue = solverInfo->currentTime;

      continue;
    }

    qik[ind] = nQh[ind];

    xik[ind] = qik[ind];
    state[ind] = qik[ind];

    tx[ind] = tqp[ind];
    tq[ind] = tqp[ind];

    solverInfo->currentTime = tqp[ind];

#ifdef D
    fprintf(fid,"Index: %d\n\n",ind);
#endif

    /* the state[ind] will change again in dTnextQ*/
    retValue = deltaQ(data, dQ[ind], ind, &dTnextQ, &nextQ, &diffQ);
    if (OK != retValue)
      return retValue;
    tqp[ind] = tq[ind] + dTnextQ;
    nQh[ind] = nextQ;

    if (0 != strcmp("ia", MMC_STRINGDATA(data->simulationInfo.outputFormat)))
    {
      communicateStatus("Running", (solverInfo->currentTime-simInfo->startTime)/(simInfo->stopTime-simInfo->startTime));
    }

    /* get the derivatives depending on state[ind] */
    for (i = 0; i < ROWS; i++)
      der[i] = -1;
    retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, ind);

    uinteger k = 0, j = 0;
    for (k = 0; k < numDer; k++)
    {
      j = der[k];
      if (j != ind)
      {
        xik[j] = xik[j] + derXik[j] * (solverInfo->currentTime - tx[j]);
        state[j] = xik[j];
        tx[j] = solverInfo->currentTime;
      }
    }

    /*
     * Recalculate all equations which are affected by state[ind].
     * Unfortunately all equations will be calculated up to now. And we need to evaluate
     * the equations as f(t,q) and not f(t,x). So all states were saved onto a local stack
     * and overwritten by q. After evaluating the equations the states are written back.
     */
    for (i = 0; i < STATES; i++)
    {
      xik[i] = state[i];   /* save current state */
      state[i] = qik[i];  /* overwrite current state for dx/dt = f(t,q) */
    }

    /* update continous system */
    sData->timeValue = solverInfo->currentTime;
    externalInputUpdate(data);
    data->callback->input_function(data);
    data->callback->functionODE(data);
    data->callback->functionAlgebraics(data);
    data->callback->output_function(data);
    data->callback->function_storeDelayed(data);

    for (i = 0; i < STATES; i++)
    {
      state[i] = xik[i];  /* restore current state */
    }


    /*
     * Get derivatives affected by state[ind] and write back ALL derivatives. After that we have
     * states and derivatives for different times tx.
    */

    for (k = 0; k < numDer; k++)
    {
      j = der[k];
      derXik[j] = stateDer[j];
    }
    derXik[ind] = stateDer[ind];  /* not in every case part of the above derivatives */

    for (i = 0; i < STATES; i++)
    {
      stateDer[i] = derXik[i];  /* write back all derivatives */
    }

    /* recalculate the time of next change only for the affected states */
    for (k = 0; k < numDer; k++)
    {
       j = der[k];
      retValue = deltaQ(data, dQ[j], j, &dTnextQ, &nextQ, &diffQ);
      if (OK != retValue)
        return retValue;
      tqp[j] = solverInfo->currentTime + dTnextQ;
      nQh[j] = nextQ;
    }

    /*sData->timeValue = solverInfo->currentTime;*/
    solverInfo->laststep = solverInfo->currentTime;

    sim_result.emit(&sim_result, data);

    /* check if terminate()=true */
    if (terminationTerminate)
    {
      printInfo(stdout, TermInfo);
      fputc('\n', stdout);
      infoStreamPrint(LOG_STDOUT, 0, "Simulation call terminate() at time %f\nMessage : %s", data->localData[0]->timeValue, TermMsg);
      simInfo->stopTime = solverInfo->currentTime;
    }

    /* terminate for some cases:
     * - integrator fails
     * - non-linear system failed to solve
     * - assert was called
     */
    if (retValIntegrator)
    {
      retValue = -1 + retValIntegrator;
      infoStreamPrint(LOG_STDOUT, 0, "model terminate | Integrator failed. | Simulation terminated at time %g", solverInfo->currentTime);
      break;
    }
    else if (check_nonlinear_solutions(data, 0))
    {
      retValue = -2;
      infoStreamPrint(LOG_STDOUT, 0, "model terminate | non-linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
      break;
    }
    else if (check_linear_solutions(data, 0))
    {
      retValue = -3;
      infoStreamPrint(LOG_STDOUT, 0, "model terminate | linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
      break;
    }
    else if (check_mixed_solutions(data, 0))
    {
      retValue = -4;
      infoStreamPrint(LOG_STDOUT, 0, "model terminate | mixed system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
      break;
    }
    success = 1;
#if !defined(OMC_EMCC)
    }
    /* catch */
    MMC_CATCH_INTERNAL(simulationJumpBuffer)
#endif
    if (!success)
    {
      retValue =  -1;
      infoStreamPrint(LOG_STDOUT, 0, "model terminate | Simulation terminated by an assert at time: %g", data->localData[0]->timeValue);
      break;
    }

    TRACE_POP /* pop loop */
  }
  /* End of main loop */

#ifdef D
  fprintf(fid,"t = %.8f\n",solverInfo->currentTime);
  fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp");
  for (i = 0; i < STATES; i++)
  {
    fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]);
  }
  fclose(fid);
#endif

  /* free memory*/
   free(der);
 /*  for (i = 0; i < ROWS; i++) free(*(StatesInDer + i));
   free(StatesInDer);
   free(numStatesInDer); */
   free(qik);
   free(xik);
   free(derXik);
   free(tq);
   free(tx);
   free(tqp);
   free(nQh);
   free(dQ);
   /* end - free memory */

  TRACE_POP
  return retValue;
}
Beispiel #5
0
/*!
 *  create initial guess dasslColorSymJac
 *  author: Vitalij Ruge
 **/
static short initial_guess_ipopt_sim(OptData *optData, SOLVER_INFO* solverInfo, const short o)
{
  double *u0;
  int i,j,k,l;
  modelica_real ***v;
  long double tol;
  short printGuess, op=1;

  const int nx = optData->dim.nx;
  const int nu = optData->dim.nu;
  const int np = optData->dim.np;
  const int nsi = optData->dim.nsi;
  const int nReal = optData->dim.nReal;
  char *cflags = (char*)omc_flagValue[FLAG_IIF];

  DATA* data = optData->data;
  threadData_t *threadData = optData->threadData;
  SIMULATION_INFO *sInfo = data->simulationInfo;

  if(!data->simulationInfo->external_input.active){
     externalInputallocate(data);
  }

   /* Initial DASSL solver */
   DASSL_DATA* dasslData = (DASSL_DATA*) malloc(sizeof(DASSL_DATA));
   tol = data->simulationInfo->tolerance;
   data->simulationInfo->tolerance = fmin(fmax(tol,1e-8),1e-3);

   infoStreamPrint(LOG_SOLVER, 0, "Initial Guess: Initializing DASSL");
   sInfo->solverMethod = "dassl";
   solverInfo->solverMethod = S_DASSL;
   dassl_initial(data, threadData, solverInfo, dasslData);
   solverInfo->solverMethod = S_OPTIMIZATION;
   solverInfo->solverData = dasslData;

   u0 = optData->bounds.u0;
   v = optData->v;

   if(!data->simulationInfo->external_input.active)
     for(i = 0; i< nu;++i)
       data->simulationInfo->inputVars[i] = u0[i]/*optData->bounds.scalF[i + nx]*/;

   printGuess = (short)(ACTIVE_STREAM(LOG_INIT) && !ACTIVE_STREAM(LOG_SOLVER));

   if((double)data->simulationInfo->startTime < optData->time.t0){
     double t = data->simulationInfo->startTime;
     const int nBoolean = data->modelData->nVariablesBoolean;
     const int nInteger = data->modelData->nVariablesInteger;
     const int nRelations =  data->modelData->nRelations;

     FILE * pFile = optData->pFile;
     fprintf(pFile, "%lf ",(double)t);
     for(i = 0; i < nu; ++i){
       fprintf(pFile, "%lf ", (float)data->simulationInfo->inputVars[i]);
     }
     fprintf(pFile, "%s", "\n");
     if(1){
       printf("\nPreSim");
       printf("\n========================================================\n");
       printf("\ndone: time[%i] = %g",0,(double)data->simulationInfo->startTime);
     }
     while(t < optData->time.t0){
       externalInputUpdate(data);
       smallIntSolverStep(data, threadData, solverInfo, fmin(t += optData->time.dt[0], optData->time.t0));
       printf("\ndone: time[%i] = %g",0,(double)data->localData[0]->timeValue);
       sim_result.emit(&sim_result,data,threadData);
       fprintf(pFile, "%lf ",(double)data->localData[0]->timeValue);
       for(i = 0; i < nu; ++i){
         fprintf(pFile, "%lf ", (float)data->simulationInfo->inputVars[i]);
       }
       fprintf(pFile, "%s", "\n");
     }
     memcpy(optData->v0, data->localData[0]->realVars, nReal*sizeof(modelica_real));
     memcpy(optData->i0, data->localData[0]->integerVars, nInteger*sizeof(modelica_integer));
     memcpy(optData->b0, data->localData[0]->booleanVars, nBoolean*sizeof(modelica_boolean));
     memcpy(optData->i0Pre, data->simulationInfo->integerVarsPre, nInteger*sizeof(modelica_integer));
     memcpy(optData->b0Pre, data->simulationInfo->booleanVarsPre, nBoolean*sizeof(modelica_boolean));
     memcpy(optData->v0Pre, data->simulationInfo->realVarsPre, nReal*sizeof(modelica_real));
     memcpy(optData->rePre, data->simulationInfo->relationsPre, nRelations*sizeof(modelica_boolean));
     memcpy(optData->re, data->simulationInfo->relations, nRelations*sizeof(modelica_boolean));
     memcpy(optData->storeR, data->simulationInfo->storedRelations, nRelations*sizeof(modelica_boolean));

     if(1){
       printf("\n--------------------------------------------------------");
       printf("\nfinished: PreSim");
       printf("\n========================================================\n");
     }
   }

   if(o == 2 && cflags && strcmp(cflags, ""))
     op = 2;

   if(printGuess ){
     printf("\nInitial Guess");
     printf("\n========================================================\n");
     printf("\ndone: time[%i] = %g",0,(double)optData->time.t0);
   }

   for(i = 0, k=1; i < nsi; ++i){
     for(j = 0; j < np; ++j, ++k){
       externalInputUpdate(data);
       if(op==1)
         smallIntSolverStep(data, threadData, solverInfo, (double)optData->time.t[i][j]);
       else{
         rotateRingBuffer(data->simulationData, 1, (void**) data->localData);
         importStartValues(data, threadData, cflags, (double)optData->time.t[i][j]);
         for(l=0; l<nReal; ++l){
            data->localData[0]->realVars[l] = data->modelData->realVarsData[l].attribute.start;
         }
       }

       if(printGuess)
         printf("\ndone: time[%i] = %g", k, (double)optData->time.t[i][j]);

       memcpy(v[i][j], data->localData[0]->realVars, nReal*sizeof(double));
       for(l = 0; l < nx; ++l){

         if(((double) v[i][j][l] < (double)optData->bounds.vmin[l]*optData->bounds.vnom[l])
             || (double) (v[i][j][l] > (double) optData->bounds.vmax[l]*optData->bounds.vnom[l])){
           printf("\n********************************************\n");
           warningStreamPrint(LOG_STDOUT, 0, "Initial guess failure at time %g",(double)optData->time.t[i][j]);
           warningStreamPrint(LOG_STDOUT, 0, "%g<= (%s=%g) <=%g",
               (double)optData->bounds.vmin[l]*optData->bounds.vnom[l],
               data->modelData->realVarsData[l].info.name,
               (double)v[i][j][l],
               (double)optData->bounds.vmax[l]*optData->bounds.vnom[l]);
           printf("\n********************************************");
         }
       }
     }
  }

  if(printGuess){
    printf("\n--------------------------------------------------------");
    printf("\nfinished: Initial Guess");
    printf("\n========================================================\n");
  }

  dassl_deinitial(solverInfo->solverData);
  solverInfo->solverData = (void*)optData;
  sInfo->solverMethod = "optimization";
  data->simulationInfo->tolerance = tol;

  externalInputFree(data);
  return op;
}
Beispiel #6
0
/*! \fn sym_euler_im_with_step_size_control_step
 *
 *   Function does one implicit euler step
 *   and calculates step size for the next step
 *   using the implicit midpoint rule
 *
 */
int sym_euler_im_with_step_size_control_step(DATA* data, threadData_t *threadData, SOLVER_INFO* solverInfo)
{
  int retVal = 0;
  SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
  SIMULATION_DATA *sDataOld = (SIMULATION_DATA*)data->localData[1];
  DATA_SYM_IMP_EULER* userdata = (DATA_SYM_IMP_EULER*)solverInfo->solverData;

  double sc, err, a, b, diff;
  double Atol = data->simulationInfo->tolerance, Rtol = data->simulationInfo->tolerance;
  int i,j;
  double fac = 0.9;
  double facmax = 3.5;
  double facmin = 0.3;
  double saveTime = sDataOld->timeValue;
  double targetTime = sDataOld->timeValue + solverInfo->currentStepSize;


  if (userdata->firstStep  || solverInfo->didEventStep == 1)
  {
    first_step(data, solverInfo);
    userdata->radauStepSizeOld = 0;
  }

  infoStreamPrint(LOG_SOLVER,0, "new step: time=%e", userdata->radauTime);
  while (userdata->radauTime < targetTime)
  {
    do
    {
      /*** do one step with original step size ***/

      infoStreamPrint(LOG_SOLVER,0, "radauStepSize = %e and time = %e", userdata->radauStepSize, userdata->radauTime);

      /* update time */
      sDataOld->timeValue = userdata->radauTime;
      solverInfo->currentTime = userdata->radauTime + userdata->radauStepSize;
      sData->timeValue = solverInfo->currentTime;

      /* update step size */
      data->callback->symEulerUpdate(data, userdata->radauStepSize);

      memcpy(sDataOld->realVars, userdata->radauVars, data->modelData->nStates*sizeof(double));

      infoStreamPrint(LOG_SOLVER,0, "first system time = %e", sData->timeValue);

      /* evaluate function ODE */
      externalInputUpdate(data);
      data->callback->input_function(data, threadData);
      data->callback->functionODE(data,threadData);

      /* save values in y05 */
      memcpy(userdata->y05, sData->realVars, data->modelData->nStates*sizeof(double));

      /* extrapolate values in y1 */
      for (i=0; i<data->modelData->nStates; i++)
      {
        userdata->y1[i] = 2.0 * userdata->y05[i] - userdata->radauVars[i];
      }

      /*** do another step with original step size ***/
      memcpy(sDataOld->realVars, userdata->y05, data->modelData->nStates*sizeof(double));

      /* update time */
      sDataOld->timeValue = userdata->radauTime + userdata->radauStepSize;
      solverInfo->currentTime = userdata->radauTime + 2.0*userdata->radauStepSize;
      sData->timeValue = solverInfo->currentTime;

      infoStreamPrint(LOG_SOLVER,0, "second system time = %e", sData->timeValue);

      /* update step size */
      data->callback->symEulerUpdate(data, userdata->radauStepSize);

      /* evaluate function ODE */
      externalInputUpdate(data);
      data->callback->input_function(data, threadData);
      data->callback->functionODE(data, threadData);

      /* save values in y2 */
      memcpy(userdata->y2, sData->realVars, data->modelData->nStates*sizeof(double));

      /*** calculate error ***/
      for (i=0, err=0.0; i<data->modelData->nStates; i++)
      {
        sc = Atol + fmax(fabs(userdata->y2[i]),fabs(userdata->y1[i]))*Rtol;
        diff = userdata->y2[i]-userdata->y1[i];
        err += (diff*diff)/(sc*sc);
      }

      err /= data->modelData->nStates;
      err = sqrt(err);

      userdata->stepsDone += 1;
      infoStreamPrint(LOG_SOLVER, 0, "err = %e", err);
      infoStreamPrint(LOG_SOLVER, 0, "min(facmax, max(facmin, fac*sqrt(1/err))) = %e",  fmin(facmax, fmax(facmin, fac*sqrt(1.0/err))));


      /* update step size */
      userdata->radauStepSizeOld = 2.0 * userdata->radauStepSize;
      userdata->radauStepSize *=  fmin(facmax, fmax(facmin, fac*sqrt(1.0/err)));

      if (isnan(userdata->radauStepSize))
      {
        userdata->radauStepSize = 1e-6;
      }

    } while  (err > 1.0 );

    userdata->radauTimeOld =  userdata->radauTime;

    userdata->radauTime += userdata->radauStepSizeOld;

    memcpy(userdata->radauVarsOld, userdata->radauVars, data->modelData->nStates*sizeof(double));
    memcpy(userdata->radauVars, userdata->y2, data->modelData->nStates*sizeof(double));
  }

  sDataOld->timeValue = saveTime;
  solverInfo->currentTime = sDataOld->timeValue + solverInfo->currentStepSize;
  sData->timeValue = solverInfo->currentTime;


  /* linear interpolation */
  for (i=0; i<data->modelData->nStates; i++)
  {
    sData->realVars[i] = (userdata->radauVars[i] * (sData->timeValue - userdata->radauTimeOld) + userdata->radauVarsOld[i] * (userdata->radauTime - sData->timeValue))/(userdata->radauTime - userdata->radauTimeOld);

  }

  /* update first derivative  */
  infoStreamPrint(LOG_SOLVER,0, "Time  %e", sData->timeValue);
  for(i=0, j=data->modelData->nStates; i<data->modelData->nStates; ++i, ++j)
  {
    a = 4.0 * (userdata->y2[i] - 2.0 * userdata->y05[i] + userdata->radauVarsOld[i]) / (userdata->radauStepSizeOld * userdata->radauStepSizeOld);
    b = 2.0 * (userdata->y2[i] - userdata->y05[i])/userdata->radauStepSizeOld - userdata->radauTime * a;
    data->localData[0]->realVars[j] = a * sData->timeValue + b;
  }

  /* update step size */
  data->callback->symEulerUpdate(data, 0);
  solverInfo->solverStepSize = userdata->radauStepSizeOld;
  infoStreamPrint(LOG_SOLVER,0, "Step done to %f with step size = %e", sData->timeValue, solverInfo->solverStepSize);

  return retVal;
}