#include <AccelStepper.h>
#include <Servo.h>

// Definindo os pinos para controle dos motores (CNC Shield V3)
#define motorPinX1 8  // Pino STEP para o eixo X
#define motorPinX2 9  // Pino DIR para o eixo X
#define motorPinY1 7  // Pino STEP para o eixo Y
#define motorPinY2 6  // Pino DIR para o eixo Y
#define servoPin 3   // Pino para controle do servo (eixo Z)
#define on 13
#define off 10
#define led_on 12
#define led_off 11

// Inicializando os motores de passo (eixos X e Y)
AccelStepper stepperX(AccelStepper::DRIVER, motorPinX1, motorPinX2);
AccelStepper stepperY(AccelStepper::DRIVER, motorPinY1, motorPinY2);

// Posições atuais dos eixos
long posX = 0;  // Posição inicial no eixo X
long posY = 0;  // Posição inicial no eixo Y
int posZ = 90;  // Posição inicial do servo (eixo Z)
int status;
// Criação do objeto Servo
Servo meuServo;



void setup() {
  Serial.begin(9600);  // Inicializa a comunicação serial

  // Configura a velocidade e a aceleração dos motores
  stepperX.setMaxSpeed(1000);  // Velocidade máxima do motor X
  stepperY.setMaxSpeed(1000);  // Velocidade máxima do motor Y
  stepperX.setAcceleration(500);  // Aceleração do motor X
  stepperY.setAcceleration(500);  // Aceleração do motor Y

  meuServo.attach(servoPin);  // Conecta o servo ao pino 10
  meuServo.write(posZ);  // Define a posição inicial do servo
  Serial.println("Aguardando comandos...");

  pinMode(on, INPUT);
  pinMode(off, INPUT);
  pinMode(led_on,OUTPUT);
  pinMode(led_off,OUTPUT);

  
}

void loop() {
 
  if(digitalRead(on))
  {
    status = 1;
    digitalWrite(led_on,1);
    digitalWrite(led_off,0);
    
  
  }
  else if (digitalRead(off))
  {
    status = 0;
    digitalWrite(led_on,0);
    digitalWrite(led_off,1);  
    
   
  }
  
  
  
  
  if ((Serial.available() > 0) && (status == 1))  {
    // Lê o comando enviado via serial
    String command = Serial.readStringUntil('\n');
    command.trim();  // Remove espaços extras

    // Verifica se o comando é G0 ou G1 com coordenadas X, Y e Z
    if (command.startsWith("G0") || command.startsWith("G1")) {
      // Extrair os comandos G0/G1 e as coordenadas X, Y e Z
      long newPosX = extractCoordinate(command, 'X');  // Extrai X
      long newPosY = extractCoordinate(command, 'Y');  // Extrai Y
      int newPosZ = extractCoordinate(command, 'Z');   // Extrai Z

      if (command.startsWith("G0")) {
        // Ação para o G0: Movimento rápido
        moveToPosition(newPosX, newPosY, newPosZ, true);
      } 
      else if (command.startsWith("G1")) {
        // Ação para o G1: Movimento controlado (preciso)
        moveToPosition(newPosX, newPosY, newPosZ, false);
      }
    }
  }

  // Atualiza os motores para que eles movam até a posição
  stepperX.run();
  stepperY.run();
  
}

// Função para extrair coordenadas X, Y ou Z de uma string
long extractCoordinate(String command, char axis) {
  int axisIndex = command.indexOf(axis);  // Encontra a posição da letra (X, Y ou Z)
  if (axisIndex == -1)  return 0;  // Se não encontrar, retorna 0 (não há coordenada)

  // Encontra a parte do número após X, Y ou Z
  String valueString = command.substring(axisIndex + 1);
  int spaceIndex = valueString.indexOf(' ');  // Encontra o espaço (se houver) que separa os valores
  if (spaceIndex != -1) {
    valueString = valueString.substring(0, spaceIndex);  // Extrai o número
  }

  // Converte o valor para inteiro
  return valueString.toInt();
}

// Função para mover para a posição X, Y e Z
void moveToPosition(long newPosX, long newPosY, int newPosZ, bool isRapid) {
  // Ação para mover os motores para as posições X e Y
  Serial.print("Movendo para X: ");
  Serial.print(newPosX);
  Serial.print(", Y: ");
  Serial.print(newPosY);
  Serial.print(", Z: ");
  Serial.println(newPosZ);

  // Define a nova posição dos motores
  stepperX.moveTo(newPosX);
  stepperY.moveTo(newPosY);

  // Se o movimento for rápido (G0), ajusta a velocidade
  if (isRapid) {
    stepperX.setMaxSpeed(1500);  // Velocidade mais alta para G0
    stepperY.setMaxSpeed(1500);  // Velocidade mais alta para G0
  } else {
    stepperX.setMaxSpeed(500);  // Velocidade mais baixa para G1
    stepperY.setMaxSpeed(500);  // Velocidade mais baixa para G1
  }

  // Mover o servo para a posição Z
  if (newPosZ >= 90 && newPosZ <= 180) {
    meuServo.write(newPosZ);  // Mover o servo para a posição Z (0 a 180)
    delay(500);  // Aguardar o movimento do servo
  } else {
    Serial.println("Valor de Z inválido. O valor deve ser 0 ou 180");
  }

  
 
}