#include <Stepper.h>
#include <math.h>

#define STEPS_PER_REVOLUTION 200
#define STEP_RESOLUTION 1.0  // Each step corresponds to 0.01 units
#define MAX_SPEED 60  // Maximum speed setting for stepper motors

// Define the command codes for different G-codes
#define RAPID_POSITIONING 0  // G0
#define LINEAR_INTERPOLATION 1  // G1
#define CW_ARC_INTERPOLATION 2  // G2
#define CCW_ARC_INTERPOLATION 3  // G3

Stepper X_Axis(STEPS_PER_REVOLUTION, 2, 3, 4, 5);
Stepper Y_Axis(STEPS_PER_REVOLUTION, 6, 7, 8, 9);

// Current position of the stepper motors
float currentX = 0.0, currentY = 0.0;

void setup() {
  Serial.begin(9600);
  X_Axis.setSpeed(MAX_SPEED);
  Y_Axis.setSpeed(MAX_SPEED);
  Serial.println("Enter G-code commands:");
}

void loop() {
  static String inputString = "";  // Stores the incoming data
  static boolean stringComplete = false;  // Indicates if the string is complete

  while (Serial.available()) {
    char inChar = (char)Serial.read();
    if (inChar == '\n') {
      stringComplete = true;
    } else {
      inputString += inChar;
    }
  }

  if (stringComplete) {
    parseGCode(inputString);
    inputString = "";
    stringComplete = false;
  }
}

void parseGCode(String command) {
  char buf[command.length() + 1];
  command.toCharArray(buf, sizeof(buf));
  char* token = strtok(buf, " ");

  int gCode = -1;
  float xTarget = currentX, yTarget = currentY;
  float iCenter = 0, jCenter = 0;  // Centers for arcs
  float feedrate = MAX_SPEED;

  while (token != NULL) {
    switch (token[0]) {
      case 'G':
        gCode = atoi(&token[1]);
        break;
      case 'X':
        xTarget = atof(&token[1]);
        break;
      case 'Y':
        yTarget = atof(&token[1]);
        break;
      case 'I':
        iCenter = atof(&token[1]);
        break;
      case 'J':
        jCenter = atof(&token[1]);
        break;
      case 'F':
        feedrate = atof(&token[1]);
        adjustSpeed(feedrate);
        break;
    }
    token = strtok(NULL, " ");
  }

  executeMove(gCode, xTarget, yTarget, iCenter, jCenter);
}

void executeMove(int gCode, float x, float y, float i, float j) {
  Serial.print("Executing G");
  Serial.print(gCode);
  Serial.print(": Moving to (");
  Serial.print(x);
  Serial.print(", ");
  Serial.print(y);
  Serial.println(")");

  switch (gCode) {
    case RAPID_POSITIONING:
    case LINEAR_INTERPOLATION:
      linearMove(x, y);
      break;
    case CW_ARC_INTERPOLATION:
      arcMove(x, y, i, j, true);
      break;
    case CCW_ARC_INTERPOLATION:
      arcMove(x, y, i, j, false);
      break;
  }
}

void linearMove(float x, float y) {
  int xSteps = (x - currentX) / STEP_RESOLUTION;
  int ySteps = (y - currentY) / STEP_RESOLUTION;

  int xDir = xSteps > 0 ? 1 : -1;
  int yDir = ySteps > 0 ? 1 : -1;

  xSteps = abs(xSteps);
  ySteps = abs(ySteps);

  int maxSteps = max(xSteps, ySteps);

  for (int step = 0; step < maxSteps; step++) {
    if (step < xSteps) {
      X_Axis.step(xDir);
      currentX += xDir * STEP_RESOLUTION;
    }
    if (step < ySteps) {
      Y_Axis.step(yDir);
      currentY += yDir * STEP_RESOLUTION;
    }
    Serial.print(currentX);
    Serial.print(",");
    Serial.println(currentY);
  }
}

void arcMove(float x, float y, float i, float j, bool clockwise) {
  float radius = sqrt(pow(x - (currentX + i), 2) + pow(y - (currentY + j), 2));
  float startAngle = atan2(currentY - j, currentX - i);
  float endAngle = atan2(y - j, x - i);

  if (clockwise && startAngle <= endAngle) {
    startAngle += 2 * PI;
  } else if (!clockwise && startAngle >= endAngle) {
    startAngle -= 2 * PI;
  }

  float angleStep = STEP_RESOLUTION / radius;  // Smaller step for more precision in arc
  if (clockwise) {
    angleStep = -angleStep;
  }

  for (float angle = startAngle; (clockwise ? angle > endAngle : angle < endAngle); angle += angleStep) {
    float newX = i + radius * cos(angle);
    float newY = j + radius * sin(angle);
    linearMove(newX, newY);
  }
}

void adjustSpeed(float feedrate) {
  int speed = map(feedrate, 0, 100, 0, MAX_SPEED);  // Map the feedrate to stepper speed range
  X_Axis.setSpeed(speed);
  Y_Axis.setSpeed(speed);
}
uno:A5.2
uno:A4.2
uno:AREF
uno:GND.1
uno:13
uno:12
uno:11
uno:10
uno:9
uno:8
uno:7
uno:6
uno:5
uno:4
uno:3
uno:2
uno:1
uno:0
uno:IOREF
uno:RESET
uno:3.3V
uno:5V
uno:GND.2
uno:GND.3
uno:VIN
uno:A0
uno:A1
uno:A2
uno:A3
uno:A4
uno:A5
stepper1:A-
stepper1:A+
stepper1:B+
stepper1:B-
stepper2:A-
stepper2:A+
stepper2:B+
stepper2:B-