#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;   // create a set of stepper motor sequencers
  
  robot.IOPort1 = parallel_Init(0x278);  // create a parallel port
  stepInit( &robot.PhaseSet1);           // initialize step buffering system
  axisInit( &(robot.Interpolator), motorSync ); // initialize the interpolator with the motor synchronization function
  
  axisAdd(&(robot.Interpolator),  0,  0, BaseCCW, BaseCW);  // init functions to control first axis
  axisAdd(&(robot.Interpolator),  0,  0, ArmUp, ArmDown);  
  axisAdd(&(robot.Interpolator),  0,  0, ArmOut, ArmIn);  
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );  // call to init axies
  
   
  robot.Interpolator.axii[2].target = 60; 
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );

  robot.Interpolator.axii[2].target = -70;  
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[0].target = 200;  
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
   
  robot.Interpolator.axii[2].target = 30;
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[2].target = -50;
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[0].target = 375;   
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[2].target = 50;
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[2].target = -50;
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
  
  robot.Interpolator.axii[0].target = -5;  
  robot.Interpolator.axii[2].target = -4;
  robot.Interpolator.axii[2].target = 0;  
  SimotaniouslyLinearlyInterpolateMultiAxis( &robot.Interpolator );
 
  parallel_Destroy (robot.IOPort1);

  return 0;
}