#include "Controller.h"
#include "Controller_private.h"

D_Work_Controller Controller_DWork;

ExternalInputs_Controller Controller_U;

ExternalOutputs_Controller Controller_Y;

void Controller_step(void)
{

  real_T rtb_U;
  real_T rtb_IntegralGain;
  real_T rtb_statefeedback;
  real_T rtb_Sum3[4];

  {
    rtb_Sum3[0] =
      (Controller_P.Observer_C[0])*Controller_DWork.Observer_DSTATE[0];

    rtb_Sum3[1] =
      (Controller_P.Observer_C[1])*Controller_DWork.Observer_DSTATE[1];

    rtb_Sum3[2] =
      (Controller_P.Observer_C[2])*Controller_DWork.Observer_DSTATE[2];

    rtb_Sum3[3] =
      (Controller_P.Observer_C[3])*Controller_DWork.Observer_DSTATE[3];
  }

  rtb_Sum3[0] = Controller_U.trajectory[1] - rtb_Sum3[0];
  rtb_Sum3[1] = Controller_U.trajectory[2] - rtb_Sum3[1];
  rtb_Sum3[2] = Controller_U.trajectory[1] - rtb_Sum3[2];
  rtb_Sum3[3] = Controller_U.trajectory[2] - rtb_Sum3[3];

  {
    static const int dims[3] = { 1, 4, 1 };
    rt_MatMultRR_Dbl((real_T *)&rtb_statefeedback, (real_T
      *)&Controller_P.L2_Gain[0],
     (real_T *)rtb_Sum3, &dims[0]);
  }

  rtb_U = ((Controller_U.trajectory[0] * Controller_P.FeedForwardGain_Gain) +
    rtb_statefeedback) + Controller_DWork.DiscreteTimeIntegrator_DSTATE;

  Controller_Y.U = rtb_U;

  rtb_IntegralGain = (Controller_U.trajectory[2] - Controller_U.Encoder) *
    Controller_P.IntegralGain_Gain;

  {
    static real_T xnew[4];
    xnew[0] = (Controller_P.Observer_B[0])*rtb_U +
      (Controller_P.Observer_B[1])*Controller_U.Encoder;
    xnew[0] += (Controller_P.Observer_A[0])*Controller_DWork.Observer_DSTATE[0]
      + (Controller_P.Observer_A[1])*Controller_DWork.Observer_DSTATE[1]
      + (Controller_P.Observer_A[2])*Controller_DWork.Observer_DSTATE[2]
      + (Controller_P.Observer_A[3])*Controller_DWork.Observer_DSTATE[3];

    xnew[1] = (Controller_P.Observer_B[2])*rtb_U +
      (Controller_P.Observer_B[3])*Controller_U.Encoder;
    xnew[1] += (Controller_P.Observer_A[4])*Controller_DWork.Observer_DSTATE[0]
      + (Controller_P.Observer_A[5])*Controller_DWork.Observer_DSTATE[1]
      + (Controller_P.Observer_A[6])*Controller_DWork.Observer_DSTATE[2]
      + (Controller_P.Observer_A[7])*Controller_DWork.Observer_DSTATE[3];

    xnew[2] = (Controller_P.Observer_B[4])*rtb_U +
      (Controller_P.Observer_B[5])*Controller_U.Encoder;
    xnew[2] += (Controller_P.Observer_A[8])*Controller_DWork.Observer_DSTATE[0]
      + (Controller_P.Observer_A[9])*Controller_DWork.Observer_DSTATE[1]
      + (Controller_P.Observer_A[10])*Controller_DWork.Observer_DSTATE[2]
      + (Controller_P.Observer_A[11])*Controller_DWork.Observer_DSTATE[3];

    xnew[3] = (Controller_P.Observer_B[6])*rtb_U +
      (Controller_P.Observer_B[7])*Controller_U.Encoder;
    xnew[3] += (Controller_P.Observer_A[12])*Controller_DWork.Observer_DSTATE[0]
      + (Controller_P.Observer_A[13])*Controller_DWork.Observer_DSTATE[1]
      + (Controller_P.Observer_A[14])*Controller_DWork.Observer_DSTATE[2]
      + (Controller_P.Observer_A[15])*Controller_DWork.Observer_DSTATE[3];
    (void) memcpy(&Controller_DWork.Observer_DSTATE[0],xnew,
     sizeof(real_T)*4);
  }

  Controller_DWork.DiscreteTimeIntegrator_DSTATE =
    Controller_P.DiscreteTimeIntegrator_g * rtb_IntegralGain +
    Controller_DWork.DiscreteTimeIntegrator_DSTATE;
}

void Controller_initialize(boolean_T firstTime)
{
  if (firstTime) {

    {
      int_T i;
      real_T *dwork_ptr = (real_T *) &Controller_DWork.Observer_DSTATE[0];

      for (i = 0; i < 5; i++) {
        dwork_ptr[i] = 0.0;
      }
    }

    {
      int_T i;
      for (i = 0; i < 3; i++) {
        Controller_U.trajectory[i] = 0.0;
      }
    }

    Controller_U.Encoder = 0.0;

    Controller_Y.U = 0.0;
  }

  Controller_DWork.Observer_DSTATE[0] = Controller_P.Observer_X0;
  Controller_DWork.Observer_DSTATE[1] = Controller_P.Observer_X0;
  Controller_DWork.Observer_DSTATE[2] = Controller_P.Observer_X0;
  Controller_DWork.Observer_DSTATE[3] = Controller_P.Observer_X0;

  Controller_DWork.DiscreteTimeIntegrator_DSTATE =
    Controller_P.DiscreteTimeIntegrator_I;
}