#include <Stepper.h>
#include <Servo.h>

// Definindo o número de passos por revolução (ajuste conforme seu motor de passo)
#define STEPS_PER_REVOLUTION 200

// Pinos para os motores de passo
const int stepPinX = 2;
const int dirPinX = 4;
const int stepPinY = 3;
const int dirPinY = 5;

// Pino do servo motor
const int servoPin = 9;

Servo myservo;

// Definindo o número de passos para mover os motores de passo
long positionX = 0;
long positionY = 0;
int servoPosition = 90; // Posição inicial da caneta

void setup() {
  // Inicializando os pinos dos motores de passo
  pinMode(stepPinX, OUTPUT);
  pinMode(dirPinX, OUTPUT);
  pinMode(stepPinY, OUTPUT);
  pinMode(dirPinY, OUTPUT);

  // Inicializando o servo motor
  myservo.attach(servoPin);
  myservo.write(servoPosition); // Define a posição inicial da caneta

  Serial.begin(9600); // Para receber o G-code via Serial Monitor
  Serial.println("Pronto para receber comandos G-code!");
}

void loop() {
  if (Serial.available()) {
    char command = Serial.read();
    if (command == 'G') {
      // Comando G0 ou G1
      long xMove = 0;
      long yMove = 0;
      char axis;
      int steps;

      // Recebe o comando com as coordenadas
      while (Serial.available()) {
        axis = Serial.read();
        if (axis == 'X' || axis == 'Y') {
          while (Serial.available() && isDigit(Serial.peek())) {
            if (axis == 'X') {
              xMove = Serial.parseInt();
            } else if (axis == 'Y') {
              yMove = Serial.parseInt();
            }
          }
        }
      }

      // Move o motor X
      if (xMove != positionX) {
        steps = abs(xMove - positionX);
        digitalWrite(dirPinX, xMove > positionX ? HIGH : LOW);
        for (int i = 0; i < steps; i++) {
          digitalWrite(stepPinX, HIGH);
          delayMicroseconds(500); // Ajuste a velocidade
          digitalWrite(stepPinX, LOW);
          delayMicroseconds(500); // Ajuste a velocidade
        }
        positionX = xMove;
      }

      // Move o motor Y
      if (yMove != positionY) {
        steps = abs(yMove - positionY);
        digitalWrite(dirPinY, yMove > positionY ? HIGH : LOW);
        for (int i = 0; i < steps; i++) {
          digitalWrite(stepPinY, HIGH);
          delayMicroseconds(500); // Ajuste a velocidade
          digitalWrite(stepPinY, LOW);
          delayMicroseconds(500); // Ajuste a velocidade
        }
        positionY = yMove;
      }

      // Controle do servo motor para levantar/baixar a caneta
      if (positionX == 0 && positionY == 0) { // Exemplo: Posiciona a caneta
        myservo.write(0); // Baixa a caneta
      } else {
        myservo.write(90); // Levanta a caneta
      }
    }
  }
}