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

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;
  StepControl_t stepme;
  
  robot.IOPort1 = parallel_Init(0x278);
  stepInit( &robot.PhaseSet1);
  axisInit( &(robot.Interpolator), motorSync );
  
  axisAdd(&(robot.Interpolator),  0,  -50, BaseCCW, BaseCW);
  axisAdd(&(robot.Interpolator),  0,  100, ArmUp, ArmDown);
  axisAdd(&(robot.Interpolator),  0,  200, ArmOut, ArmIn);
  
 /* 
  for ( i = 0; i < 400; i++) {      
    robot1.PhaseSet1.stepBuffer = M1R | M2R | M3R;
    motorSync();   
  }
*/
   
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   
  parallel_Destroy (robot.IOPort1);

  return 0;
}