/* A dual iterative program in CH for RCCC mechanism in paper
   "Dual Iterative Displacement Analysis
    of Spatial Mechanisms Using the \CH\ Programming Language,"
    Mechanism and Machine Theory,
    by  Cheng, H. H., and Thompson, S.
*/

#include <linkage.h>     /* PI is defined in <linkage.h> */
#define D2R PI/180
#define R2D 180/PI
main(){
  int i, j, theta1, n=4, m=6;
  float error, epsilon=0.00001, MaxError=100000;
  dual theta[n], alpha[n]; 
  array dual Q[3][3],Bi[3][3],B1[3][3],Ai[3][3],M[m][3],d[3],v[m],N[3][3],u[3];
  void ai(dual theta, alpha, array dual Ai[3][3]){    /* calculates Ai matrix */
    Ai[0][0] = cos(theta);            Ai[0][1] = -sin(theta)*cos(alpha);
    Ai[0][2] = sin(theta)*sin(alpha); Ai[1][0] = sin(theta);
    Ai[1][1] = cos(theta)*cos(alpha); Ai[1][2] = -cos(theta)*sin(alpha);
    Ai[2][0] = 0; Ai[2][1] = sin(alpha); Ai[2][2] = cos(alpha);     /*(1) */
  }

  alpha[0]=dual(30*D2R,2);  alpha[1]=dual(55*D2R,4);
  alpha[2]=dual(45*D2R,3);  alpha[3]=dual(60*D2R,5);  /* link parameters */
  theta[0]=dual(0,0);       theta[1]=dual(100*D2R,0);
  theta[2]=dual(100*D2R,0); theta[3]=dual(100*D2R,0); /* initial values */
  printf("  theta1     s1    theta2      s2     "
         "theta3     s3    theta4      s4\n");        /* print heading */
  Q = 0; Q[0][1] = -1; Q[1][0] = 1.0;                               /*(9) */
  for(theta1=0;theta1<=360;theta1+=90){        /* 90 deg step for theta1 */
    theta[0] = theta1*D2R;                     /* change deg to rad */
    do{
      for (i=1;i<n;i++){
        for (Bi=0,j=0;j<=2;j++) Bi[j][j] = 1;
        for (j=0;j<=i-1;j++,Bi*=Ai) ai(theta[j],alpha[j],Ai);       /*(16)*/
        for (Bi*=Q,j=i;j<n;j++,Bi*=Ai) ai(theta[j],alpha[j],Ai);    /*(16)*/
        M[0][i-1]=Bi[0][0]; M[1][i-1]=Bi[1][1]; M[2][i-1]=Bi[2][2]; /*(33)*/
        M[3][i-1]=Bi[1][0]; M[4][i-1]=Bi[2][0]; M[5][i-1]=Bi[2][1]; /*(34)*/
      }
      ai(theta[0],alpha[0],B1);                                     /*(14)*/
      for (i=1;i<n;i++,B1*=Ai) ai(theta[i],alpha[i],Ai);            /*(14)*/
      v[0]=1-B1[0][0]; v[1]=1-B1[1][1]; v[2]=1-B1[2][2];            /*(33)*/
      v[3]= -B1[1][0]; v[4]= -B1[2][0]; v[5]= -B1[2][1];            /*(34)*/
      N=transpose(M)*M;                                             /*(41)*/
      u=transpose(M)*v;                                             /*(42)*/
      real(d) = linearsolver(real(N), real(u));                     /*(50)*/
      dual(d) = linearsolver(real(N), dual(u) - dual(N)*real(d));   /*(51)*/
      for (error=0,i=0;i<n-1;i++)                         
          error += abs(real(d[i]))+abs(dual(d[i]));                 /*(52)*/
      for (i=1;i<n;i++) theta[i] += d[i-1]; /* update theta */      /*(3) */
    }while (error >= epsilon && error < MaxError);
    if(error >= MaxError)                   /* diverged */
      printf("Error: the iterative algorithm diverges at theta1 = %d \n", theta1);
    else                                    /* converged, print results */
      printf("%8.2f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n",
      real(theta[0])*R2D,dual(theta[0]),real(theta[1])*R2D,dual(theta[1]),
      real(theta[2])*R2D,dual(theta[2]),real(theta[3])*R2D,dual(theta[3]));
  }      
}
/****  The output from execution of the above CH program 
  theta1     s1    theta2      s2     theta3     s3    theta4      s4
    0.00    0.000  149.679   -0.210   45.556   -2.693  144.209   -0.115
   90.00    0.000   54.512   -3.171   92.715   -1.513   81.114   -2.114
  180.00    0.000  -59.094   -0.301  142.648   -1.814   83.700   -0.173
  270.00    0.000 -157.692    1.136   92.714   -1.513  148.494   -0.515
  360.00    0.000 -210.320   -0.210   45.556   -2.693  144.209   -0.115
****/