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

// Pinos para motores e servo
#define STEP_X 2
#define DIR_X 5
#define STEP_Y 3
#define DIR_Y 6
#define SERVO_Z_PIN 11

// Botões
#define BTN_START 10
#define BTN_STOP 9

// Estados globais
bool startEnabled = false;          // Movimento habilitado ou não
float posX = 0, posY = 0;           // Coordenadas atuais dos motores
bool zAbaixado = false;             // Estado do servo (caneta levantada ou abaixada)

// Objetos de controle
AccelStepper motorX(AccelStepper::DRIVER, STEP_X, DIR_X);
AccelStepper motorY(AccelStepper::DRIVER, STEP_Y, DIR_Y);
Servo servoZ; // Controla o servo motor

// Fator de conversão (25 passos por mm para 8 mm/revolução com 16x microstepping)
const float PASSOS_POR_MM = 25.0;

// Configuração de velocidade e aceleração
const float VELOCIDADE_G0 = 1100.0; // Velocidade máxima para G0 (incrementada em 10%)
const float ACELERACAO = 50.0;     // Aceleração reduzida para suavidade

// Função para mover motores para coordenadas sincronizadas
void moverMotores(float novoX, float novoY) {
    long passosX = novoX * PASSOS_POR_MM; // Passos desejados para X
    long passosY = novoY * PASSOS_POR_MM; // Passos desejados para Y

    motorX.moveTo(passosX); // Define a posição final de X
    motorY.moveTo(passosY); // Define a posição final de Y

}

// Função para processar comandos
void processarComando(String comando) {
    float novoX = posX, novoY = posY; // Coordenadas padrão são as atuais
    int novoZ = zAbaixado ? 1 : 0;    // Estado padrão do Z

    // Extrai valores de X, Y e Z do comando
    if (comando.indexOf('X') >= 0) novoX = comando.substring(comando.indexOf('X') + 1).toFloat();
    if (comando.indexOf('Y') >= 0) novoY = comando.substring(comando.indexOf('Y') + 1).toFloat();
    if (comando.indexOf('Z') >= 0) novoZ = comando.substring(comando.indexOf('Z') + 1).toInt();

    // Move motores de passo
    if (startEnabled) {
       // Permite o movimento
        moverMotores(novoX, novoY);
    }

    // Movimenta o servo, se necessário
    if (novoZ == 1 && !zAbaixado) {
        servoZ.write(90); // Abaixa a caneta
        zAbaixado = true;
        Serial.println("Z: Caneta abaixada");
    } else if (novoZ == 0 && zAbaixado) {
        servoZ.write(0); // Levanta a caneta
        zAbaixado = false;
        Serial.println("Z: Caneta levantada");
    }
}

void setup() {
    Serial.begin(115200);
    Serial.println("Iniciando Mini CNC");

    // Configura os motores
    motorX.setMaxSpeed(VELOCIDADE_G0); // Define a velocidade máxima
    motorY.setMaxSpeed(VELOCIDADE_G0);
    motorX.setAcceleration(ACELERACAO); // Define a aceleração
    motorY.setAcceleration(ACELERACAO);

    // Configura o servo motor
    servoZ.attach(SERVO_Z_PIN);
    servoZ.write(0); // Inicializa com a caneta levantada

    // Configura os botões
    pinMode(BTN_START, INPUT_PULLUP); // Botão de início
    pinMode(BTN_STOP, INPUT_PULLUP);  // Botão de parada
}

void loop() {
    // Verifica botão de início (Start)
    if (digitalRead(BTN_START) == LOW && !startEnabled) {
        startEnabled = true;
        Serial.println("Movimento habilitado");
    }

    // Verifica botão de parada (Stop)
    if (digitalRead(BTN_STOP) == LOW && startEnabled) {
        startEnabled = false;
        motorX.stop(); // Para o motor X imediatamente
        motorY.stop(); // Para o motor Y imediatamente
        Serial.println("Movimento interrompido");
    }

    // Processa comandos da Serial
    if (Serial.available() > 0) {
        String comando = Serial.readStringUntil('\n'); // Lê o comando
        comando.trim(); // Remove espaços extras
        processarComando(comando);
    }
}