#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Stepper.h>
#include <Servo.h>

// Define the number of steps per revolution for your motor
#define STEPS_PER_REV 2048

// Initialize stepper motors
Stepper stepperX(STEPS_PER_REV, 2, 4, 3, 5);
Stepper stepperY(STEPS_PER_REV, 6, 8, 7, 9);

// Initialize servo motor
Servo servoZ;

// Initialize the LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Adjust the address (0x27) if needed

// Variables to store positions
int xPos = 0;
int yPos = 0;
int zPos = 0;

void setup() {
  // Set the speed for the steppers
  stepperX.setSpeed(10); // Adjust speed as needed
  stepperY.setSpeed(10); // Adjust speed as needed
  
  // Attach the servo to pin 10
  servoZ.attach(10);
  
  // Initialize the LCD with 16 columns and 2 rows
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("CNC Initialized");
  delay(2000);
  lcd.clear();

  // Initialize serial communication
  Serial.begin(9600);
  Serial.println("CNC PCB Machine Initialized. Awaiting G-code commands...");
}

void loop() {
  // Check if data is available to read
  if (Serial.available() > 0) {
    // Read the incoming string
    String gcode = Serial.readStringUntil('\n');
    gcode.trim(); // Remove any trailing whitespace
    
    // Process the G-code command
    processGcode(gcode);
  }
}

void processGcode(String gcode) {
  if (gcode.startsWith("G0") || gcode.startsWith("G1")) {
    // G0 or G1: Linear move
    int xIndex = gcode.indexOf('X');
    int yIndex = gcode.indexOf('Y');
    int zIndex = gcode.indexOf('Z');
    
    if (xIndex >= 0) {
      float xValue = gcode.substring(xIndex + 1).toFloat();
      moveX(xValue * STEPS_PER_REV); // Convert to steps
    }
    if (yIndex >= 0) {
      float yValue = gcode.substring(yIndex + 1).toFloat();
      moveY(yValue * STEPS_PER_REV); // Convert to steps
    }
    if (zIndex >= 0) {
      float zValue = gcode.substring(zIndex + 1).toFloat();
      moveZ(zValue); // Z value is in degrees
    }
  } else {
    Serial.println("Unsupported G-code command");
  }
}

void moveX(int steps) {
  stepperX.step(steps);
  xPos += steps;
  Serial.print("Moved X axis by ");
  Serial.print(steps);
  Serial.println(" steps");
  updateLCD();
}

void moveY(int steps) {
  stepperY.step(steps);
  yPos += steps;
  Serial.print("Moved Y axis by ");
  Serial.print(steps);
  Serial.println(" steps");
  updateLCD();
}

void moveZ(float angle) {
  servoZ.write(angle);
  zPos = angle;
  Serial.print("Moved Z axis to ");
  Serial.print(angle);
  Serial.println(" degrees");
  updateLCD();
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.print(xPos);
  lcd.setCursor(8, 0);
  lcd.print("Y: ");
  lcd.print(yPos);
  lcd.setCursor(0, 1);
  lcd.print("Z: ");
  lcd.print(zPos);
}