//
//  Academic License - for use in teaching, academic research, and meeting
//  course requirements at degree granting institutions only.  Not for
//  government, commercial, or other organizational use.
//
//  getGravityForce.cpp
//
//  Code generation for function 'getGravityForce'
//


// Include files
#include "getGravityForce.h"
#include "FKinSpaceExpand.h"
#include "InverseDynamics_debug.h"
#include "calculateGravityForce.h"
#include "calculateGravityForce_data.h"
#include "getSimpackF.h"
#include "rt_nonfinite.h"

// Variable Definitions
static emlrtRSInfo c_emlrtRSI = { 23,  // lineNo
  "getGravityForce",                   // fcnName
  "C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
};

static emlrtRSInfo d_emlrtRSI = { 24,  // lineNo
  "getGravityForce",                   // fcnName
  "C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
};

static emlrtRSInfo f_emlrtRSI = { 20,  // lineNo
  "getGravityForce",                   // fcnName
  "C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
};

static emlrtRSInfo g_emlrtRSI = { 25,  // lineNo
  "getGravityForce",                   // fcnName
  "C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
};

// Function Definitions
void getGravityForce(const emlrtStack *sp, const real_T thetalist[9], const
                     real_T robot_m[9], const real_T robot_I[81], const real_T
                     robot_slist[54], const real_T robot_Home_R[81], const
                     struct_T *robot_kine, const real_T robot_gravity[3], real_T
                     GravityForce[6])
{
  int32_T b_i;
  real_T dv[6];
  real_T Glist_data[324];
  real_T unusedU0[60];
  real_T unusedU1[60];
  real_T unusedU2[360];
  real_T Fmat[54];
  real_T b_robot_Home_R[9];
  real_T d;
  int32_T Glist_data_tmp;
  real_T G[144];
  real_T T[144];
  real_T F_Simpack[54];
  real_T link_inertia_data[81];
  static const int8_T a[36] = { 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1, 0,
    0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1 };

  emlrtStack st;
  st.prev = sp;
  st.tls = sp->tls;

  //  Get info from robot
  //  Get general mass matrix
  for (int32_T i = 0; i < 9; i++) {
    int32_T b_Glist_data_tmp;
    int32_T c_Glist_data_tmp;
    for (b_i = 0; b_i < 3; b_i++) {
      real_T d1;
      Glist_data_tmp = 3 * b_i + 9 * i;
      for (b_Glist_data_tmp = 0; b_Glist_data_tmp < 3; b_Glist_data_tmp++) {
        c_Glist_data_tmp = 3 * b_Glist_data_tmp + 9 * i;
        b_robot_Home_R[b_i + 3 * b_Glist_data_tmp] =
          (robot_Home_R[Glist_data_tmp] * robot_I[c_Glist_data_tmp] +
           robot_Home_R[Glist_data_tmp + 1] * robot_I[c_Glist_data_tmp + 1]) +
          robot_Home_R[Glist_data_tmp + 2] * robot_I[c_Glist_data_tmp + 2];
      }

      d = b_robot_Home_R[b_i + 3];
      d1 = b_robot_Home_R[b_i + 6];
      for (Glist_data_tmp = 0; Glist_data_tmp < 3; Glist_data_tmp++) {
        b_Glist_data_tmp = 3 * Glist_data_tmp + 9 * i;
        link_inertia_data[(b_i + 3 * Glist_data_tmp) + 9 * i] =
          (b_robot_Home_R[b_i] * robot_Home_R[b_Glist_data_tmp] + d *
           robot_Home_R[b_Glist_data_tmp + 1]) + d1 *
          robot_Home_R[b_Glist_data_tmp + 2];
      }
    }

    d = robot_m[i];
    for (b_i = 0; b_i < 3; b_i++) {
      c_Glist_data_tmp = 6 * b_i + 36 * i;
      Glist_data_tmp = 3 * b_i + 9 * i;
      Glist_data[c_Glist_data_tmp] = link_inertia_data[Glist_data_tmp];
      b_Glist_data_tmp = 6 * (b_i + 3) + 36 * i;
      Glist_data[b_Glist_data_tmp] = 0.0;
      Glist_data[c_Glist_data_tmp + 3] = 0.0;
      Glist_data[b_Glist_data_tmp + 3] = d * static_cast<real_T>(iv[3 * b_i]);
      Glist_data[c_Glist_data_tmp + 1] = link_inertia_data[Glist_data_tmp + 1];
      Glist_data[b_Glist_data_tmp + 1] = 0.0;
      Glist_data[c_Glist_data_tmp + 4] = 0.0;
      Glist_data[b_Glist_data_tmp + 4] = d * static_cast<real_T>(iv[3 * b_i + 1]);
      Glist_data[c_Glist_data_tmp + 2] = link_inertia_data[Glist_data_tmp + 2];
      Glist_data[b_Glist_data_tmp + 2] = 0.0;
      Glist_data[c_Glist_data_tmp + 5] = 0.0;
      Glist_data[b_Glist_data_tmp + 5] = d * static_cast<real_T>(iv[3 * b_i + 2]);
    }

    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  for (b_i = 0; b_i < 6; b_i++) {
    dv[b_i] = 0.0;
  }

  st.site = &f_emlrtRSI;
  InverseDynamics_debug(&st, thetalist, robot_gravity, dv, robot_kine->Mlist_CG,
                        Glist_data, robot_slist, unusedU0, unusedU1, unusedU2,
                        Fmat, b_robot_Home_R);
  st.site = &c_emlrtRSI;
  FKinSpaceExpand(&st, robot_kine->Mlist_CG, robot_slist, thetalist, G);
  st.site = &d_emlrtRSI;
  FKinSpaceExpand(&st, robot_kine->Mlist_ED, robot_slist, thetalist, T);
  st.site = &g_emlrtRSI;
  getSimpackF(&st, G, T, Fmat, F_Simpack);

  //  get J7
  for (b_i = 0; b_i < 6; b_i++) {
    d = 0.0;
    for (Glist_data_tmp = 0; Glist_data_tmp < 6; Glist_data_tmp++) {
      d += static_cast<real_T>(a[b_i + 6 * Glist_data_tmp]) *
        F_Simpack[Glist_data_tmp + 36];
    }

    GravityForce[b_i] = d;
  }
}

// End of code generation (getGravityForce.cpp)