text: // Copyright 2011 Jonán C. Martín
  // 
  // Licensed under the Apache License, Version 2.0 (the "License");
  // you may not use this file except in compliance with the License.
  // You may obtain a copy of the License at
  // 
  //   http://www.apache.org/licenses/LICENSE-2.0
  // 
  // Unless required by applicable law or agreed to in writing, software
  // distributed under the License is distributed on an "AS IS" BASIS,
  // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  // See the License for the specific language governing permissions and
  // limitations under the License.
  
  #include "rhino_xr4.hpp"
  
  #include <cmath>
  
  #include <iostream>
  using namespace std;
  
  real nullify(real num)
  {
      if (abs(num) < TOL)
          return 0.0;
      else
          return num;
  }
  
  float truncate(float num)
  {
      long x = 1000 * num;
      float y = (float)x / (float)1000;
      return y;
  }
  
  real sign(real num)
  {
      return (num < 0.0 ? -1 : 1);
  }
  
  real deg2rad(real angle)
  {
      return (PI*angle)/180;
  }
  
  real rad2deg(real angle)
  {
      return (angle*180)/PI;
  }
  
  JointAngles::JointAngles(real t1, real t2, real t3, real t4, real t5)
  {
      this->t1 = deg2rad(t1);
      this->t2 = deg2rad(t2);
      this->t3 = deg2rad(t3);
      this->t4 = deg2rad(t4);
      this->t5 = deg2rad(t5);
  }
  
  Range::Range(real min, real max)
  {
      this->min = min;
      this->max = max;
  }
  
  void JointAngles::print()
  {
      cout << "t1 = " << rad2deg(t1) << " degrees" << endl;
      cout << "t2 = " << rad2deg(t2) << " degrees" << endl;
      cout << "t3 = " << rad2deg(t3) << " degrees" << endl;
      cout << "t4 = " << rad2deg(t4) << " degrees" << endl;
      cout << "t5 = " << rad2deg(t5) << " degrees" << endl;
  }
  
  RhinoXR4::RhinoXR4()
  {
      angles.t1 = 0;
      angles.t2 = 0;
      angles.t3 = 0;
      angles.t4 = 0;
      angles.t5 = 0;
  }
  
  Matrix RhinoXR4::dirkin(JointAngles &angles)
  {
      Matrix matrix(4, 4);
      
      real d1   = 260;
      real a2   = 228.6;
      real a3   = 228.6;
      real a4   = 9.5;
      real d5   = 165;
      
      real s1   = sin(angles.t1);
      real c1   = cos(angles.t1);
      real s2   = sin(angles.t2);
      real c2   = cos(angles.t2);
      real s5   = sin(angles.t5);
      real c5   = cos(angles.t5);
      real s23  = sin(angles.t2 + angles.t3);
      real c23  = cos(angles.t2 + angles.t3);
      real s234 = sin(angles.t2 + angles.t3 + angles.t4);
      real c234 = cos(angles.t2 + angles.t3 + angles.t4);
      
      matrix.at(1, 1) = nullify(s1*s5 + c5*c1*c234);
      matrix.at(1, 2) = nullify(c5*s1 - s5*c1*c234);
      matrix.at(1, 3) = nullify(c1*s234);
      matrix.at(1, 4) = nullify(c1*(d5*s234 + a2*c2 + a3*c23 + a4*c234));
      
      matrix.at(2, 1) = nullify(c5*s1*c234 - c1*s5);
      matrix.at(2, 2) = nullify(-c1*c5 - s5*s1*c234);
      matrix.at(2, 3) = nullify(s1*s234);
      matrix.at(2, 4) = nullify(s1*(d5*s234 + a2*c2 + a3*c23 + a4*c234));
      
      matrix.at(3, 1) = nullify(c5*s234);
      matrix.at(3, 2) = nullify(-s5*s234);
      matrix.at(3, 3) = nullify(-c234);
      matrix.at(3, 4) = nullify(d1 - d5*c234 + a2*s2 + a3*s23 + a4*s234);
      
      matrix.at(4, 1) = 0;
      matrix.at(4, 2) = 0;
      matrix.at(4, 3) = 0;
      matrix.at(4, 4) = 1;
      
      return matrix;
  }
  
  JointAngles RhinoXR4::invkin(Matrix &matrix)
  {
      JointAngles angles;
      real A, B, alph, sa, s1, c1, s3, c3;
  
      real d1 = 260;
      real a2 = 228.6;
      real a3 = 228.6;
      real a4 = 9.5;
      real d5 = 165;
  
      // row 1
      real r11 = matrix.at(1, 1);
      real r12 = matrix.at(1, 2);
      real r13 = matrix.at(1, 3);
      qDebug("r11 = %f\tr12 = %f\tr13 = %f", (double)r11, (double)r12, (double)r13);
      // row 2
      real r21 = matrix.at(2, 1);
      real r22 = matrix.at(2, 2);
      real r23 = matrix.at(2, 3);
      qDebug("r21 = %f\tr22 = %f\tr23 = %f", (double)r21, (double)r22, (double)r23);
      // row 3
      real r33 = matrix.at(3, 3);
      // row 4
      real t1  = matrix.at(1, 4);
      real t2  = matrix.at(2, 4);
      real t3  = matrix.at(3, 4);
  
      angles.t1 = atan2(t2, t1);
      qDebug("t1 = %f rad (%f deg)", (double)angles.t1, (double)rad2deg(angles.t1));
      qDebug("deg2rad(jointRangeDeg('F').min) = %f", (double)deg2rad(jointRangeDeg('F', false).min));
      qDebug("deg2rad(jointRangeDeg('F').max) = %f", (double)deg2rad(jointRangeDeg('F', false).max));
      if (angles.t1 < deg2rad(jointRangeDeg('F').min) || angles.t1 > deg2rad(jointRangeDeg('F').max))
          angles.t1 += -sign(angles.t1)*PI; // t1 + 180º
      qDebug("t1 = %f rad (%f deg)", (double)angles.t1, (double)rad2deg(angles.t1));
      s1 = sin(angles.t1);
      qDebug("s1 = %f", (double)s1);
      c1 = cos(angles.t1);
      qDebug("c1 = %f", (double)c1);
  
      angles.t5 = atan2(s1*r11 - c1*r21, s1*r12 - c1*r22);
      qDebug("s1*r11 = %f", (double)(s1*r11));
      qDebug("c1*r21 = %f", (double)(c1*r21));
      qDebug("s1*r11 - c1*r21 = %f", (double)(s1*r11 - c1*r21));
      qDebug("s1*r12 = %f", (double)(s1*r12));
      qDebug("c1*r22 = %f", (double)(c1*r22));
      qDebug("s1*r12 - c1*r22 = %f", (double)(s1*r12 - c1*r22));
      qDebug("t5 = %f rad (%f deg)", (double)angles.t5, (double)rad2deg(angles.t5));
  
      alph = atan2(c1*r13+s1*r23, -r33);
      qDebug("alpha = %f (%f deg)", (double)alph, (double)rad2deg(alph));
      sa = sin(alph);
      qDebug("sa = %f", (double)sa);
  
      // calculation of th3
      A = t3 - d1 - d5*r33 - a4*sa;
      qDebug("A = %f", (double)A);
      if (nullify(s1) == 0.0)
          B = t1/c1 - d5*sa + a4*r33;
      else
          B = t2/s1 - d5*sa + a4*r33;
      qDebug("B = %f", (double)B);
      qDebug("(A*A + B*B - a2*a2 - a3*a3) = %f", (double)(A*A + B*B - a2*a2 - a3*a3));
      qDebug("(A*A + B*B - a2*a2 - a3*a3) / (2*a2*a3) = %f", (double)((A*A + B*B - a2*a2 - a3*a3) / (2*a2*a3)));
      real tmp = (A*A + B*B - a2*a2 - a3*a3) / (2*a2*a3);
      tmp = tmp > 1.0 ? 1.0 : tmp;
      angles.t3 = -acos(truncate(tmp)); // elbow-up solution
      qDebug("t3 = %f (%f deg)", (double)angles.t3, (double)rad2deg(angles.t3));
      s3 = sin(angles.t3);
      qDebug("s3 = %f", (double)s3);
      c3 = cos(angles.t3);
      qDebug("c3 = %f", (double)c3);
  
      // calculation of th2
      qDebug("A*(a2+a3*c3) = %f", (double)(A*(a2+a3*c3)));
      qDebug("B*a3*s3 = %f", (double)(B*a3*s3));
      qDebug("A*(a2+a3*c3)-B*a3*s3 = %f", (double)(A*(a2+a3*c3)-B*a3*s3));
      qDebug("B*(a2+a3*c3) = %f", (double)(B*(a2+a3*c3)));
      qDebug("A*a3*s3 = %f", (double)(A*a3*s3));
      qDebug("B*(a2+a3*c3)+A*a3*s3 = %f", (double)(B*(a2+a3*c3)+A*a3*s3));
      qDebug("A*(a2+a3*c3)-B*a3*s3 / B*(a2+a3*c3)+A*a3*s3 = %f", (double)((A*(a2+a3*c3)-B*a3*s3) / (B*(a2+a3*c3)+A*a3*s3)));
      angles.t2 = atan2(A*(a2+a3*c3)-B*a3*s3, B*(a2+a3*c3)+A*a3*s3);
      qDebug("t2 = %f (%f deg)", (double)angles.t2, (double)rad2deg(angles.t2));
  
      // calculation of th4
      angles.t4 = alph - angles.t2 - angles.t3;
  
      angles.t1 = nullify(angles.t1);
      angles.t2 = nullify(angles.t2);
      angles.t3 = nullify(angles.t3);
      angles.t4 = nullify(angles.t4);
      angles.t5 = nullify(angles.t5);
  
      return angles;
  }
  
  JointAngles RhinoXR4::invkin2(Matrix &matrix)
  {
      JointAngles angles;
      real t234, s234, c234, A, B, s1, c1, s3, c3;
      
      real d1 = 260;
      real a2 = 228.6;
      real a3 = 228.6;
      real a4 = 9.5;
      real d5 = 165;
      
      // N
      real nx = matrix.at(1, 1);
      real nz = matrix.at(3, 1);
      // S
      real sx = matrix.at(1, 2);
      real sz = matrix.at(3, 2);
      // A
      real r13 = matrix.at(1, 3);
      real r23 = matrix.at(2, 3);
      real az = matrix.at(3, 3);
      real &r33 = az;
      // P
      real px = matrix.at(1, 4);
      real py = matrix.at(2, 4);
      real pz = matrix.at(3, 4);
  
      //qDebug("az = %f", (double)az);
      //t234 = acos(-az);
      //s234 = sin(t234);
      //c234 = cos(t234);
  
      angles.t1 = atan2(py, px);
      if (angles.t1 < deg2rad(jointRangeDeg('F', false).min) || angles.t1 > deg2rad(jointRangeDeg('F', false).max))
          angles.t1 += PI; // t1 + 180º
      qDebug("t1 = %f (%f deg)", (double)angles.t1, (double)rad2deg(angles.t1));
      s1 = sin(angles.t1);
      c1 = cos(angles.t1);
  
      t234 = atan2(c1*r13+s1*r23, -r33);
      qDebug("t234 = %f (%f deg)", (double)t234, (double)rad2deg(t234));
      s234 = sin(t234);
      c234 = cos(t234);
  
      if (nullify(s234) == 0.0)
      {
          if (c234 > 0)
              angles.t5 = angles.t1 - atan2(sx, nx);
          else
              angles.t5 = -angles.t1 + atan2(sx, -nx);
      }
      else
          angles.t5 = atan2(-sz, nz);
  
      A = pz - a4*s234 + d5*c234 - d1;
      B = px*c1 + py*s1 - a4*c234 - d5*s234;
  
      real num = A*A + B*B - a2*a2 - a3*a3;
      real den = 2*a2*a3;
      qDebug("num / den = %f", (double)(num/den));
      angles.t3 = -acos(truncate(num / den));
      s3 = sin(angles.t3);
      c3 = cos(angles.t3);
  
      num = A*(a2 + a3*c3) - B*a3*s3;
      den = A*a3*s3 + B*(a2 + a3*c3);
      angles.t2 = atan2(num, den);
  
      angles.t4 = t234 - angles.t2 - angles.t3;
      
      angles.t1 = nullify(angles.t1);
      angles.t2 = nullify(angles.t2);
      angles.t3 = nullify(angles.t3);
      angles.t4 = nullify(angles.t4);
      angles.t5 = nullify(angles.t5);
      
      return angles;
  }
  
  real RhinoXR4::deg2steps(real deg, char motor, bool correct)
  {
      switch (motor)
      {
      case 'B':
          if (correct)
              return (deg - 180)*12.8;
          else
              return deg*12.8;
      case 'C':
          return deg*35.2736;
      case 'D':
          if (correct)
              return (deg + 90)*35.2736;
          else
              return deg*35.2736;
      case 'E':
          if (correct)
              return (deg - 90)*35.2736; // Home de la cinemática: 1196 \simeq 1200
          else
              return deg*35.2736; // Home de la cinemática: 1196 \simeq 1200
      case 'F':
          if (correct)
              return (deg - 90)*17.6368;
          else
              return deg*17.6368;
      default:
          throw new InvalidMotor();
      }
  }
  
  real RhinoXR4::steps2deg(real steps, char motor, bool correct)
  {
      switch (motor)
      {
      case 'B':
          if (correct)
              return steps*0.078125 + 180;
          else
              return steps*0.078125;
      case 'C':
          return steps*0.02834981402522;
      case 'D':
          if (correct)
              return steps*0.02834981402522 - 90;
          else
              return steps*0.02834981402522;
      case 'E':
          if (correct)
              return steps*0.02834981402522 + 90; // Home de la cinemática: 1196 \simeq 1200
          else
              return steps*0.02834981402522; // Home de la cinemática: 1196 \simeq 1200
      case 'F':
          if (correct)
              return steps*0.05669962805044 + 90;
          else
              return steps*0.05669962805044;
      default:
          throw new InvalidMotor();
      }
  }
  
  Range RhinoXR4::jointRangeDeg(char motor, bool correct)
  {
      switch (motor)
      {
      case 'B':
          return Range(steps2deg(-20000, 'B', true), steps2deg(20000, 'B', true));
      case 'C':                             
          return Range(steps2deg(-2800, 'C', true), steps2deg(8200, 'C', true));
      case 'D':                             
          return Range(steps2deg(-2200, 'D', true), steps2deg(5115, 'D', true));
      case 'E':                             
          return Range(steps2deg(-550, 'E', true), steps2deg(4300, 'E', true));
      case 'F':                             
          return Range(steps2deg(-3056, 'F', true), steps2deg(3056, 'F', correct));
      default:
          throw new InvalidMotor();
      }
  }
  
  Range RhinoXR4::jointRangeSteps(char motor)
  {
      switch (motor)
      {
      case 'B':
          return Range(-20000, 20000);
      case 'C':
          return Range(-2800, 8200);
      case 'D':
          return Range(-2200, 5115);
      case 'E':
          return Range(-550, 4300);
      case 'F':
          return Range(-3056, 3056);
      default:
          throw new InvalidMotor();
      }
  }
  
  InvalidMotor::InvalidMotor()
  {
      _description = QObject::tr("Invalid motor letter");
      setMessage(QObject::tr("Invalid mottor letter: motor letter must be between B and F"));
  }
  