#include <Servo.h>

//Declare global variables
int inByte = 0;      // Holders for incoming data
byte testByte = 0;

int step_outX = 6;   // StepX digital out pin
int step_outY = 5;   // StepY digital out pin
int dirX = 4;        // DirectionX digital out pin
int dirY = 3;        // DirectionY digital out pin
int xHome = 8;       // X home limit switch digital in
int yHome = 7;       // Y home limit switch digital in

byte currentX = 0;
byte nextX = 0;
byte currentY = 0;
byte nextY = 0;
byte tempX = 0;
byte tempY = 0;

Servo magServo;

//Directional guides - Down y HIGH / Up y LOW / Left x HIGH / Right x LOW

void setup() {
  //Set up appropriate variables and serial port stuff
  Serial.begin(9600);
  Serial.flush();
  magServo.attach(9);

  //Set up pins for stepper pulses and direction and limit switches
  pinMode(8, INPUT);  
  pinMode(7, INPUT);  
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(3, OUTPUT);
  
  digitalWrite(dirX, HIGH);
  digitalWrite(dirY, HIGH);
  magServo.write(0);
}


void loop()
{
//Code to continuously check the serial port for incoming messages
  if (Serial.available()>0)
  {
    testByte = Serial.read();
    //Serial.write(testByte);
    if (testByte == 255) getCoordPairUnlatched();
    if (testByte == 254) getCoordPairLatched();
    if (testByte == 253) goToOrigin();
    if (testByte == 252) magOn();
    if (testByte == 251) magOff();
    
  }
}

void getCoordPairUnlatched(){
  testByte = 250;
  Serial.write(testByte);
  
  while (Serial.available() == 0){}
  nextX = Serial.read();
  Serial.write(nextX);
  moveX();
  
  while (Serial.available() == 0){}
  nextY = Serial.read();
  Serial.write(nextY);
  moveY();
}

void getCoordPairLatched(){ 
 testByte = 250;
 Serial.write(testByte);
  
  
  while (Serial.available() == 0){}
  nextX = Serial.read();
  Serial.write(nextX);
  testByte = 250;  //Send command signifying X command was received
  Serial.write(testByte);
  //moveX();
  
  while (Serial.available() == 0){}
  nextY = Serial.read();
  Serial.write(nextY);
  testByte = 249;
  Serial.write(testByte);
  //moveY();
  
  if ((nextX!=currentX) && (nextY!=currentY))
  {MoveDiag();
  }
  else {
    moveX();
    moveY();
  }
  
}

void moveX(){
  if (nextX < currentX){ 
    //Set direction to reverse (x right)
    digitalWrite(dirX, LOW);
    tempX = (currentX - nextX);
    stepperXPulse(tempX);
  }
  
  if (nextX > currentX){
    //Set direction to forward
    digitalWrite(dirX, HIGH);
    tempX = (nextX - currentX);
    stepperXPulse(tempX);
  }
  
  if (nextX == currentX){
    //Do nothing
  }
  
  testByte = 248;  //Send command signifying X move is done
  Serial.write(testByte);
  currentX = nextX;

}


void moveY(){
  if (nextY < currentY){ 
    //Set direction to reverse
    digitalWrite(dirY, HIGH);
    tempY = (currentY - nextY);
    stepperYPulse(tempY);
  }
  
  if (nextY > currentY){
    //Set direction to forward
    digitalWrite(dirY, LOW);
    tempY = (nextY - currentY);
    stepperYPulse(tempY);
  }
  
  if (nextY == currentY){
    //Do nothing
  }
  
  testByte = 247;  //Send command signifying Y move is done
  Serial.write(testByte);
  currentY = nextY;
}

void MoveDiag(){
  
  if(nextY < currentY){digitalWrite(dirY, HIGH);}
  if(nextY > currentY){digitalWrite(dirY, LOW);}
  
  if(nextX < currentX){digitalWrite(dirX, LOW);}
  if(nextX > currentX){digitalWrite(dirX, HIGH);}
  
  for (int i = 0; i<273; i++){
    digitalWrite(step_outX, LOW); //One X
    digitalWrite(step_outX, HIGH);
    digitalWrite(step_outX, LOW);
    delay(1);
    digitalWrite(step_outY, LOW);
    digitalWrite(step_outY, HIGH);
    digitalWrite(step_outY, LOW);
    delay(1);
    digitalWrite(step_outX, LOW);
    digitalWrite(step_outX, HIGH);
    digitalWrite(step_outX, LOW);
    delay(1);
    digitalWrite(step_outY, LOW);
    digitalWrite(step_outY, HIGH);
    digitalWrite(step_outY, LOW);
    delay(1);
    digitalWrite(step_outX, LOW);
    digitalWrite(step_outX, HIGH);
    digitalWrite(step_outX, LOW);
    delay(1);
       digitalWrite(step_outY, LOW);
    digitalWrite(step_outY, HIGH);
    digitalWrite(step_outY, LOW);
    delay(1);
    digitalWrite(step_outX, LOW);
    digitalWrite(step_outX, HIGH);
    digitalWrite(step_outX, LOW);
  }
  
   testByte = 248;  //Send command signifying Y move is done
  Serial.write(testByte);
  currentX = nextX;
  
  testByte = 247;  //Send command signifying Y move is done
  Serial.write(testByte);
  currentY = nextY;
    
}

void magOn(){
  magServo.write(90);
  delay(1000);
}

void magOff(){
  magServo.write(0);
  delay(500);
}


void goToOrigin(){
  //Serial.println("Going to Origin");
  //Set X and Y motors in reverse - Down and right
  digitalWrite(dirX, LOW);
  digitalWrite(dirY, HIGH);
  
  //While xhome is low, step X
  while (digitalRead(xHome) == LOW){
    digitalWrite(step_outX, LOW);
    digitalWrite(step_outX, HIGH);
    digitalWrite(step_outX, LOW);
    delayMicroseconds(300);
    currentX = 0;
  }
  
  //While yhome is low, step Y
    while (digitalRead(yHome) == LOW){
    digitalWrite(step_outY, LOW);
    digitalWrite(step_outY, HIGH);
    digitalWrite(step_outY, LOW);
    delayMicroseconds(400);
    currentY = 0;
  }
}


void stepperXPulse(int numPulses)
{
  for (int i = 0; i<(182*numPulses); i++){
  digitalWrite(step_outX, LOW);
  digitalWrite(step_outX, HIGH);
  digitalWrite(step_outX, LOW);
  delayMicroseconds(900);
  }

}

void stepperYPulse(int numPulses)
{
  for (int i = 0; i<(202*numPulses); i++){
  digitalWrite(step_outY, LOW);
  digitalWrite(step_outY, HIGH);
   digitalWrite(step_outY, LOW);
  delayMicroseconds(1400);
  }

}