//
//  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.
//
//  getSimpackF.cpp
//
//  Code generation for function 'getSimpackF'
//


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

// Function Definitions
void getSimpackF(const emlrtStack *sp, const real_T T_GC[144], const real_T T_O
                 [144], const real_T Fmat[54], real_T Flist[54])
{
  real_T invT_tmp[9];
  real_T b_invT_tmp[9];
  real_T c_invT_tmp[16];
  real_T s;
  real_T y[16];
  real_T Adgab[36];

  // base frame
  //  Adgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
  //  Flist(:,1) = Adgab'*Fmat(1,:)';
  //  for i = 2: N
  //  %     Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
  //      Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
  //      Flist(:,i) = Adgab'*Fmat(i,:)';
  //  endAdgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
  for (int32_T i = 0; i < 9; i++) {
    int32_T b_i;
    int32_T aoffset;
    int32_T k;
    real_T d;
    int32_T Adgab_tmp;

    //      Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes a transformation matrix T.
    //  Returns its inverse.
    //  Uses the structure of transformation matrices to avoid taking a matrix
    //  inverse, for efficiency.
    //  Example Input:
    //
    //  clear; clc;
    //  T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
    //  invT = TransInv(T)
    //
    //  Ouput:
    //  invT =
    //      1     0     0     0
    //      0     0     1    -3
    //      0    -1     0     0
    //      0     0     0     1
    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes the transformation matrix T in SE(3)
    //  Returns R: the corresponding rotation matrix
    //          p: the corresponding position vector .
    //  Example Input:
    //
    //  clear; clc;
    //  T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
    //  [R, p] = TransToRp(T)
    //
    //  Output:
    //  R =
    //      1     0     0
    //      0     0    -1
    //      0     1     0
    //  p =
    //      0
    //      0
    //      3
    for (b_i = 0; b_i < 3; b_i++) {
      aoffset = b_i + (i << 4);
      invT_tmp[3 * b_i] = T_GC[aoffset];
      invT_tmp[3 * b_i + 1] = T_GC[aoffset + 4];
      invT_tmp[3 * b_i + 2] = T_GC[aoffset + 8];
    }

    for (b_i = 0; b_i < 9; b_i++) {
      b_invT_tmp[b_i] = -invT_tmp[b_i];
    }

    b_i = i << 4;
    for (k = 0; k < 3; k++) {
      aoffset = k << 2;
      c_invT_tmp[aoffset] = invT_tmp[3 * k];
      c_invT_tmp[aoffset + 1] = invT_tmp[3 * k + 1];
      c_invT_tmp[aoffset + 2] = invT_tmp[3 * k + 2];
      c_invT_tmp[k + 12] = (b_invT_tmp[k] * T_GC[b_i + 12] + b_invT_tmp[k + 3] *
                            T_GC[b_i + 13]) + b_invT_tmp[k + 6] * T_GC[b_i + 14];
    }

    c_invT_tmp[3] = 0.0;
    c_invT_tmp[7] = 0.0;
    c_invT_tmp[11] = 0.0;
    c_invT_tmp[15] = 1.0;
    for (b_i = 0; b_i < 4; b_i++) {
      real_T d1;
      s = c_invT_tmp[b_i + 4];
      d = c_invT_tmp[b_i + 8];
      d1 = c_invT_tmp[b_i + 12];
      for (k = 0; k < 4; k++) {
        Adgab_tmp = k << 2;
        aoffset = Adgab_tmp + (i << 4);
        y[b_i + Adgab_tmp] = ((c_invT_tmp[b_i] * T_O[aoffset] + s * T_O[aoffset
          + 1]) + d * T_O[aoffset + 2]) + d1 * T_O[aoffset + 3];
      }
    }

    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes T a transformation matrix SE3.
    //  Returns the corresponding 6x6 adjoint representation [AdT].
    //  Example Input:
    //
    //  clear; clc;
    //  T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
    //  AdT = Adjoint(T)
    //
    //  Output:
    //  AdT =
    //      1     0     0     0     0     0
    //      0     0    -1     0     0     0
    //      0     1     0     0     0     0
    //      0     0     3     1     0     0
    //      3     0     0     0     0    -1
    //      0     0     0     0     1     0
    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes the transformation matrix T in SE(3)
    //  Returns R: the corresponding rotation matrix
    //          p: the corresponding position vector .
    //  Example Input:
    //
    //  clear; clc;
    //  T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
    //  [R, p] = TransToRp(T)
    //
    //  Output:
    //  R =
    //      1     0     0
    //      0     0    -1
    //      0     1     0
    //  p =
    //      0
    //      0
    //      3
    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes a 3-vector (angular velocity).
    //  Returns the skew symmetric matrix in so(3).
    //  Example Input:
    //
    //  clear; clc;
    //  omg = [1; 2; 3];
    //  so3mat = VecToso3(omg)
    //
    //  Output:
    //  so3mat =
    //      0    -3     2
    //      3     0    -1
    //     -2     1     0
    invT_tmp[0] = 0.0;
    invT_tmp[3] = -y[14];
    invT_tmp[6] = y[13];
    invT_tmp[1] = y[14];
    invT_tmp[4] = 0.0;
    invT_tmp[7] = -y[12];
    invT_tmp[2] = -y[13];
    invT_tmp[5] = y[12];
    invT_tmp[8] = 0.0;
    for (b_i = 0; b_i < 3; b_i++) {
      s = invT_tmp[b_i + 3];
      d = invT_tmp[b_i + 6];
      for (k = 0; k < 3; k++) {
        Adgab_tmp = k << 2;
        b_invT_tmp[b_i + 3 * k] = (invT_tmp[b_i] * y[Adgab_tmp] + s *
          y[Adgab_tmp + 1]) + d * y[Adgab_tmp + 2];
        Adgab[k + 6 * b_i] = y[k + (b_i << 2)];
        Adgab[k + 6 * (b_i + 3)] = 0.0;
      }
    }

    for (b_i = 0; b_i < 3; b_i++) {
      Adgab[6 * b_i + 3] = b_invT_tmp[3 * b_i];
      aoffset = b_i << 2;
      Adgab_tmp = 6 * (b_i + 3);
      Adgab[Adgab_tmp + 3] = y[aoffset];
      Adgab[6 * b_i + 4] = b_invT_tmp[3 * b_i + 1];
      Adgab[Adgab_tmp + 4] = y[aoffset + 1];
      Adgab[6 * b_i + 5] = b_invT_tmp[3 * b_i + 2];
      Adgab[Adgab_tmp + 5] = y[aoffset + 2];
    }

    for (Adgab_tmp = 0; Adgab_tmp < 6; Adgab_tmp++) {
      aoffset = Adgab_tmp * 6;
      s = 0.0;
      for (k = 0; k < 6; k++) {
        s += Adgab[aoffset + k] * Fmat[i + 9 * k];
      }

      Flist[Adgab_tmp + 6 * i] = s;
    }

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

// End of code generation (getSimpackF.cpp)