#include <SoftwareSerial.h>

int x_dir = 2;
int y_dir = 4;
int z_dir = 8;
int x_stp = 3;
int y_stp = 5;
int z_stp = 9;
int enable = 6;

int pasosPorBloqueY = 200;
int pasosPorBloqueX = 200;
int pasosPorBloqueZ = 30;
int actFX = 0;
int actFY = 0;

SoftwareSerial emision(10, 11); // RX, TX cambiar por serial principal

String recibido;
int lenRecibido;
char arrayRecibido[6];
int fila;
int columna;
int profundidad;

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(enable, OUTPUT);
  Serial.begin(9600);
  while (!emision) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  emision.begin(9600);
  
}

void loop() {
 if (Serial.available()) {

    recibido = Serial.readStringUntil(';');
    lenRecibido = recibido.length();
    if (lenRecibido >= 6){
        recibido.toCharArray(arrayRecibido, lenRecibido);
        fila = arrayRecibido[1] - 48;
        columna = arrayRecibido[2] - 48;
        profundidad = arrayRecibido[4]-48;
       digitalWrite(enable, LOW); 
       moveX(fila);
       moveY(columna);
       moveZ(profundidad);
       String salida = "L" + String(fila) + String(columna)+ String(arrayRecibido[4]) +  ";";
       emision.print(salida);
       digitalWrite(enable, HIGH); 
      }
  }
}


void moveX(int fila){
  int vFX = fila - actFX;
  actFX = actFX + vFX;
  boolean dir;
  if(vFX > 0)
  dir = true;
  else
  dir = false;
  int stpsFX = abs(vFX) * pasosPorBloqueX;
  motor(dir, x_dir, x_stp, stpsFX);
  Serial.print(vFX);
   }

void moveY(int columna){
  int vFY = columna - actFY;
  actFY = actFY + vFY;
  boolean dir;
  if(vFY > 0)
  dir = true;
  else
  dir = false;
  
  int stpsFY = abs(vFY) * pasosPorBloqueY;
   motor(dir, y_dir, y_stp, stpsFY);
   Serial.println(vFY);
   }
void moveZ(int profundidad){
   boolean dir;
   int stpsFZ = profundidad * pasosPorBloqueZ;
   dir = true;
   motor(dir, z_dir, z_stp, stpsFZ);
   dir = false;
   motor(dir, z_dir, z_stp, stpsFZ);
   }

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