#include <stdio.h>
#include <unistd.h>
#include <pigpio.h>

#define stp_X 2
#define dir_X 3
// #define MS1_X 4
// #define MS2_X 5
// #define MS3_X 6
// #define EN_X  7
// #define stp_Y 2
// #define dir_Y 3
// #define MS1_Y 4
// #define MS2_Y 5
// #define MS3_Y 6
// #define EN_Y  7

#define XSTEPS  0 //X movement per one step
#define YSTEPS  0 //Y movement per one step

int x=0;
int y=0;

void step(int motor,int num)
{
  while(num>0){
    gpioWrite(motor, 1);
    usleep(100); //time_sleep(0.5);
    gpioWrite(motor, 0);
    usleep(100);
    num--;
  }

}

void gotoXY(int new_x, int new_y)
{
  if(new_x-x>0)
    gpioWrite(dir_X, 1);
  else
    gpioWrite(dir_X, 0);

  if(new_y-y>0)
    gpioWrite(dir_Y, 1);
  else
    gpioWrite(dir_Y, 0);

  step(stp_X,abs(new_x-x)/XSTEPS);
  // step(stp_Y,abs(new_y-y)/YSTEPS);

  x=new_x;
  y=new_y;
}

int main(int argc, char *argv[])
{
   double start;

   if (gpioInitialise() < 0)
   {
      fprintf(stderr, "pigpio initialisation failed\n");
      return 1;
   }


   gpioSetMode(stp_X, PI_OUTPUT);
   gpioSetMode(dir_X, PI_OUTPUT);
   // gpioSetMode(MS1_X, PI_OUTPUT);
   // gpioSetMode(MS2_X, PI_OUTPUT);
   // gpioSetMode(MS3_X, PI_OUTPUT);
   // gpioSetMode(EN_X, PI_OUTPUT);

   // gpioSetMode(stp_Y, PI_OUTPUT);
   // gpioSetMode(dir_Y, PI_OUTPUT);

   // gpioSetMode(MS1_Y, PI_OUTPUT);
   // gpioSetMode(MS2_Y, PI_OUTPUT);
   // gpioSetMode(MS3_Y, PI_OUTPUT);
   // gpioSetMode(EN_Y, PI_OUTPUT);

   //Set to 1/16th microstep
   // gpioWrite(MS1_X, 1);
   // gpioWrite(MS2_X, 1);
   // gpioWrite(MS3_X, 1);
   // gpioWrite(MS1_Y, 1);
   // gpioWrite(MS2_Y, 1);
   // gpioWrite(MS3_Y, 1);

   // gpioWrite(EN_X, 0);
   // gpioWrite(EN_Y, 0);


   //get x and y and call goto fucntion
   gotoXY(50,50);
   gotoXY(10,50);

   gpioTerminate();

   return 0;
}