#include <stdio.h>
#include <mechanism3D.h>
int main() {
  int i;
  double RAD2DEG=180/PI, DEG2RAD=PI/180;
  double d[3]={0,2.5,3}; /* D-H parameters */
  double dual theta1,theta2[4],theta3[4],theta4[4],
      theta5[4], a[5]={dual(30*DEG2RAD,1),
      dual(35*DEG2RAD,4), dual(45*DEG2RAD,3),
      dual(60*DEG2RAD,2.5), dual(10*DEG2RAD,3.2)}; 

  theta1=dual(PI,d[0]); /* 180 degree for theta1 */
  RCRCR(a,d,theta1,theta2,theta3,theta4,theta5); 
  printf("%8s %8s %8s %8s %8s %8s %8s\n","theta1", 
    "theta2","d2","theta3","theta4","d4","theta5");
  for (i = 0; i<4; i++)
    printf("%8i %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", 
      180, real(theta2[i])*RAD2DEG, dual(theta2[i]), 
      real(theta3[i])*RAD2DEG, real(theta4[i])*RAD2DEG, 
      dual(theta4[i]), real(theta5[i])*RAD2DEG);
}