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

// Configuração dos pinos do CNC Shield (eixos X e Y)
#define STEP_X 2
#define DIR_X 5
#define STEP_Y 3
#define DIR_Y 6

// Pino do servo motor (eixo Z)
#define SERVO_Z_PIN 11

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

// Estados dos botões
bool startEnabled = false;

// Motores de Passo
AccelStepper motorX(AccelStepper::DRIVER, STEP_X, DIR_X);
AccelStepper motorY(AccelStepper::DRIVER, STEP_Y, DIR_Y);

// Servo motor
Servo servoZ;

// Coordenadas absolutas
float posX = 0; // Posição atual do eixo X
float posY = 0; // Posição atual do eixo Y

// Estado do eixo Z
bool zAbaixado = false;

// Função para validar o comando
bool validarComando(String comando) {
    // Converte todo o comando para maiúsculas
    comando.toUpperCase();

    for (char c : comando) {
        if (!isalnum(c) && c != ' ' && c != '.' && c != '-') {
            return false; // Contém caracteres inválidos
        }
        if (isalpha(c) && c != 'G' && c != 'X' && c != 'Y' && c != 'Z') {
            return false; // Contém letras não permitidas
        }
    }
    return true;
}

// Função para processar comandos
void processarComando(String comando) {
    // Converte todo o comando para maiúsculas
    comando.toUpperCase();

    char cmd = '\0';  // Comando G (G0 ou G1)
    float valorX = posX, valorY = posY; // Coordenadas interpretadas
    int valorZ = zAbaixado ? 1 : 0;    // Estado do servo Z
    bool moveX = false, moveY = false, moveZ = false;

    int idxG = comando.indexOf('G');
    int idxX = comando.indexOf('X');
    int idxY = comando.indexOf('Y');
    int idxZ = comando.indexOf('Z');

    // Processa comando G
    if (idxG >= 0) {
        cmd = comando[idxG + 1];
    }

    // Processa coordenadas X
    if (idxX >= 0) {
        int endIdx = comando.indexOf(' ', idxX);
        if (endIdx == -1) endIdx = comando.length();
        valorX = comando.substring(idxX + 1, endIdx).toFloat();
        moveX = true;
    }

    // Processa coordenadas Y
    if (idxY >= 0) {
        int endIdx = comando.indexOf(' ', idxY);
        if (endIdx == -1) endIdx = comando.length();
        valorY = comando.substring(idxY + 1, endIdx).toFloat();
        moveY = true;
    }

    // Processa comando Z
    if (idxZ >= 0) {
        valorZ = comando.substring(idxZ + 1).toInt();
        moveZ = true;
    }

    // Exibe o que foi lido e entendido
    if (cmd != '\0') {
        Serial.print("G: ");
        Serial.println(cmd);
    }
    if (moveX) {
        Serial.print("X: ");
        Serial.println(valorX, 2); // Duas casas decimais
    }
    if (moveY) {
        Serial.print("Y: ");
        Serial.println(valorY, 2); // Duas casas decimais
    }
    if (moveZ) {
        Serial.print("Z: ");
        Serial.println(valorZ == 1 ? "Abaixar Caneta" : "Levantar Caneta");
    }

    // Movimento absoluto
    if (moveX) {
        motorX.moveTo(valorX * 200); // Multiplica pelo número de passos por unidade
        posX = valorX;
    }
    if (moveY) {
        motorY.moveTo(valorY * 200);
        posY = valorY;
    }
    if (moveZ) {
        if (valorZ == 1) {
            servoZ.write(90); // Abaixa a caneta
            zAbaixado = true;
        } else if (valorZ == 0) {
            servoZ.write(0); // Levanta a caneta
            zAbaixado = false;
        }
    }
}

void setup() {
    // Inicializa serial
    Serial.begin(115200);
    Serial.println("Iniciando Mini CNC com Servo Z");

    // Configura motores
    motorX.setMaxSpeed(500);
    motorX.setAcceleration(100);
    motorY.setMaxSpeed(500);
    motorY.setAcceleration(100);

    // Configura servo
    servoZ.attach(SERVO_Z_PIN);
    servoZ.write(0); // Posição inicial (caneta levantada)

    // Configura botões
    pinMode(BTN_START, INPUT);
    pinMode(BTN_STOP, INPUT);

    Serial.println("Aguardando comandos...");
}

void loop() {
    // Verifica botão de start
    if (digitalRead(BTN_START) == HIGH && !startEnabled) {
        startEnabled = true;
        Serial.println("Movimento habilitado");
    }

    // Verifica botão de stop
    if (digitalRead(BTN_STOP) == HIGH && startEnabled) {
        startEnabled = false;
        motorX.stop();
        motorY.stop();
        Serial.println("Movimento interrompido");
    }

    // Verifica comandos via serial
    if (Serial.available() > 0) {
        String comando = Serial.readStringUntil('\n');
        comando.trim(); // Remove espaços extras

        if (validarComando(comando)) {
            if (startEnabled) {
                processarComando(comando);
            } else {
                Serial.println("Ative o botão Start para iniciar o movimento");
            }
        } else {
            Serial.println("Comando inválido");
        }
    }

    // Atualiza motores
    motorX.run();
    motorY.run();
}