/* Example Newton's Interation Scheme for simple two link robot
   Bill Goodwine
   Last Updated:  21 September 1998

   Command line arguments: x1_des, x2_des, theta1_start, theta2_start
*/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

main(int argc, char *argv[]) {  
  double theta1,theta2,x1,x2,x1_des,x2_des,det,l1,l2;
  int max_tries=1000;            /* only try maxtries times */
  int i;
  l1 = 2.0;
  l2 = 2.0;

  x1_des = atof(argv[1]);        /* atof convets the command line argument */
  x2_des = atof(argv[2]);        /* which is a string, to a floating point */
  theta1 = atof(argv[3]);
  theta2 = atof(argv[4]);

  printf("Starting values: theta1 = %f,  theta2 = %f\n", theta1, theta2);

  for(i=1;i<=max_tries;i++) {
  
    det = -l2*l2 * sin(theta1 + theta2) * cos(theta1 + theta2) - 
      l1 * l2 * sin(theta1) * cos(theta1 + theta2) + 
      l2*l2 * cos(theta1 + theta2) * sin(theta1 + theta2) - 
      l1*l2 * sin(theta1 + theta2) * cos(theta1);

    theta1 -= (1.0/det)*(l2*cos(theta1 + theta2)*
      (l2*cos(theta1 + theta2) + l1*cos(theta1) - x1_des) +
      l2*sin(theta1 + theta2)*
	(l2*sin(theta1 + theta2) + l1*sin(theta1) - x2_des));

    theta2 -= (1.0/det)*((-l2*cos(theta1+theta2)+l1*cos(theta1))*
      (l2*cos(theta1 + theta2) + l1*cos(theta1) - x1_des) +
      (-l2*sin(theta1+theta2) - l1*sin(theta1))
      *(l2*sin(theta1 + theta2) + l1*sin(theta1) - x2_des));

    printf("%f\t%f\n", theta1, theta2);
  }
  printf("Final values: theta1 = %f,  theta2 = %f\n", theta1, theta2);
}