#include <SoftwareSerial.h>

int x_dir = 5;
int y_dir = 6;
int z_dir = 7;
int x_stp = 2;
int y_stp = 3;
int z_stp = 4;
int enable = 8;
int x_stop = 9;
int y_stop = 10;
bool sentido;
int steps = 2000;
int actFX = 0;
int actFY = 0;
int xStopState;
int yStopState;

char recibido;
int lenRecibido;
int fila;

int delayTime = 500;

void setup() {

  pinMode(x_dir, OUTPUT);
  pinMode(y_dir, OUTPUT);
  pinMode(z_dir, OUTPUT);
  pinMode(x_stp, OUTPUT);
  pinMode(y_stp, OUTPUT);
  pinMode(z_stp, OUTPUT);
  pinMode(x_stop, INPUT_PULLUP);
  pinMode(y_stop, INPUT_PULLUP);
  pinMode(enable, OUTPUT);
  Serial.begin(9600);
}


void loop() {

 if (Serial.available()) {
    recibido = Serial.read();
    xStopState = digitalRead(x_stop);
    yStopState = digitalRead(y_stop);

if (xStopState == HIGH && yStopState == HIGH) 
      {
       if (recibido == '1') {         // Condition For Motor Forward
       sentido = true; 
       Serial.println("Forward");         //Prints in the screen the actual state
       digitalWrite(enable, HIGH);   //Enable the writing at the CNC shield
       moveX();
       moveY();
       recibido = '2';
       }
      else if (recibido == '0') {      // Condition For Motor Backward
      sentido = false;
      Serial.println("Backward");         //Prints in the screen the actual state
      digitalWrite(enable, HIGH); 
      moveX();
      moveY();
      recibido = '2';
      }
     xStopState = digitalRead(x_stop);
     yStopState = digitalRead(y_stop);
    }
if (xStopState == LOW && yStopState == HIGH) 
      {
       if (recibido == '1') {         // Condition For Motor Forward
       sentido = true; 
       Serial.println("Forward");         //Prints in the screen the actual state
       digitalWrite(enable, HIGH);   //Enable the writing at the CNC shield
       moveX();
       moveY();
       recibido = '2';
       }
     xStopState = digitalRead(x_stop);
     yStopState = digitalRead(y_stop);
    }
if (xStopState == HIGH && yStopState == LOW) 
      {
      if (recibido == '0') {      // Condition For Motor Backward
      sentido = false;
      Serial.println("Backward");         //Prints in the screen the actual state
      digitalWrite(enable, HIGH); 
      moveX();
      moveY();
      recibido = '2';
      }
     xStopState = digitalRead(x_stop);
     yStopState = digitalRead(y_stop);
     }
  }
}

void moveX() {
  boolean dir;
  dir = sentido;
  motor(dir, x_dir, x_stp, steps);
   }

void moveY() { 
  boolean dir;
  dir = sentido;
  motor(dir, y_dir, y_stp, steps);
   }

void motor(boolean dir, int dirPin, int stpPin, int steps){
  delay(10);
  
  digitalWrite(dirPin, dir);
  for (int i = 0; i<steps; i++){
    digitalWrite(stpPin, HIGH);
    delayMicroseconds(delayTime);
    digitalWrite(stpPin, LOW);
    delayMicroseconds(delayTime);
    }  
  }