/*
 * IPa_control.c
 *
 * Real-Time Workshop code generation for Simulink model "IPa_control.mdl".
 *
 * Model Version                        : 1.38
 * Real-Time Workshop file version      : 4.0 $Date: 2000/09/19 19:45:27 $
 * Real-Time Workshop file generated on : Fri Aug 03 15:50:52 2001
 * TLC version                          : 4.0 (Aug 21 2000)
 * C source code generated on           : Fri Aug 03 15:50:53 2001
 *
 * Relevant TLC Options:
 *   InlineParameters      = 0
 *   RollThreshold         = 5
 *   CodeFormat            = RealTime
 *
 * Simulink model settings:
 *   Solver     : FixedStep
 *   StartTime  : 0.0 s
 *   StopTime   : 6.0 s
 *   FixedStep  : 0.01 s
 */

#include <math.h>
#include <string.h>
#include "IPa_control.h"
#include "IPa_control_prm.h"

/* Start of Functions in model "IPa_control" */

void MdlInitialize(void)
{
  /* Integrator Block: <S2>/Integrator1 */
  if (ssIsFirstInitCond(rtS)) {
    rtX.s2_Integrator1 = 0.0;
  }
  rtDWork.s2_Integrator1_IWORK.IcNeedsLoading = 1;

  /* Integrator Block: <S2>/Integrator */
  if (ssIsFirstInitCond(rtS)) {
    rtX.s2_Integrator = 0.0;
  }
  rtDWork.s2_Integrator_IWORK.IcNeedsLoading = 1;

  /* Derivative Block: <Root>/Derivative */
  rtDWork.root_Derivative_RWORK.TimeStampA = rtInf;
  rtDWork.root_Derivative_RWORK.TimeStampB = rtInf;

  /* TransferFcn Block: <S2>/Motor Transfer Fcn */
  rtX.s2_Motor_Transfer_Fcn = 0.0;
}

/* Start the model */
void MdlStart(void)
{

  /* RandomNumber Block: <Root>/Random Number */
  {
    uint_T *RandSeed = (uint_T *) &rtDWork.root_Random_Number_IWORK.RandSeed;
    int_T r, t;
    *RandSeed = 0U;
    r = *RandSeed >> 16;
    t = *RandSeed & BIT16;
    *RandSeed = ((*RandSeed - (r << 16) - t) << 16) + t + r;
    if (*RandSeed < 1) {
      *RandSeed = SEED0;
    }
    if (*RandSeed > MAXSEED) {
      *RandSeed = MAXSEED;
    }

    rtDWork.root_Random_Number_RWORK.NextOutput =
      rt_NormalRand(RandSeed++) * (rtP.root_Random_Number_StdDev) + (rtP.root_Random_Number_Mean);
  }

  /* ToWorkspace Block: <Root>/To Workspace */
  {
    int_T dimensions[1] = {1};

    rtDWork.root_To_Workspace_PWORK.LoggedData = rt_CreateLogVar(rtS, "y", SS_DOUBLE,
     0, 0, 0, 1, 1, dimensions, 0, 1, 0.01, 1);
    if (rtDWork.root_To_Workspace_PWORK.LoggedData == NULL) return;
  }

  /* ToWorkspace Block: <Root>/To Workspace1 */
  {
    int_T dimensions[1] = {1};

    rtDWork.root_To_Workspace1_PWORK.LoggedData = rt_CreateLogVar(rtS, "dy", SS_DOUBLE,
     0, 0, 0, 1, 1, dimensions, 0, 1, 0.01, 1);
    if (rtDWork.root_To_Workspace1_PWORK.LoggedData == NULL) return;
  }

  /* ToWorkspace Block: <Root>/To Workspace2 */
  {
    int_T dimensions[1] = {1};

    rtDWork.root_To_Workspace2_PWORK.LoggedData = rt_CreateLogVar(rtS, "t", SS_DOUBLE,
     0, 0, 0, 1, 1, dimensions, 0, 1, 0.01, 1);
    if (rtDWork.root_To_Workspace2_PWORK.LoggedData == NULL) return;
  }

  MdlInitialize();
}

/* Compute block outputs */
void MdlOutputs(int_T tid)
{
  /* local block i/o variables */
  real_T rtb_root_Random_Number;
  real_T rtb_s17_impMethod[101];
  real_T rtb_s24_impMethod[101];
  real_T rtb_s25_impMethod[101];
  real_T rtb_s26_impMethod[101];
  real_T rtb_s27_impMethod[101];
  real_T rtb_s28_impMethod[101];
  real_T rtb_s29_impMethod[101];
  real_T rtb_s30_impMethod[101];
  real_T rtb_s7_impMethod[101];
  real_T rtb_s8_impMethod[101];
  real_T rtb_s9_impMethod[101];
  real_T rtb_s10_impMethod[101];
  real_T rtb_s11_impMethod[101];
  real_T rtb_s12_impMethod[101];
  real_T rtb_s13_impMethod[101];
  real_T rtb_s14_impMethod[101];
  real_T rtb_s15_impMethod[101];
  real_T rtb_s16_impMethod[101];
  real_T rtb_s18_impMethod[101];
  real_T rtb_s19_impMethod[101];
  real_T rtb_s20_impMethod[101];
  real_T rtb_s21_impMethod[101];
  real_T rtb_s22_impMethod[101];
  real_T rtb_temp130;
  real_T rtb_temp131[101];
  real_T rtb_temp132;
  real_T rtb_temp133;
  real_T rtb_temp134;
  real_T rtb_temp135;
  real_T rtb_temp136;
  real_T rtb_temp137;
  real_T rtb_temp138;
  real_T rtb_temp139;
  real_T rtb_temp140;
  real_T rtb_temp141[101];
  real_T rtb_temp142;
  real_T rtb_temp143;
  real_T rtb_temp144;
  real_T rtb_temp145;
  real_T rtb_temp146;
  real_T rtb_temp147;
  real_T rtb_temp148;
  real_T rtb_temp149;
  real_T rtb_temp150;
  real_T rtb_temp151;
  real_T rtb_temp152;
  real_T rtb_temp153;
  real_T rtb_temp154;
  real_T rtb_temp155;
  real_T rtb_temp156;
  real_T rtb_temp157;
  real_T rtb_temp158;
  real_T rtb_temp159;
  real_T rtb_temp160;

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Step Block: <Root>/Step */
    if (ssGetTaskTime(rtS,tid) < (rtP.root_Step_Time)) {
      rtb_temp157 = (rtP.root_Step_Y0);
    } else {
      rtb_temp157 = (rtP.root_Step_YFinal);
    }
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Gain Block: <Root>/Gain2 */
    rtB.root_Gain2 = (rtP.root_Constant_Value) * (rtP.root_Gain2_Gain);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Integrator Block: <S2>/Integrator1 */
    if (rtDWork.s2_Integrator1_IWORK.IcNeedsLoading) {
      rtX.s2_Integrator1 = rtB.root_Gain2;
      rtDWork.s2_Integrator1_IWORK.IcNeedsLoading = 0;
    }
    rtb_temp130 = rtX.s2_Integrator1;

    /* Saturate Block: <S2>/Saturation */
    if (rtb_temp130 >= (rtP.s2_Saturation_UpperSat)) {
      rtb_temp130 = (rtP.s2_Saturation_UpperSat);
    } else if (rtb_temp130 <= (rtP.s2_Saturation_LowerSat)) {
      rtb_temp130 = (rtP.s2_Saturation_LowerSat);
      /* Buffer Reuse Active; Following statement(s) unnecessary */
      /* } else { */
      /*   rtb_temp130 = rtb_temp130; */
    }
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* RandomNumber Block: <Root>/Random Number */
    rtb_root_Random_Number = rtDWork.root_Random_Number_RWORK.NextOutput;

    /* Gain Block: <Root>/Gain4 */
    rtB.root_Gain4 = rtb_root_Random_Number * (rtP.root_Gain4_Gain);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <Root>/Sum1 */
    rtb_temp160 = rtb_temp130 - rtB.root_Gain4;

    /* Sum Block: <Root>/Sum */
    rtB.root_Sum = rtb_temp157 - rtb_temp160;

    /* Outport Block: <Root>/Out1 */
    rtY.root_Out1 = rtB.root_Sum;

    /* Outport Block: <Root>/Out2 */
    rtY.root_Out2 = rtb_temp157;

    /* Gain Block: <Root>/Gain */
    rtB.root_Gain = rtb_temp160 * (rtP.root_Gain_Gain);

    /* Outport Block: <Root>/Out3 */
    rtY.root_Out3 = rtB.root_Gain;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Gain Block: <Root>/Gain3 */
    rtB.root_Gain3 = (rtP.root_Constant1_Value) * (rtP.root_Gain3_Gain);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Integrator Block: <S2>/Integrator */
    if (rtDWork.s2_Integrator_IWORK.IcNeedsLoading) {
      rtX.s2_Integrator = rtB.root_Gain3;
      rtDWork.s2_Integrator_IWORK.IcNeedsLoading = 0;
    }
    rtB.s2_Integrator = rtX.s2_Integrator;

    /* Gain Block: <Root>/Gain1 */
    rtB.root_Gain1 = rtB.s2_Integrator * (rtP.root_Gain1_Gain);

    /* Outport Block: <Root>/Out4 */
    rtY.root_Out4 = rtB.root_Gain1;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* ToWorkspace Block: <Root>/To Workspace */
    rt_UpdateLogVar((LogVar*) (rtDWork.root_To_Workspace_PWORK.LoggedData), &rtB.root_Gain);

    /* ToWorkspace Block: <Root>/To Workspace1 */
    rt_UpdateLogVar((LogVar*) (rtDWork.root_To_Workspace1_PWORK.LoggedData), &rtB.root_Gain1);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Derivative Block: <Root>/Derivative */
    {
      real_T t = ssGetTaskTime(rtS,tid);
      real_T timeStampA = rtDWork.root_Derivative_RWORK.TimeStampA;
      real_T timeStampB = rtDWork.root_Derivative_RWORK.TimeStampB;

      if (timeStampA >= t && timeStampB >= t) {
        rtb_temp160 = 0.0;
      } else {
        real_T deltaT;
        real_T *lastBank = &rtDWork.root_Derivative_RWORK.TimeStampA;
        if (timeStampA < timeStampB) {
          if (timeStampB < t) {
            lastBank += 2;
          }
        } else if (timeStampA >= t) {
          lastBank += 2;
        }
        deltaT = t - *lastBank++;
        rtb_temp160 = (rtB.root_Sum - *lastBank++) / deltaT;
      }
    }

    /* Gain Block: <S1>/Gain2 */
    rtb_temp157 = rtB.root_Sum * (rtP.s1_Gain2_Gain);

    /* Saturate Block: <S1>/Saturation */
    if (rtb_temp157 >= (rtP.s1_Saturation_UpperSat)) {
      rtb_temp157 = (rtP.s1_Saturation_UpperSat);
    } else if (rtb_temp157 <= (rtP.s1_Saturation_LowerSat)) {
      rtb_temp157 = (rtP.s1_Saturation_LowerSat);
      /* Buffer Reuse Active; Following statement(s) unnecessary */
      /* } else { */
      /*   rtb_temp157 = rtb_temp157; */
    }

    /* Sum Block: <S39>/Sum */
    rtb_temp149 = rtb_temp157 - (rtP.s39_a_Value);
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S39>/Sum1 */
    rtB.s39_b_a = - (rtP.s39_a_Value) + (rtP.s39_b_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Product Block: <S39>/Product ab (trimf) */
    rtb_temp149 = rtb_temp149 /
      rtB.s39_b_a;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S39>/Sum2 */
    rtB.s39_c_b = - (rtP.s39_b_Value) + (rtP.s39_c_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <S39>/Sum3 */
    rtb_temp159 = (rtP.s39_c_Value) - rtb_temp157;

    /* Product Block: <S39>/Product cd (trimf) */
    rtb_temp159 = 1.0 / rtB.s39_c_b *
      rtb_temp159;

    /* MinMax Block: <S39>/Min (trimf) */
    rtb_temp149 = (rtb_temp149 < rtb_temp159) ? rtb_temp149 : rtb_temp159;

    /* MinMax Block: <S39>/Max (trimf) */
    rtb_temp149 = (rtb_temp149 > (rtP.s39_0_Value)) ? rtb_temp149 : (rtP.s39_0_Value);

    /* Gain Block: <S1>/Gain1 */
    rtb_temp160 *= (rtP.s1_Gain1_Gain);

    /* Saturate Block: <S1>/Saturation1 */
    if (rtb_temp160 >= (rtP.s1_Saturation1_UpperSat)) {
      rtb_temp160 = (rtP.s1_Saturation1_UpperSat);
    } else if (rtb_temp160 <= (rtP.s1_Saturation1_LowerSat)) {
      rtb_temp160 = (rtP.s1_Saturation1_LowerSat);
      /* Buffer Reuse Active; Following statement(s) unnecessary */
      /* } else { */
      /*   rtb_temp160 = rtb_temp160; */
    }

    /* Sum Block: <S34>/Sum */
    rtb_temp159 = rtb_temp160 - (rtP.s34_a_Value);
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S34>/Sum1 */
    rtB.s34_b_a = - (rtP.s34_a_Value) + (rtP.s34_b_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Product Block: <S34>/Product ab (trimf) */
    rtb_temp159 = rtb_temp159 /
      rtB.s34_b_a;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S34>/Sum2 */
    rtB.s34_c_b = - (rtP.s34_b_Value) + (rtP.s34_c_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <S34>/Sum3 */
    rtb_temp150 = (rtP.s34_c_Value) - rtb_temp160;

    /* Product Block: <S34>/Product cd (trimf) */
    rtb_temp150 = 1.0 / rtB.s34_c_b *
      rtb_temp150;

    /* MinMax Block: <S34>/Min (trimf) */
    rtb_temp159 = (rtb_temp159 < rtb_temp150) ? rtb_temp159 : rtb_temp150;

    /* MinMax Block: <S34>/Max (trimf) */
    rtb_temp159 = (rtb_temp159 > (rtP.s34_0_Value)) ? rtb_temp159 : (rtP.s34_0_Value);

    /* MinMax Block: <S6>/andorMethod */
    {
      real_T min = rtb_temp149;

      if (rtb_temp159 < min) {
        min = rtb_temp159;
      }
      rtb_temp150 = min;
    }

    /* Product Block: <S6>/Weighting */
    rtb_temp150 = (rtP.s6_Weight_Value) *
      rtb_temp150;

    /* MinMax Block: <S6>/impMethod */
    {
      real_T min;

      {
        int_T i1;
        const real_T *u1 = &rtP.s33_PL_Value[0];
        real_T *y0 = &rtb_temp131[0];

        for (i1=0; i1 < 101; i1++) {
          min = rtb_temp150;
          if (u1[i1] < min) {
            min = u1[i1];
          }
          y0[i1] = min;
        }
      }
    }

    /* Sum Block: <S35>/Sum */
    rtb_temp151 = rtb_temp160 - (rtP.s35_a_Value);
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S35>/Sum1 */
    rtB.s35_b_a = - (rtP.s35_a_Value) + (rtP.s35_b_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Product Block: <S35>/Product ab (trimf) */
    rtb_temp151 = rtb_temp151 /
      rtB.s35_b_a;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S35>/Sum2 */
    rtB.s35_c_b = - (rtP.s35_b_Value) + (rtP.s35_c_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <S35>/Sum3 */
    rtb_temp143 = (rtP.s35_c_Value) - rtb_temp160;

    /* Product Block: <S35>/Product cd (trimf) */
    rtb_temp143 = 1.0 / rtB.s35_c_b *
      rtb_temp143;

    /* MinMax Block: <S35>/Min (trimf) */
    rtb_temp151 = (rtb_temp151 < rtb_temp143) ? rtb_temp151 : rtb_temp143;

    /* MinMax Block: <S35>/Max (trimf) */
    rtb_temp151 = (rtb_temp151 > (rtP.s35_0_Value)) ? rtb_temp151 : (rtP.s35_0_Value);

    /* MinMax Block: <S17>/andorMethod */
    {
      real_T min = rtb_temp149;

      if (rtb_temp151 < min) {
        min = rtb_temp151;
      }
      rtb_temp143 = min;
    }

    /* Product Block: <S17>/Weighting */
    rtb_temp143 = (rtP.s17_Weight_Value) *
      rtb_temp143;

    /* MinMax Block: <S17>/impMethod */
    {
      real_T min;

      {
        int_T i1;
        const real_T *u1 = &rtP.s33_PL_Value[0];
        real_T *y0 = &rtb_s17_impMethod[0];

        for (i1=0; i1 < 101; i1++) {
          min = rtb_temp143;
          if (u1[i1] < min) {
            min = u1[i1];
          }
          y0[i1] = min;
        }
      }
    }

    /* Sum Block: <S38>/Sum */
    rtb_temp152 = rtb_temp160 - (rtP.s38_a_Value);
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S38>/Sum1 */
    rtB.s38_b_a = - (rtP.s38_a_Value) + (rtP.s38_b_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Product Block: <S38>/Product ab (trimf) */
    rtb_temp152 = rtb_temp152 /
      rtB.s38_b_a;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S38>/Sum2 */
    rtB.s38_c_b = - (rtP.s38_b_Value) + (rtP.s38_c_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <S38>/Sum3 */
    rtb_temp144 = (rtP.s38_c_Value) - rtb_temp160;

    /* Product Block: <S38>/Product cd (trimf) */
    rtb_temp144 = 1.0 / rtB.s38_c_b *
      rtb_temp144;

    /* MinMax Block: <S38>/Min (trimf) */
    rtb_temp152 = (rtb_temp152 < rtb_temp144) ? rtb_temp152 : rtb_temp144;

    /* MinMax Block: <S38>/Max (trimf) */
    rtb_temp152 = (rtb_temp152 > (rtP.s38_0_Value)) ? rtb_temp152 : (rtP.s38_0_Value);

    /* MinMax Block: <S24>/andorMethod */
    {
      real_T min = rtb_temp149;

      if (rtb_temp152 < min) {
        min = rtb_temp152;
      }
      rtb_temp144 = min;
    }

    /* Product Block: <S24>/Weighting */
    rtb_temp144 = (rtP.s24_Weight_Value) *
      rtb_temp144;

    /* MinMax Block: <S24>/impMethod */
    {
      real_T min;

      {
        int_T i1;
        const real_T *u1 = &rtP.s33_PL_Value[0];
        real_T *y0 = &rtb_s24_impMethod[0];

        for (i1=0; i1 < 101; i1++) {
          min = rtb_temp144;
          if (u1[i1] < min) {
            min = u1[i1];
          }
          y0[i1] = min;
        }
      }
    }

    /* Sum Block: <S37>/Sum */
    rtb_temp153 = rtb_temp160 - (rtP.s37_a_Value);
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S37>/Sum1 */
    rtB.s37_b_a = - (rtP.s37_a_Value) + (rtP.s37_b_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Product Block: <S37>/Product ab (trimf) */
    rtb_temp153 = rtb_temp153 /
      rtB.s37_b_a;
  }

  if (ssIsSampleHit(rtS, 1, tid)) {     /* Sample time: [0.01, 0.0] */
    /* Sum Block: <S37>/Sum2 */
    rtB.s37_c_b = - (rtP.s37_b_Value) + (rtP.s37_c_Value);
  }

  if (ssIsContinuousTask(rtS, tid)) {   /* Sample time: [0.0, 0.0] */
    /* Sum Block: <S37>/Sum3 */
    rtb_temp145 = (rtP.s37_c_Value) - rtb_temp160;

    /* Product Block: <S37>/Product cd (trimf) */
    rtb_temp145 = 1.0 / rtB.s37_c_b *
      rtb_temp145;

    /* MinMax Block: <S37>/Min (trimf) */
    rtb_temp153 = (rtb_temp153 < rtb_temp145) ? rtb_temp153 : rtb_temp145;

    /* MinMax Block: <S37>/Max (trimf) */
    rtb_temp153 = (rtb_temp153 > (rtP.s37_0_Value)) ? rtb_temp153 : (rtP.s37_0_Value);

    /* MinMax Block: <S25>/andorMethod */
    {
      real_T min = rtb_temp149;

      if (rtb_temp153 < min) {