#include "parallel.h"
#include "stepSeq.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;

int main(void) {
  int i;
  StepControl_t stepme;
  
  robot.IOPort1 = parallel_Init(0x278);
  stepInit( &robot.PhaseSet1);
  
  stepme = M1F |M2F | M3F; 
  for ( i = 0; i < 400; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }

  stepme = M1R ; 
  for ( i = 0; i < 200; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }
  
  stepme = M2R ; 
  for ( i = 0; i < 300; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }

  stepme = M2F ; 
  for ( i = 0; i < 300; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }
  
  stepme = M1F ; 
  for ( i = 0; i < 200; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }
  
  stepme = M1R | M2R | M3R;
  for ( i = 0; i < 400; i++) {      
    stepmotors(&robot.PhaseSet1, stepme);  
    parallel_WriteData (robot.IOPort1, robot.PhaseSet1.phases.block);    
    usleep(500);   
  }

  
  /*
  axisInit( &(robot.axies), motorSync );
  
  axisAdd(&(robot.axies),  0,  0, BaseCCW, BaseCW);
  axisAdd(&(robot.axies),  0,  0, ArmUp, ArmDown);
  axisAdd(&(robot.axies),  0,  0, ArmOut, ArmIn);
  
  
  SimotaniouslyLinearlyInterpolateMultiAxis( &axies );
  */
  
  parallel_Destroy (robot.IOPort1);

  return 0;
}