#include <AccelStepper.h>

// Define stepper motor connections and steps per revolution
#define X_STEP_PIN 2
#define X_DIR_PIN 3
#define Y_STEP_PIN 4
#define Y_DIR_PIN 5
#define Z_STEP_PIN 6
#define Z_DIR_PIN 7

AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);

// Define G-code commands
const char* gcodeCommands[] = {
  "G0 X0 Y0 Z0",
  "G0 X10 Y10 Z10",
  "G1 X20 Y20 Z20",
  "G2 X30 Y30 I40 K50",
  "G3 X40 Y40 I-40 K-50",
  // Add more G-code commands as needed
};

int currentCommandIndex = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("G-code Interpreter");

  // Set the maximum speed and acceleration for each stepper motor
  stepperX.setMaxSpeed(1000);
  stepperX.setAcceleration(500);
  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(500);
  stepperZ.setMaxSpeed(1000);
  stepperZ.setAcceleration(500);
}

void loop() {
  // Move the motors using G-code commands
  executeGCode(gcodeCommands[currentCommandIndex]);

  // Increment the command index
  currentCommandIndex++;

  // If all commands are executed, reset to the first command
  if (currentCommandIndex >= sizeof(gcodeCommands) / sizeof(gcodeCommands[0])) {
    currentCommandIndex = 0;
  }

  // Add a delay to slow down the execution (adjust as needed)
  delay(2000);
}

void executeGCode(const char* gcodeCommand) {
  // Parse G-code command and extract relevant values
  char command = gcodeCommand[0];

  if (command == 'G') {
    switch (gcodeCommand[1]) {
      case '0':
      case '1':
        executeLinearMove(gcodeCommand);
        break;
      case '2':
        executeArcClockwise(gcodeCommand);
        break;
      case '3':
        executeArcCounterclockwise(gcodeCommand);
        break;
      // Add more cases for other G-code commands if needed
    }
  }
}

void executeLinearMove(const char* gcodeCommand) {
  // Parse G1 command and extract X, Y, and Z values
  float targetX = getValue(gcodeCommand, 'X', stepperX.currentPosition());
  float targetY = getValue(gcodeCommand, 'Y', stepperY.currentPosition());
  float targetZ = getValue(gcodeCommand, 'Z', stepperZ.currentPosition());

  // Move the motors to the target positions
  stepperX.moveTo(targetX);
  stepperY.moveTo(targetY);
  stepperZ.moveTo(targetZ);

  // Wait for the motors to reach the target positions
  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
    stepperZ.run();
  }
}

void executeArcClockwise(const char* gcodeCommand) {
  // Parse G2 command and extract X, Y, I, and J values
  float targetX = getValue(gcodeCommand, 'X', stepperX.currentPosition());
  float targetY = getValue(gcodeCommand, 'Y', stepperY.currentPosition());
  float centerX = getValue(gcodeCommand, 'I', stepperX.currentPosition());
  float centerY = getValue(gcodeCommand, 'J', stepperY.currentPosition());

  // Calculate the radius and angles
  float radius = hypot(centerX - targetX, centerY - targetY);
  float startAngle = atan2(stepperY.currentPosition() - centerY, stepperX.currentPosition() - centerX);
  float endAngle = atan2(targetY - centerY, targetX - centerX);

  // Move along the arc
  for (float angle = startAngle; angle > endAngle; angle -= 0.01) {
    float x = centerX + radius * cos(angle);
    float y = centerY + radius * sin(angle);

    stepperX.moveTo(x);
    stepperY.moveTo(y);

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

void executeArcCounterclockwise(const char* gcodeCommand) {
  // Parse G3 command and extract X, Y, I, and J values
  float targetX = getValue(gcodeCommand, 'X', stepperX.currentPosition());
  float targetY = getValue(gcodeCommand, 'Y', stepperY.currentPosition());
  float centerX = getValue(gcodeCommand, 'I', stepperX.currentPosition());
  float centerY = getValue(gcodeCommand, 'J', stepperY.currentPosition());

  // Calculate the radius and angles
  float radius = hypot(centerX - targetX, centerY - targetY);
  float startAngle = atan2(stepperY.currentPosition() - centerY, stepperX.currentPosition() - centerX);
  float endAngle = atan2(targetY - centerY, targetX - centerX);

  // Move along the arc
  for (float angle = startAngle; angle < endAngle; angle += 0.01) {
    float x = centerX + radius * cos(angle);
    float y = centerY + radius * sin(angle);

    stepperX.moveTo(x);
    stepperY.moveTo(y);

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

float getValue(const char* data, char axis, float currentValue) {
  const char* axisPtr = strchr(data, axis);

  if (axisPtr == NULL) {
    // Axis not found in the command, return the current value
    return currentValue;
  }

  // Find the position of the axis character
  int start = axisPtr - data + 1;

  // Find the position of the next space character
  const char* end = strchr(data + start, ' ');

  if (end == NULL) {
    // If there is no space, read until the end of the string
    end = data + strlen(data);
  }

  // Ensure the size of the buffer is sufficient to hold the substring
  char valueStr[16];
  int length = end - start;
  if (length >= 15) {
    length = 15;
  }
  strncpy(valueStr, data + start, length);
  valueStr[length] = '\0';

  // Convert the string to a floating-point number
  return atof(valueStr);
}