/*
 * vehicle_behavior_model.c
 *
 * Trial License - for use to evaluate programs for possible purchase as
 * an end-user only.
 *
 * Code generation for model "vehicle_behavior_model".
 *
 * Model version              : 1.0
 * Simulink Coder version : 24.1 (R2024a) 19-Nov-2023
 * C source code generated on : Mon Jun  3 16:39:46 2024
 *
 * Target selection: grt.tlc
 * Note: GRT includes extra infrastructure and instrumentation for prototyping
 * Embedded hardware selection: Intel->x86-64 (Windows64)
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "vehicle_behavior_model.h"
#include "vehicle_behavior_model_private.h"

/* Block signals (default storage) */
B_vehicle_behavior_model_T vehicle_behavior_model_B;

/* Continuous states */
X_vehicle_behavior_model_T vehicle_behavior_model_X;

/* Disabled State Vector */
XDis_vehicle_behavior_model_T vehicle_behavior_model_XDis;

/* Real-time model */
static RT_MODEL_vehicle_behavior_mod_T vehicle_behavior_model_M_;
RT_MODEL_vehicle_behavior_mod_T *const vehicle_behavior_model_M =
  &vehicle_behavior_model_M_;

/*
 * This function updates continuous states using the ODE3 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  /* Solver Matrices */
  static const real_T rt_ODE3_A[3] = {
    1.0/2.0, 3.0/4.0, 1.0
  };

  static const real_T rt_ODE3_B[3][3] = {
    { 1.0/2.0, 0.0, 0.0 },

    { 0.0, 3.0/4.0, 0.0 },

    { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
  };

  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si);
  real_T *y = id->y;
  real_T *f0 = id->f[0];
  real_T *f1 = id->f[1];
  real_T *f2 = id->f[2];
  real_T hB[3];
  int_T i;
  int_T nXc = 1;
  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

  /* Save the state values at time t in y, we'll use x as ynew. */
  (void) memcpy(y, x,
                (uint_T)nXc*sizeof(real_T));

  /* Assumes that rtsiSetT and ModelOutputs are up-to-date */
  /* f0 = f(t,y) */
  rtsiSetdX(si, f0);
  vehicle_behavior_model_derivatives();

  /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
  hB[0] = h * rt_ODE3_B[0][0];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  vehicle_behavior_model_step();
  vehicle_behavior_model_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++) {
    hB[i] = h * rt_ODE3_B[1][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[1]);
  rtsiSetdX(si, f2);
  vehicle_behavior_model_step();
  vehicle_behavior_model_derivatives();

  /* tnew = t + hA(3);
     ynew = y + f*hB(:,3); */
  for (i = 0; i <= 2; i++) {
    hB[i] = h * rt_ODE3_B[2][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
  }

  rtsiSetT(si, tnew);
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}

/* Model step function */
void vehicle_behavior_model_step(void)
{
  if (rtmIsMajorTimeStep(vehicle_behavior_model_M)) {
    /* set solver stop time */
    if (!(vehicle_behavior_model_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&vehicle_behavior_model_M->solverInfo,
                            ((vehicle_behavior_model_M->Timing.clockTickH0 + 1) *
        vehicle_behavior_model_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&vehicle_behavior_model_M->solverInfo,
                            ((vehicle_behavior_model_M->Timing.clockTick0 + 1) *
        vehicle_behavior_model_M->Timing.stepSize0 +
        vehicle_behavior_model_M->Timing.clockTickH0 *
        vehicle_behavior_model_M->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

  /* Update absolute time of base rate at minor time step */
  if (rtmIsMinorTimeStep(vehicle_behavior_model_M)) {
    vehicle_behavior_model_M->Timing.t[0] = rtsiGetT
      (&vehicle_behavior_model_M->solverInfo);
  }

  /* Integrator: '<Root>/Integrator' */
  vehicle_behavior_model_B.Integrator =
    vehicle_behavior_model_X.Integrator_CSTATE;
  if (rtmIsMajorTimeStep(vehicle_behavior_model_M)) {
    /* Switch: '<Root>/Switch' incorporates:
     *  Constant: '<Root>/Braking Force'
     */
    if (vehicle_behavior_model_P.BrakingForce_Value != 0.0) {
      /* Switch: '<Root>/Switch' incorporates:
       *  Constant: '<Root>/Engine Force'
       */
      vehicle_behavior_model_B.Switch =
        vehicle_behavior_model_P.EngineForce_Value;
    } else {
      /* Switch: '<Root>/Switch' */
      vehicle_behavior_model_B.Switch = 0.0;
    }

    /* End of Switch: '<Root>/Switch' */
  }

  /* Gain: '<Root>/Acceleration' incorporates:
   *  Gain: '<Root>/Damping Force'
   *  Sum: '<Root>/Sum'
   */
  vehicle_behavior_model_B.Acceleration = (vehicle_behavior_model_B.Switch -
    vehicle_behavior_model_P.DampingForce_Gain *
    vehicle_behavior_model_B.Integrator) *
    vehicle_behavior_model_P.Acceleration_Gain;
  if (rtmIsMajorTimeStep(vehicle_behavior_model_M)) {
    /* Matfile logging */
    rt_UpdateTXYLogVars(vehicle_behavior_model_M->rtwLogInfo,
                        (vehicle_behavior_model_M->Timing.t));
  }                                    /* end MajorTimeStep */

  if (rtmIsMajorTimeStep(vehicle_behavior_model_M)) {
    /* signal main to stop simulation */
    {                                  /* Sample time: [0.0s, 0.0s] */
      if ((rtmGetTFinal(vehicle_behavior_model_M)!=-1) &&
          !((rtmGetTFinal(vehicle_behavior_model_M)-
             (((vehicle_behavior_model_M->Timing.clockTick1+
                vehicle_behavior_model_M->Timing.clockTickH1* 4294967296.0)) *
              0.2)) > (((vehicle_behavior_model_M->Timing.clockTick1+
                         vehicle_behavior_model_M->Timing.clockTickH1*
                         4294967296.0)) * 0.2) * (DBL_EPSILON))) {
        rtmSetErrorStatus(vehicle_behavior_model_M, "Simulation finished");
      }
    }

    rt_ertODEUpdateContinuousStates(&vehicle_behavior_model_M->solverInfo);

    /* Update absolute time for base rate */
    /* The "clockTick0" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick0"
     * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick0 and the high bits
     * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++vehicle_behavior_model_M->Timing.clockTick0)) {
      ++vehicle_behavior_model_M->Timing.clockTickH0;
    }

    vehicle_behavior_model_M->Timing.t[0] = rtsiGetSolverStopTime
      (&vehicle_behavior_model_M->solverInfo);

    {
      /* Update absolute timer for sample time: [0.2s, 0.0s] */
      /* The "clockTick1" counts the number of times the code of this task has
       * been executed. The resolution of this integer timer is 0.2, which is the step size
       * of the task. Size of "clockTick1" ensures timer will not overflow during the
       * application lifespan selected.
       * Timer of this task consists of two 32 bit unsigned integers.
       * The two integers represent the low bits Timing.clockTick1 and the high bits
       * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
       */
      vehicle_behavior_model_M->Timing.clockTick1++;
      if (!vehicle_behavior_model_M->Timing.clockTick1) {
        vehicle_behavior_model_M->Timing.clockTickH1++;
      }
    }
  }                                    /* end MajorTimeStep */
}

/* Derivatives for root system: '<Root>' */
void vehicle_behavior_model_derivatives(void)
{
  XDot_vehicle_behavior_model_T *_rtXdot;
  _rtXdot = ((XDot_vehicle_behavior_model_T *) vehicle_behavior_model_M->derivs);

  /* Derivatives for Integrator: '<Root>/Integrator' */
  _rtXdot->Integrator_CSTATE = vehicle_behavior_model_B.Acceleration;
}

/* Model initialize function */
void vehicle_behavior_model_initialize(void)
{
  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)vehicle_behavior_model_M, 0,
                sizeof(RT_MODEL_vehicle_behavior_mod_T));

  {
    /* Setup solver object */
    rtsiSetSimTimeStepPtr(&vehicle_behavior_model_M->solverInfo,
                          &vehicle_behavior_model_M->Timing.simTimeStep);
    rtsiSetTPtr(&vehicle_behavior_model_M->solverInfo, &rtmGetTPtr
                (vehicle_behavior_model_M));
    rtsiSetStepSizePtr(&vehicle_behavior_model_M->solverInfo,
                       &vehicle_behavior_model_M->Timing.stepSize0);
    rtsiSetdXPtr(&vehicle_behavior_model_M->solverInfo,
                 &vehicle_behavior_model_M->derivs);
    rtsiSetContStatesPtr(&vehicle_behavior_model_M->solverInfo, (real_T **)
                         &vehicle_behavior_model_M->contStates);
    rtsiSetNumContStatesPtr(&vehicle_behavior_model_M->solverInfo,
      &vehicle_behavior_model_M->Sizes.numContStates);
    rtsiSetNumPeriodicContStatesPtr(&vehicle_behavior_model_M->solverInfo,
      &vehicle_behavior_model_M->Sizes.numPeriodicContStates);
    rtsiSetPeriodicContStateIndicesPtr(&vehicle_behavior_model_M->solverInfo,
      &vehicle_behavior_model_M->periodicContStateIndices);
    rtsiSetPeriodicContStateRangesPtr(&vehicle_behavior_model_M->solverInfo,
      &vehicle_behavior_model_M->periodicContStateRanges);
    rtsiSetContStateDisabledPtr(&vehicle_behavior_model_M->solverInfo,
      (boolean_T**) &vehicle_behavior_model_M->contStateDisabled);
    rtsiSetErrorStatusPtr(&vehicle_behavior_model_M->solverInfo,
                          (&rtmGetErrorStatus(vehicle_behavior_model_M)));
    rtsiSetRTModelPtr(&vehicle_behavior_model_M->solverInfo,
                      vehicle_behavior_model_M);
  }

  rtsiSetSimTimeStep(&vehicle_behavior_model_M->solverInfo, MAJOR_TIME_STEP);
  rtsiSetIsMinorTimeStepWithModeChange(&vehicle_behavior_model_M->solverInfo,
    false);
  rtsiSetIsContModeFrozen(&vehicle_behavior_model_M->solverInfo, false);
  vehicle_behavior_model_M->intgData.y = vehicle_behavior_model_M->odeY;
  vehicle_behavior_model_M->intgData.f[0] = vehicle_behavior_model_M->odeF[0];
  vehicle_behavior_model_M->intgData.f[1] = vehicle_behavior_model_M->odeF[1];
  vehicle_behavior_model_M->intgData.f[2] = vehicle_behavior_model_M->odeF[2];
  vehicle_behavior_model_M->contStates = ((X_vehicle_behavior_model_T *)
    &vehicle_behavior_model_X);
  vehicle_behavior_model_M->contStateDisabled = ((XDis_vehicle_behavior_model_T *)
    &vehicle_behavior_model_XDis);
  vehicle_behavior_model_M->Timing.tStart = (0.0);
  rtsiSetSolverData(&vehicle_behavior_model_M->solverInfo, (void *)
                    &vehicle_behavior_model_M->intgData);
  rtsiSetSolverName(&vehicle_behavior_model_M->solverInfo,"ode3");
  rtmSetTPtr(vehicle_behavior_model_M, &vehicle_behavior_model_M->Timing.tArray
             [0]);
  rtmSetTFinal(vehicle_behavior_model_M, 10.0);
  vehicle_behavior_model_M->Timing.stepSize0 = 0.2;

  /* Setup for data logging */
  {
    static RTWLogInfo rt_DataLoggingInfo;
    rt_DataLoggingInfo.loggingInterval = (NULL);
    vehicle_behavior_model_M->rtwLogInfo = &rt_DataLoggingInfo;
  }

  /* Setup for data logging */
  {
    rtliSetLogXSignalInfo(vehicle_behavior_model_M->rtwLogInfo, (NULL));
    rtliSetLogXSignalPtrs(vehicle_behavior_model_M->rtwLogInfo, (NULL));
    rtliSetLogT(vehicle_behavior_model_M->rtwLogInfo, "tout");
    rtliSetLogX(vehicle_behavior_model_M->rtwLogInfo, "");
    rtliSetLogXFinal(vehicle_behavior_model_M->rtwLogInfo, "");
    rtliSetLogVarNameModifier(vehicle_behavior_model_M->rtwLogInfo, "rt_");
    rtliSetLogFormat(vehicle_behavior_model_M->rtwLogInfo, 4);
    rtliSetLogMaxRows(vehicle_behavior_model_M->rtwLogInfo, 0);
    rtliSetLogDecimation(vehicle_behavior_model_M->rtwLogInfo, 1);
    rtliSetLogY(vehicle_behavior_model_M->rtwLogInfo, "");
    rtliSetLogYSignalInfo(vehicle_behavior_model_M->rtwLogInfo, (NULL));
    rtliSetLogYSignalPtrs(vehicle_behavior_model_M->rtwLogInfo, (NULL));
  }

  /* block I/O */
  (void) memset(((void *) &vehicle_behavior_model_B), 0,
                sizeof(B_vehicle_behavior_model_T));

  /* states (continuous) */
  {
    (void) memset((void *)&vehicle_behavior_model_X, 0,
                  sizeof(X_vehicle_behavior_model_T));
  }

  /* disabled states */
  {
    (void) memset((void *)&vehicle_behavior_model_XDis, 0,
                  sizeof(XDis_vehicle_behavior_model_T));
  }

  /* Matfile logging */
  rt_StartDataLoggingWithStartTime(vehicle_behavior_model_M->rtwLogInfo, 0.0,
    rtmGetTFinal(vehicle_behavior_model_M),
    vehicle_behavior_model_M->Timing.stepSize0, (&rtmGetErrorStatus
    (vehicle_behavior_model_M)));

  /* InitializeConditions for Integrator: '<Root>/Integrator' */
  vehicle_behavior_model_X.Integrator_CSTATE =
    vehicle_behavior_model_P.Integrator_IC;
}

/* Model terminate function */
void vehicle_behavior_model_terminate(void)
{
  /* (no terminate code required) */
}