#include "interlin.h"
#include <stdio.h>
#include <unistd.h>


typedef struct axisSet_s {
  axies_t        Interpolator;
  StepperSet_t   PhaseSet1;
  parallel_t *   IOPort1;
} axisSet_t;


axisSet_t robot;


void motorSync() {
   SyncMotors(&robot.PhaseSet1);
   parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);
   usleep(9000);
 // printf("0: %d \n", robot.Interpolator.axii[0].current);
 // printf("1: %d \n", robot.Interpolator.axii[1].current);
 // printf("2: %d \n", robot.Interpolator.axii[2].current);
}

void BaseCCW() {
  robot.PhaseSet1.stepBuffer |= M1F;
}

void BaseCW() {
  robot.PhaseSet1.stepBuffer |= M1R;
}

void ArmUp() {
 robot.PhaseSet1.stepBuffer |= M2F;
}

void ArmDown() {
 robot.PhaseSet1.stepBuffer |= M2R;
}

void ArmOut() {
 robot.PhaseSet1.stepBuffer |= M3F;
}

void ArmIn() {
 robot.PhaseSet1.stepBuffer |= M3R;
}

int main(void) {
  int i;
  int a;
  float Place1;
  float Place2;
  StepControl_t stepme;
  
  robot.IOPort1 = parallel_Init(0x278);
  stepInit( &robot.PhaseSet1);
  axisInit( &(robot.Interpolator), motorSync );
  
  axisAdd(&(robot.Interpolator),  0,  0, BaseCCW, BaseCW);
  axisAdd(&(robot.Interpolator),  0,  0, ArmUp, ArmDown);
  axisAdd(&(robot.Interpolator),  0,  0, ArmOut, ArmIn);  
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  
 
   // ------------ zero base rotation motor ---------------
   // go forward to limit - fast
   while ((parallel_ReadStatus(robot.IOPort1)&32)==32){
     robot.Interpolator.axii[0].target+=4;
     SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }
   // go backward to limit
   while ((parallel_ReadStatus(robot.IOPort1)&32)==0){
     robot.Interpolator.axii[0].target-=1;
     SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }

   robot.Interpolator.axii[0].current=0  ;
   robot.Interpolator.axii[0].target=0;
   printf ("axis one is go\n");

   // --------------------------------------------------------


   // ----------------  zero lift (axis 1) ---------------------
   // axis 2 has to move with it
   // go forward to limit - fast
   while ((parallel_ReadStatus(robot.IOPort1)&64)==64){
       robot.Interpolator.axii[1].target-=4;
       robot.Interpolator.axii[2].target+=4;
       SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }
   
   // go backward to limit
   while ((parallel_ReadStatus(robot.IOPort1)&64)==0){
     robot.Interpolator.axii[1].target+=1;
     robot.Interpolator.axii[2].target-=1;     
     SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }
   
   robot.Interpolator.axii[1].current=0  ;
   robot.Interpolator.axii[1].target=0;
   printf ("axis 2 is go\n");
   
   // ---------------------------------------------------------


  
   // ------------------- zero axis 2 elbow --------------------
   // go forward to limit - fast
   while ((parallel_ReadStatus(robot.IOPort1)&128)==128){
       robot.Interpolator.axii[2].target-=4;
       SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }
   
      // go backward to limit
   while ((parallel_ReadStatus(robot.IOPort1)&128)==0){
     robot.Interpolator.axii[2].target+=1;    
     SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   }
   
   robot.Interpolator.axii[2].current=0  ;
   robot.Interpolator.axii[2].target=0;
   printf ("axis 1\n");

   //---------------------------- initialization done --------------------




  printf ("zeroing done/n statring movement/n");
  
  while(1) { 

    robot.Interpolator.axii[1].target = 1000; // elbow out
    robot.Interpolator.axii[2].target = -160; // main down
    robot.Interpolator.axii[0].target = 350; // base right
    SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

    sleep(1);

    // go up
    robot.Interpolator.axii[1].target = 1000; // elbow out
    robot.Interpolator.axii[2].target = 0; // main down
    robot.Interpolator.axii[0].target = 350; // base right
    SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

    robot.Interpolator.axii[1].target = 1000; // elbow out
    robot.Interpolator.axii[2].target = -130; // main down
    robot.Interpolator.axii[0].target = 1000; // base right
    SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

    sleep(1);

    // lift arm back up
    robot.Interpolator.axii[1].target = 1000; // elbow out
    robot.Interpolator.axii[2].target = 0; // main down
    robot.Interpolator.axii[0].target = 1000; // base right
    SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

  }
  
  // goto zero 
  robot.Interpolator.axii[1].target = 0; // elbow out
  robot.Interpolator.axii[2].target = 0; // main down
  robot.Interpolator.axii[0].target = 0; // base right
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

  printf("0: %d \n", robot.Interpolator.axii[0].current);
  printf("1: %d \n", robot.Interpolator.axii[1].current);
  printf("2: %d \n", robot.Interpolator.axii[2].current);

 
  parallel_Destroy (robot.IOPort1);
  printf("%d\n",robot.Interpolator.axii[1].current-700);
  return 0;
  
}