#include <AccelStepper.h>
#include <MultiStepper.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);
MultiStepper steppers; // Gerencia múltiplos motores
Servo servoZ;          // Controla o servo motor

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

// Configuração de velocidade e aceleração
const float VELOCIDADE_MAXIMA = 1100.0; // Aumenta a velocidade máxima para 1500 passos/s
const float ACELERACAO = 90.0;         // Reduz a aceleração para 50 passos/s²

// Função para mover motores para coordenadas sincronizadas
void moverMotores(float novoX, float novoY) {
    long positions[2]; // Array para posições dos motores

    // Converte coordenadas em passos
    positions[0] = novoX * PASSOS_POR_MM; // X: 25 passos por mm
    positions[1] = novoY * PASSOS_POR_MM; // Y: 25 passos por mm

    // Move os motores sincronizados
    steppers.moveTo(positions);
    steppers.runSpeedToPosition(); // Aguarda até que ambos terminem o movimento

    // Atualiza as coordenadas atuais
    posX = novoX;
    posY = novoY;

    // Exibe a posição no Monitor Serial
    Serial.print("Movendo para X: ");
    Serial.print(posX);
    Serial.print(", Y: ");
    Serial.println(posY);
}

// 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

    // Verifica se há um comando para X
    int idxX = comando.indexOf('X');
    if (idxX >= 0) {
        novoX = comando.substring(idxX + 1).toFloat();
    }

    // Verifica se há um comando para Y
    int idxY = comando.indexOf('Y');
    if (idxY >= 0) {
        novoY = comando.substring(idxY + 1).toFloat();
    }

    // Verifica se há um comando para Z
    int idxZ = comando.indexOf('Z');
    if (idxZ >= 0) {
        novoZ = comando.substring(idxZ + 1).toInt();
    }

    // Movimenta os motores de passo, se necessário
    if (startEnabled && (idxX >= 0 || idxY >= 0)) {
        moverMotores(novoX, novoY);
    }

    // Movimenta o servo, se necessário
    if (idxZ >= 0) {
        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_MAXIMA); // Define a velocidade máxima ajustada
    motorY.setMaxSpeed(VELOCIDADE_MAXIMA);
    motorX.setAcceleration(ACELERACAO);   // Define a aceleração reduzida
    motorY.setAcceleration(ACELERACAO);
    steppers.addStepper(motorX);          // Adiciona os motores ao MultiStepper
    steppers.addStepper(motorY);

    // 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);
    }
}