#include <Stepper.h>

// Variáveis para armazenar os valores de G, X, Y e Z
float Vg = 0.0;
float Vx = 0.0;
float Vy = 0.0;
float Vz = 0.0;
float X = 0.0;

// Motor de Passo 1
#define passo 2
int i;
#define sentido 15

// Motor de Passo 2
#define passo2 21
int o;
#define sentido2 5

// Solenoide
#define Sol 12
int s;

// Variável para controlar o movimento do motor com base em Vx
int movimentox = 0;
int movimentoy = 0;

// Velocidade padrão (ajustar conforme necessário)
int velocidadeRapida = 1;  // Velocidade rápida (delay menor)
int velocidadeLenta = 10;  // Velocidade lenta (delay maior)

void setup() 
{
  Serial.begin(115200);  // Inicia a comunicação serial
  
  // Pinos do Motor de Passo
  pinMode(passo, OUTPUT);
  pinMode(sentido, OUTPUT);
  pinMode(passo2, OUTPUT);
  pinMode(sentido2, OUTPUT);
  pinMode(Sol, OUTPUT);

  // Iniciar Programa
  Serial.println("Sistema pronto para receber comandos.");
}

void loop() {
  // Verifica se há dados disponíveis na porta serial
  if (Serial.available() > 0) {
    String comando = Serial.readStringUntil('\n');  // Lê a linha inteira até o caractere de nova linha
    
    // Variáveis para armazenar os comandos
    int posG, posX, posY, posZ;
    String valorG, valorX, valorY, valorZ;
    
    // Procurar pelo comando G e extrair o valor
    posG = comando.indexOf("G");
    if (posG != -1) {
      valorG = comando.substring(posG + 1, comando.indexOf(" ", posG)); // Extrai o número após G
      Vg = valorG.toFloat(); // Converte para float
    }

    // Ajusta a velocidade com base no comando G (G0 para rápido, G1 para lento)
    if (Vg == 0) {
      velocidadeRapida = 1; // Velocidade rápida
    } else if (Vg == 1) {
      velocidadeRapida = 10; // Velocidade lenta
    }

    // Procurar pelo comando X e extrair o valor
    posX = comando.indexOf("X");
    if (posX != -1) {
      valorX = comando.substring(posX + 1, comando.indexOf(" ", posX)); // Extrai o número após X
      Vx = valorX.toFloat(); // Converte para float
      movimentox = Vx;  // Atribui o valor de Vx à variável de movimento
      movimentox = movimentox * 400;
    }

    // Procurar pelo comando Y e extrair o valor
    posY = comando.indexOf("Y");
    if (posY != -1) {
      valorY = comando.substring(posY + 1); // Extrai o número após Y (não há mais espaço)
      Vy = valorY.toFloat(); // Converte para float
      movimentoy = Vy;  // Atribui o valor de Vx à variável de movimento
      movimentoy = movimentoy * 400;
    }
    
    // Procurar pelo comando Z e extrair o valor
    posZ = comando.indexOf("Z");
    if (posZ != -1) {
      valorZ = comando.substring(posZ + 1); // Extrai o número após Z (não há mais espaço)
      Vz = valorZ.toFloat(); // Converte para float
    }

    // Exibe os valores armazenados
    Serial.print("Vg: ");
    Serial.println(Vg, 2);  // Exibe com 2 casas decimais
    Serial.print("Vx: ");
    Serial.println(Vx, 2);  // Exibe com 2 casas decimais
    Serial.print("Vy: ");
    Serial.println(Vy, 2);  // Exibe com 2 casas decimais
    Serial.print("Vz: ");
    Serial.println(Vz, 2);  // Exibe com 2 casas decimais    

    // Controla a Solenoide com base no valor de Z
    if (Vz == 1) {
      digitalWrite(Sol, HIGH);  // Ativa a solenoide
    } else if (Vz == 0) {
      digitalWrite(Sol, LOW);   // Desativa a solenoide
    }

    // Movimento do Motor de Passo 1 (X)
    if (movimentox < 0) { // Verifica o primeiro caractere da string
      digitalWrite(sentido, LOW);  // Liga o pino 1
      movimentox = movimentox * -1;    
    }
    else {
      digitalWrite(sentido, HIGH);   // Desliga o pino 16
    }

    for (i = 0; i < movimentox; i++) {
      digitalWrite(passo, HIGH);
      delay(velocidadeRapida);  // Ajusta a velocidade com base em G0 ou G1
      digitalWrite(passo, LOW);
      delay(velocidadeRapida);  // Ajusta a velocidade com base em G0 ou G1
    }

    // Movimento do Motor de Passo 2 (Y)
    if (movimentoy < 0) { // Verifica o primeiro caractere da string
      digitalWrite(sentido2, LOW);  // Liga o pino 1
      movimentoy = movimentoy * -1;    
    }
    else {
      digitalWrite(sentido2, HIGH);   // Desliga o pino 16
    }

    for (o = 0; o < movimentoy; o++) {
      digitalWrite(passo2, HIGH);
      delay(velocidadeRapida);  // Ajusta a velocidade com base em G0 ou G1
      digitalWrite(passo2, LOW);
      delay(velocidadeRapida);  // Ajusta a velocidade com base em G0 ou G1
    }
  }
}