//XY variables
const int XY_LStep = 9;
const int XY_LDir = 8;

const int XY_RStep = 7;
const int XY_RDir = 6;

bool X_Calib = false;
bool Y_Calib = false;

void setup() {
  // put your setup code here, to run once:


  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
}

void loop() {
  //moveLeft(100);
  //delay(2000);
  //moveRight(100);
  //delay(2000);
  //moveUp(100);
  //delay(2000);
  //moveDown(100);
  //delay(2000);
  while(X_Calib == false)
  {
    X_Calib = calibX(X_Calib);
  }
  while(Y_Calib == false)
  {
    Y_Calib = calibY(Y_Calib);
  }
  X_Calib = false;
  while(X_Calib == false) //change to if
  {
    moveLeft(10);
    break;
  }

  //PICKING

  //TRAVEL 300 & 150
  

}

void moveLeft(int distance)
{
  digitalWrite(XY_LDir, LOW);
  digitalWrite(XY_RDir, LOW);
  
  for(int x=0; x<distance; x++)
  {
    digitalWrite(XY_LStep, HIGH);
    digitalWrite(XY_RStep, HIGH);
    delay(50);
    digitalWrite(XY_LStep, LOW);
    digitalWrite(XY_RStep, LOW);
    delay(50);
  }
}

void moveRight(int distance)
{
  digitalWrite(XY_LDir, HIGH);
  digitalWrite(XY_RDir, HIGH);
  
  for(int x=0; x<distance; x++)
  {
    digitalWrite(XY_LStep, HIGH);
    digitalWrite(XY_RStep, HIGH);
    delay(50);
    digitalWrite(XY_LStep, LOW);
    digitalWrite(XY_RStep, LOW);
    delay(50);
  }
}

void moveUp(int distance)
{
  digitalWrite(XY_LDir, HIGH);
  digitalWrite(XY_RDir, LOW);
  
  for(int x=0; x<distance; x++)
  {
    digitalWrite(XY_LStep, HIGH);
    digitalWrite(XY_RStep, HIGH);
    delay(50);
    digitalWrite(XY_LStep, LOW);
    digitalWrite(XY_RStep, LOW);
    delay(50);
  }
}

void moveDown(int distance)
{
  digitalWrite(XY_LDir, LOW);
  digitalWrite(XY_RDir, HIGH);
  
  for(int x=0; x<distance; x++)
  {
    digitalWrite(XY_LStep, HIGH);
    digitalWrite(XY_RStep, HIGH);
    delay(50);
    digitalWrite(XY_LStep, LOW);
    digitalWrite(XY_RStep, LOW);
    delay(50);
  }
}

bool calibX(bool X_Calib)
{
  digitalWrite(XY_LDir, LOW);
  for(int x=0; x<500; x++)
  {
    digitalWrite(XY_LStep, HIGH);
    delay(5);
    digitalWrite(XY_LStep, LOW);
    //if(limitSwitchX.isPressed())
    //{
    //  break;
    //}
  }
  X_Calib = true;
  return X_Calib;
}

bool calibY(bool Y_Calib)
{
  digitalWrite(XY_RDir, HIGH);
  for(int x=0; x<500; x++)
  {
    digitalWrite(XY_RStep, HIGH);
    delay(5);
    digitalWrite(XY_RStep, LOW);
    //if(limitSwitchY.isPressed())
    //{
    //  break;
    //}
  }
  Y_Calib = true;
  return Y_Calib;
}