#include <robot.h>
#include <plot.h>

int main() {
 array double P1[4][4],        //T-matrix  
       jold1[6]={0,0,0,0,0,0}, //old joint values
           p1[6];              //joint values
 // Puma is configurated as flip, below and left
 int robot1conf = FLIP|ABOVE|LEFT;  
 array double joint0[36],joint1[36],joint2[36],
              joint3[36],joint4[36],joint5[36];
 double deltax, deltay, deltaz;
 array double X[36], Y[36], Z[36];
 int x[36];
 int i;
 class CPlot plot, subplot, *spl;

 linspace(x,0,36);

//construct class robot1 with a 
//default option for simulation 
 class CRobot robot1=CRobot(ROBOT1);//setup Puma 

 deltax = -300; deltay = 150; deltaz = 200;
 /* calculate the position of the arms */ 
 for (i = 0; i<36; i++) {
  P1 = robot1ZeroPosition;//P1 in zero position
  P1[0][3] += 300; P1[1][3] += 300;
  P1[2][3] += -600;      // Puma wrist position
  P1[0][3] = P1[0][3] + i*deltax/36;
  P1[1][3] = P1[1][3] + i*deltay/36;
  P1[2][3] = P1[2][3] + i*deltaz/36;
  X[i] = P1[0][3];
  Y[i] = P1[1][3];
  Z[i] = P1[2][3];
// inverse kinematics
  robot1.InverseKinematics(P1,jold1,p1,robot1conf); 
  joint0[i] = p1[0]; 
  joint1[i] = p1[1]; 
  joint2[i] = p1[2]; 
  joint3[i] = p1[3]; 
  joint4[i] = p1[4]; 
  joint5[i] = p1[5]; 
 }
 plotxyz(X,Y,Z,"end-effector trajectory","x","y","z");
 plotxy(x,joint0,"joint 1","Time(sec)","Theta1(deg.)");
 plotxy(x,joint1,"joint 2","Time(sec)","Theta2(deg.)");
 plotxy(x,joint2,"joint 3","Time(sec)","Theta3(deg.)");
 plotxy(x,joint3,"joint 4","Time(sec)","Theta4(deg.)");
 plotxy(x,joint4,"joint 5","Time(sec)","Theta5(deg.)");
 plotxy(x,joint5,"joint 6","Time(sec)","Theta6(deg.)");

 exit(0);          // exit the CH language environment 
}