/*
 * File: IK_pose.c
 *
 * MATLAB Coder version            : 4.2
 * C/C++ source code generated on  : 01-Oct-2019 15:10:03
 */

/* Include Files */
#include <math.h>
#include "IK_pose.h"
#include "IK_pose_data.h"

/* Function Definitions */

/*
 * Arguments    : const double Rd[9]
 *                const double Td[3]
 *                double q[5]
 * Return Type  : void
 */
void IK_pose(const double Rd[9], const double Td[3], double q[5])
{
  int i;
  double a;
  static const signed char iv0[3] = { 20, 0, 0 };

  double k3_tmp;
  static const signed char iv1[3] = { 0, 10, 0 };

  double b_k3_tmp;
  static const signed char iv2[3] = { -20, 0, 0 };

  double q_idx_0_tmp_tmp;
  double q_idx_0_tmp;
  double q_idx_0;
  double q_idx_1_tmp;
  double q_idx_1;
  double q_idx_2_tmp;
  double q_idx_2;
  double y;
  double b_y;
  double dv1[9];
  int i0;

  /* This from tip to rotation center */
  for (i = 0; i < 3; i++) {
    A1[i] = iv0[i];
    A2[i] = iv1[i];
    A3[i] = iv2[i];
    s[i] = Td[i] - ((Rd[i] * 0.0 + Rd[i + 3] * 0.0) + Rd[i + 6]) * 12.0;
  }

  a = atan(-s[1] / s[2]);
  k3_tmp = sin(a);
  b_k3_tmp = cos(a);
  a = atan((s[0] - A2[0] / b_k3_tmp * (s[2] - A2[2])) - k3_tmp * (s[1] - A2[1]));
  q_idx_0_tmp_tmp = cos(a);
  q_idx_0_tmp = 10.0 * q_idx_0_tmp_tmp / 2.0;
  q_idx_0 = (s[0] - q_idx_0_tmp) - A1[0];
  a = sin(a);
  q_idx_1_tmp = 10.0 * k3_tmp * a / 2.0;
  q_idx_1 = (s[1] - q_idx_1_tmp) - A1[1];
  q_idx_2_tmp = 10.0 * b_k3_tmp * a / 2.0;
  q_idx_2 = (s[2] + q_idx_2_tmp) - A1[2];
  y = (q_idx_0 * q_idx_0 + q_idx_1 * q_idx_1) + q_idx_2 * q_idx_2;
  q_idx_0 = s[0] - A2[0];
  q_idx_1 = (s[1] + 6.0 * b_k3_tmp) - A2[1];
  q_idx_2 = (s[2] + 6.0 * k3_tmp) - A2[2];
  b_y = (q_idx_0 * q_idx_0 + q_idx_1 * q_idx_1) + q_idx_2 * q_idx_2;
  q_idx_0 = (s[0] + q_idx_0_tmp) - A3[0];
  q_idx_1 = (s[1] + q_idx_1_tmp) - A3[1];
  q_idx_2 = (s[2] - q_idx_2_tmp) - A3[2];
  dv1[0] = q_idx_0_tmp_tmp;
  dv1[1] = 0.0;
  dv1[2] = a;
  dv1[3] = k3_tmp * a;
  dv1[4] = b_k3_tmp;
  dv1[5] = -k3_tmp * q_idx_0_tmp_tmp;
  dv1[6] = -b_k3_tmp * a;
  dv1[7] = k3_tmp;
  dv1[8] = b_k3_tmp * q_idx_0_tmp_tmp;
  for (i = 0; i < 3; i++) {
    for (i0 = 0; i0 < 3; i0++) {
      Rs[i + 3 * i0] = (dv1[i] * Rd[3 * i0] + dv1[i + 3] * Rd[1 + 3 * i0]) +
        dv1[i + 6] * Rd[2 + 3 * i0];
    }
  }

  q[0] = sqrt(y);
  q[1] = sqrt(b_y);
  q[2] = sqrt((q_idx_0 * q_idx_0 + q_idx_1 * q_idx_1) + q_idx_2 * q_idx_2);
  q[3] = atan(Rs[1] / Rs[0]);
  q[4] = atan(Rs[5] / Rs[8]);
}

/*
 * File trailer for IK_pose.c
 *
 * [EOF]
 */