/* RCCC program used in paper
   "Programming with Dual Numbers and its Applications in Mechanisms Design",
   Engineering with Computers, Vol. 10, No. 4, 1994, pp. 212-229.
   by Cheng, H. H.
*/

#include <stdio.h>
#include <math.h>
int main(){
  void RCCC(int branch, FILE *stream);
  FILE *stream;
  stream = fopen("RCCC.out", "w"); /* open file "RCCC.out" */
  fprintf(stream, "theta1  theta2     d2(in)     theta3"); 
  fprintf(stream, "     d3(in)     theta4     d4(in)  \n\n"); 
  fprintf(stream, "                           First solution \n\n");
  RCCC(1, stream);
  fprintf(stream, "\n\n                        Second solution \n\n");
  RCCC(-1, stream);
  fclose(stream);                  /* close file "RCCC.out" */
}

void RCCC(int branch, FILE *stream)/* routine for RCCC analysis */
{
  int i;
  dual alpha1, alpha2, alpha3, alpha4, theta1, theta2, theta3, theta4;
  dual A, B, C, E31, E32, E13, E23;

  alpha1 = dual(30*M_PI/180, 2); 
  alpha2 = dual(55*M_PI/180, 4);
  alpha3 = dual(45*M_PI/180, 3); 
  alpha4 = dual(60*M_PI/180, 5);
  for(i = 0; i <=  360; i += 20)
  {
    theta1 = dual(i*M_PI/180, 0);             /* 0 <= theta1 <= 360 degree */
    A = sin(alpha1)*sin(alpha3)*sin(theta1);
    B = -sin(alpha3) * (cos(alpha1)*sin(alpha4) + 
        sin(alpha1)*cos(alpha4)*cos(theta1));
    C = cos(alpha3) * (cos(alpha1)*cos(alpha4) - 
        sin(alpha1)*sin(alpha4)*cos(theta1)) - cos(alpha2);
    theta4 = 2*atan((-A+branch*sqrt(A*A + B*B - C*C))/(C-B)); 
    E31 = sin(alpha3)*cos(theta1)*sin(theta4) +(cos(alpha3)*sin(alpha4)+
          sin(alpha3)*cos(alpha4)*cos(theta4))*sin(theta1);
    E32 = - sin(alpha3)*(cos(alpha1)*sin(theta1)*sin(theta4) + 
          (sin(alpha4)*sin(alpha1) - cos(alpha4)*cos(alpha1)*cos(theta1))*cos(theta4))  
          +cos(alpha3)*(cos(alpha4)*sin(alpha1)+sin(alpha4)*cos(alpha1)*cos(theta1));   
    theta2 = 2 *atan(E31/(sin(alpha2) - E32));
    E13 = sin(alpha1)*sin(theta1)*cos(theta4) +(cos(alpha1)*sin(alpha4)+
          sin(alpha1)*cos(alpha4)*cos(theta1))*sin(theta4);
    E23 = cos(alpha3)*(sin(alpha1)*sin(theta1)*sin(theta4) - 
          (sin(alpha4)*cos(alpha1) + cos(alpha4)*sin(alpha1)*cos(theta1))*cos(theta4))
          -sin(alpha3)*(cos(alpha4)*cos(alpha1)-sin(alpha4)*sin(alpha1)*cos(theta1));
    theta3 = 2 *atan(E13/(sin(alpha2) + E23));
    fprintf(stream, "%3.0f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f \n",
            real(theta1)*180/M_PI, real(theta2)*180/M_PI, dual(theta2), 
            real(theta3)*180/M_PI, dual(theta3), real(theta4)*180/M_PI, dual(theta4)); 
  }
}