/*============= Mathematica program RCRCR.mff =================
  Null 
===============================================================*/ 
void RCRCR(double dual a[5],double d[3],double dual theta1 
           double dual theta2[4],theta3[4],theta4[4],theta5[4]) 
{ 
  #define Power pow
  int j; 
  double t1,d1,d3,d5,X,EE,FF,GG,HH,II,JJ,
         SIN3,COS3, Ax,Ay,Bx,By,Cx,Cy,Dx,Dy; 
  double dual T1,T2,T3,T4,T5,a1,a2,a3,a4,a5,AA,BB,CC,DD,AA1,
              BB1,CC1,DD1,EE1,FF1,AA2,BB2,CC2,DD2,EE2,FF2; 
  double complex roots[4],p[5]; 

  T1=theta1; 
  a1=a[0]; a2=a[1]; a3=a[2]; a4=a[3]; a5=a[4]; 
  d1=d[0]; d3=d[1]; d5=d[2];
  t1=real(T1); 
  AA=-(Ca2*Ca3) + Ca1*Ca4*Ca5 - C1*Ca4*Sa1*Sa5;  BB=Sa2*Sa3;             /* eq(7-8) */
  CC=S1*Sa1*Sa4;  DD=-(C1*Ca5*Sa1*Sa4) - Ca1*Sa4*Sa5;                    /* eq(9-10) */
  Ax=real(AA); Ay=dual(AA); Bx=real(BB); By=dual(BB);                    
  Cx=real(CC); Cy=dual(CC); Dx=real(DD); Dy=dual(DD);
  EE=(Ay*Bx - Ax*By - Bx*Cx*d5 + By*Dx - Bx*Dy)/(Power(Bx,2)*d3);        /* eq(18) */
  FF=(2*(-(By*Cx) + Bx*Cy - Bx*d5*Dx))/(Power(Bx,2)*d3);                 /* eq(19) */
  GG=(Ay*Bx - Ax*By + Bx*Cx*d5 - By*Dx + Bx*Dy)/(Power(Bx,2)*d3);        /* eq(20) */
  HH=(-Ax + Dx)/Bx; II=(-2*Cx)/Bx;  JJ=(-Ax - Dx)/Bx;                    /* eq(21-23) */ 
  p[4]=-1 + Power(EE,2) + Power(HH,2);                                   /* eq(26) */
  p[3]=2*(EE*FF + HH*II);                                                /* eq(27) */
  p[2]=-2 + Power(FF,2) + 2*EE*GG + Power(II,2) + 2*HH*JJ;               /* eq(28) */
  p[1]=2*(FF*GG + II*JJ);                                                /* eq(29) */
  p[0]=-1 + Power(GG,2) + Power(JJ,2);                                   /* eq(30) */
  zroots(p,4,roots,1);  /* solve polynomial equation */
  for(j=0;j<=3;j++) { 
    X=real(roots[j]);
    T5=dual(2*atan(X), d5);                                              /* eq(31) */
    SIN3=(GG + FF*X + EE*Power(X,2))/(1+X*X);                            /* eq(16) */
    COS3=(JJ + II*X + HH*Power(X,2))/(1+X*X);                            /* eq(17) */
    T3=dual(2*atan(SIN3/(1+COS3)), d3);                                  /* eq(32) */
    AA1=-(C5*Ca5*S1*Sa4) - C1*S5*Sa4 - Ca4*S1*Sa5; 
    BB1=Ca4*Ca5*Sa1 + C1*C5*Ca1*Ca5*Sa4 - Ca1*S1*S5*Sa4 + C1*Ca1*Ca4*Sa5 - C5*Sa1*Sa4*Sa5; 
    CC1=-(Ca3*Sa2) - C3*Ca2*Sa3; DD1=-(S3*Sa3); 
    EE1=S3*Sa3; FF1=-(Ca3*Sa2) - C3*Ca2*Sa3; 
    T2=2*atan((AA1*FF1-BB1*DD1)/(CC1*FF1-DD1*EE1+BB1*CC1-AA1*EE1));      /* eq(37) */
    AA2=-(C5*S1*Sa1) - C1*Ca5*S5*Sa1 - Ca1*S5*Sa5;
    BB2=-(C1*C5*Ca4*Ca5*Sa1) + Ca4*S1*S5*Sa1 - Ca1*Ca5*Sa4 - C5*Ca1*Ca4*Sa5 + C1*Sa1*Sa4*Sa5; 
    CC2=-(C3*Ca3*Sa2) - Ca2*Sa3; DD2=-(S3*Sa2); 
    EE2=-(S3*Sa2); FF2=C3*Ca3*Sa2 + Ca2*Sa3; 
    T4=2*atan((AA2*FF2-BB2*DD2)/(CC2*FF2-DD2*EE2+BB2*CC2-AA2*EE2));      /* eq(42) */
    theta2[j]=T2; theta3[j]=T3; theta4[j]=T4; theta5[j]=T5;
  } 
}