#include <AccelStepper.h>

// Definição dos pinos para os motores de passo
#define X_STEP_PIN 2
#define X_DIR_PIN 3
#define Y_STEP_PIN 4
#define Y_DIR_PIN 5

// Configuração da relação de passos por mm e micropassos
#define PASSOS_POR_REVOLUCAO 200   // Passos para uma volta completa
#define AVANCO_POR_REVOLUCAO 4.0   // Avanço em mm por volta
#define MICROPASSOS 16             // Divisão de passos
#define PASSOS_POR_MM (PASSOS_POR_REVOLUCAO * MICROPASSOS / AVANCO_POR_REVOLUCAO)

// Limites da área de trabalho (em mm)
#define X_MAX 400
#define Y_MAX 500

// Configuração dos motores de passo
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);

float posX_mm = 0;  // Posição X atual
float posY_mm = 0;  // Posição Y atual

void setup() {
  Serial.begin(115200);
  Serial.println("Interpretador de G-code para Impressora 2D com G1 e Micropassos");
  Serial.println("Digite um comando G-code (ex.: G1 X100 Y200 F500):");

  // Configuração de velocidade e aceleração dos motores
  stepperX.setMaxSpeed(2000);  // Configurado para passos por segundo
  stepperX.setAcceleration(1000); // Configurado para passos por segundo^2
  stepperY.setMaxSpeed(2000);
  stepperY.setAcceleration(1000);
}

void loop() {
  if (Serial.available() > 0) {
    // Lê o comando G-code do monitor serial
    String comandoGCode = Serial.readStringUntil('\n');
    comandoGCode.trim(); // Remove espaços ou quebras de linha no início e no final

    // Executa o comando G-code
    executarGCode(comandoGCode.c_str());
  }
}

void executarGCode(const char* comandoGCode) {
  // Identifica o comando inicial
  char comando = comandoGCode[0];

  if (comando == 'G') {
    switch (comandoGCode[1]) {
      case '0':
      case '1': // Movimento linear
        executarMovimentoLinear(comandoGCode);
        break;
      case '28': // Homing
        executarHoming();
        break;
      default:
        Serial.println("Comando G não suportado!");
        break;
    }
  } else if (comando == 'M') {
    switch (comandoGCode[1]) {
      case '1':
      case '14': // Relatar posição atual (M114)
        relatarPosicaoAtual();
        break;
      default:
        Serial.println("Comando M não suportado!");
        break;
    }
  } else {
    Serial.println("Comando inválido!");
  }
}

void executarMovimentoLinear(const char* comandoGCode) {
  // Obtém as posições-alvo dos eixos X e Y em milímetros
  float alvoX_mm = limitarValor(getValor(comandoGCode, 'X', posX_mm), 0, X_MAX);
  float alvoY_mm = limitarValor(getValor(comandoGCode, 'Y', posY_mm), 0, Y_MAX);
  float feedRate = getValor(comandoGCode, 'F', 1000); // Velocidade em mm/min

  // Converte as posições para passos
  long alvoX_passos = alvoX_mm * PASSOS_POR_MM;
  long alvoY_passos = alvoY_mm * PASSOS_POR_MM;

  // Define a velocidade dos motores
  float velocidade_mm_por_s = feedRate / 60.0; // Converte mm/min para mm/s
  float velocidade_passos_por_s = velocidade_mm_por_s * PASSOS_POR_MM;
  stepperX.setMaxSpeed(velocidade_passos_por_s);
  stepperY.setMaxSpeed(velocidade_passos_por_s);

  // Define as posições-alvo para os motores
  stepperX.moveTo(alvoX_passos);
  stepperY.moveTo(alvoY_passos);

  // Move os motores até as posições definidas
  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }

  // Atualiza as posições
  posX_mm = alvoX_mm;
  posY_mm = alvoY_mm;

  Serial.print("Movimento concluído para: X=");
  Serial.print(posX_mm);
  Serial.print(" mm, Y=");
  Serial.print(posY_mm);
  Serial.println(" mm");
}

// Função para executar o comando de homing (G28)
void executarHoming() {
  Serial.println("Executando homing (G28)...");

  // Move os motores para a posição de origem (0,0)
  stepperX.moveTo(0);
  stepperY.moveTo(0);

  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }

  // Atualiza as posições após o homing
  posX_mm = 0;
  posY_mm = 0;

  Serial.println("Homing concluído. Posição: X=0 Y=0");
}

// Função para relatar a posição atual (M114)
void relatarPosicaoAtual() {
  Serial.print("Posição atual: X=");
  Serial.print(posX_mm);
  Serial.print(" mm, Y=");
  Serial.print(posY_mm);
  Serial.println(" mm");
}

// Função para limitar os valores ao tamanho máximo da área de trabalho
float limitarValor(float valor, float minimo, float maximo) {
  if (valor < minimo) return minimo;
  if (valor > maximo) return maximo;
  return valor;
}

// Função para extrair valores do comando G-code
float getValor(const char* dados, char eixo, float valorAtual) {
  const char* eixoPtr = strchr(dados, eixo);

  if (eixoPtr == NULL) {
    // Retorna o valor atual se o eixo não estiver presente no comando
    return valorAtual;
  }

  // Encontra o início do valor correspondente
  int inicio = eixoPtr - dados + 1;

  // Determina o fim do valor (ou espaço, ou fim da string)
  const char* fim = strchr(dados + inicio, ' ');
  if (fim == NULL) {
    fim = dados + strlen(dados);
  }

  // Extrai o valor como string e converte para float
  char valorStr[16];
  int comprimento = fim - (dados + inicio);
  if (comprimento >= 15) comprimento = 15;
  strncpy(valorStr, dados + inicio, comprimento);
  valorStr[comprimento] = '\0';

  return atof(valorStr);
}