#include "robot.h"

#define NUM_RUNS 3

int main(void) 
{
   // IBM Variables
   array double 
     ibm_jold[4]         = {0,0,0,0},
     ibm_jnew[4],
     ibm_tm[4][4],
     ibm_etm[4][4],
     ibm_ready2[4]       = {120.0,143.0,-0.21,0},
     ibm_PickupPos[3][2] = {319,   387, 
                            271.5, 370, 
                            224,   363};
   // IBM 7575 Configuration
   int ibm_conf = FLIP;

   // PUMA Variables
   array double 
     puma_jold[6]       = {0,0,0,0,0,0},
     puma_jnew[6],
     puma_tm[4][4],
     puma_etm[4][4],
     puma_DropoffPos[3] = {-643, -702, -765};

   // Puma 560 Configuration
   int puma_conf = FLIP|ABOVE|RIGHT;

   // Drop off transformation matrix
   array double puma_DropoffTM[4][4] =
       {-0.996195, -0.087156, -0.000000,  270.00,
        -0.087156,  0.996195,  0.000000, -646.00,
        -0.000000, -0.000000, -1.000000, -200.00,
         0.000000,  0.000000,  0.000000,    1.00};

   // Pickup transformation matrix
   array double puma_PickupTM[4][4] =
       {0.358368,  0.933580,  0.000000, -653.00,
        0.933580, -0.358368,  0.000000,  315.00,
        0.000000,  0.000000, -1.000000, -250.00,
        0.000000,  0.000000,  0.000000,    1.00};

   // Conveyer Variables
   array double 
     conveyorPos[3]      = {427, 203, 200},
     conveyorDisp[1]     = {0};

   // Helper variables
   int status, j;	

   class CRobot  ibm = CRobot(ROBOT2,    RUN_REALTIME),
                puma = CRobot(ROBOT1,    RUN_REALTIME),
            conveyor = CRobot(CONVEYOR1, RUN_REALTIME);

   status = puma.Calibrate();    
   status = ibm.Calibrate();    
   status = conveyor.Calibrate();    
   
   status = ibm.Drive(ibm_ready2); 
   status = ibm.MoveWait(); 
   status = ibm.MoveReady(); 
   status = ibm.MoveWait(); 
  
   while(1)
   {
     for(j=0; j<NUM_RUNS; j++)
     { 
       conveyorDisp[0] = conveyorPos[j];
       conveyor.Drive(conveyorDisp);

       puma_tm = puma_PickupTM;
       puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
       puma.Drive(puma_jnew);

       ibm_tm=robot2ZeroPosition;
       ibm_tm[0][0] =  cos(110*3.1415/180);
       ibm_tm[0][1] = -sin(110*3.1415/180);
       ibm_tm[1][0] =  sin(110*3.1415/180);
       ibm_tm[1][1] =  cos(110*3.1415/180);
       ibm_tm[0][3] =   ibm_PickupPos[j][0]; 
       ibm_tm[1][3] =  340.0;
       ibm_tm[2][3] =  -20.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);

       //ALL: Wait untill move complete
       puma.MoveWait();
       ibm.MoveWait();
       conveyor.MoveWait();

       // IBM: Drop Z down right above the part
       ibm_tm[0][3] =   ibm_PickupPos[j][0];
       ibm_tm[1][3] =  340.0;
       ibm_tm[2][3] = -150.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);
       ibm.MoveWait();

       // PUMA: Open the gripper
       printf("PUMA Opening Gripper\n");
       status = puma.GripperOpen();

       // IBM: Open the gripper
       printf("IBM Opening Gripper\n");
       status = ibm.GripperOpen();

       // PUMA: Slowly move the gipper into position
       puma_etm = puma_PickupTM;
       puma_etm[2][3] = -310.00;
       puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);

       // IBM: Slowly move the gipper into position
       ibm_tm[0][3]  =   ibm_PickupPos[j][0];
       ibm_tm[1][3]  =  340.0;
       ibm_tm[2][3]  = -150.0;
       ibm_etm       = ibm_tm;
       ibm_etm[1][3] = ibm_PickupPos[j][1];
       ibm.MoveLine(ibm_tm, ibm_etm, 10, ibm_conf);

       // PUMA: Close the gripper
       printf("PUMA Closing Gripper\n");
       puma.GripperClose();

       // IBM: Close the gripper
       printf("IBM Closing Gripper\n");
       status = ibm.GripperClose();

       // PUMA: Move the part up slowly
       puma_tm = puma_etm;
       puma_etm[2][3] = -280.00;
       puma.MoveLine(puma_tm, puma_etm, 5, puma_conf);
       puma.MoveWait();

       // PUMA: Move the part up
       puma_tm[2][3] = 0;
       puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
       puma.Drive(puma_jnew);
       puma.MoveWait();

       // PUMA: Move to intermediate position
       puma_tm = puma_DropoffTM;
       puma_tm[0][3] = -400;
       puma_tm[1][3] = -300;
       puma_tm[2][3] =    0;
       puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
       puma.Drive(puma_jnew);

       // IBM: Move the part up
       ibm_tm = ibm_etm;
       ibm_tm[2][3] =  -20.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);

       // ALL: Wait for move complete
       puma.MoveWait();
       ibm.MoveWait();

       // PUMA: Move the part to the drop off position
       puma_tm = puma_DropoffTM;
       puma_tm[1][3] = puma_DropoffPos[j];
       puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
       puma.Drive(puma_jnew);

       // IBM: Move the part to the drop off position
       ibm_tm=robot2ZeroPosition;
       ibm_tm[0][0] =  cos(20*3.1415/180);
       ibm_tm[0][1] = -sin(20*3.1415/180);
       ibm_tm[1][0] =  sin(20*3.1415/180);
       ibm_tm[1][1] =  cos(20*3.1415/180);
       ibm_tm[0][3] =  347.0;
       ibm_tm[1][3] = -405.0;
       ibm_tm[2][3] =  -20.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);

       // ALL: Wait for move complete
       puma.MoveWait();
       ibm.MoveWait();

       // PUMA: Slowly move the part down to drop off 
       puma_etm = puma_tm;
       puma_etm[2][3] = -255.00;
       puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);

       // IBM: Move the part down to drop off 
       ibm_tm[2][3] =  -92.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);

       // ALL: Wait for move complete
       puma.MoveWait();
       ibm.MoveWait();

       // PUMA: Open the gripper
       printf("PUMA Opening Gripper\n");
       status = puma.GripperOpen();

       // IBM: Open the gripper
       printf("IBM Opening Gripper\n");
       status = ibm.GripperOpen();

       // PUMA: Slowly move the gripper away from the dropped part
       puma_tm        = puma_etm;
       puma_etm[2][3] = -200.00;
       puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);
       puma.MoveWait();

       // PUMA: Move to the intermediat position
       puma_tm       = puma_DropoffTM;
       puma_tm[0][3] = -400;
       puma_tm[1][3] = -300;
       puma_tm[2][3] =    0;
       puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
       puma.Drive(puma_jnew);

       // IBM: Move the gripper away from the dropped part
       ibm_tm[2][3] =  -20.0;
       printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
           ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
       ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
       ibm.Drive(ibm_jnew);

       // ALL: Wait until move complete
       puma.MoveWait();
       ibm.MoveWait();

       if(mc_GetAgentStatus(mc_current_agent) == MC_AGENT_NEUTRAL)
           break;
     }
   }

   // PUMA: Move to ready position 
   status = puma.MoveReady();

   // IBM: move to modified ready position
   //   ibm_ready2 position is ready position with Z = -50
   status = ibm.Drive(ibm_ready2); 
   status = ibm.MoveWait(); 

   // IBM: move to ready position
   status = ibm.MoveReady(); 

   // ALL: Move wait
   status = puma.MoveWait();
   status = ibm.MoveWait(); 

   // ALL: Close the gripper
   status = puma.GripperClose();
   status = ibm.GripperClose();

   // ALL: Disable
   status = puma.Disable();
   status = ibm.Disable();
   status = conveyor.Disable();

   printf("Program Complete.\n\n");

   return 0;
}