// PU100,100;PD200,200;PU300,300; demo code

#include <AccelStepper.h>

// Lépések per milliméter (testre szabható)
#define STEPS_PER_MM 80

// Motor csatlakozások
AccelStepper stepperX(AccelStepper::DRIVER, 2, 5); // X tengely léptetőmotor: (stepPin, dirPin)
AccelStepper stepperY(AccelStepper::DRIVER, 3, 6); // Y tengely léptetőmotor: (stepPin, dirPin)

// Jelenlegi pozíciók
float currentX = 0;
float currentY = 0;

void setup() {
  Serial.begin(9600);
  stepperX.setMaxSpeed(1000);
  stepperX.setAcceleration(500);
  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(500);
  Serial.println("Hello HPGL Plotter V1.0");
}

void loop() {
  if (Serial.available()) {
    String command = Serial.readStringUntil(';');
    command.trim();
    parseCommand(command);
  }

  stepperX.run();
  stepperY.run();
}

// HPGL parancs feldolgozó
void parseCommand(String command) {
  if (command.startsWith("PU")) {
    // "Pen Up" - mozgás toll felvétele nélkül
    command.remove(0, 2);
    moveTo(command);
  }
  else if (command.startsWith("PD")) {
    // "Pen Down" - mozgás tollal rajzolva
    command.remove(0, 2);
    lineTo(command);
  }
}

// "Move To" (PU) parancs feldolgozása
void moveTo(String coordinates) {
  int xIndex = coordinates.indexOf(',');
  if (xIndex == -1) return;

  float x = coordinates.substring(0, xIndex).toFloat();
  float y = coordinates.substring(xIndex + 1).toFloat();

  // Koordináták lépésekké konvertálása
  long targetX = x * STEPS_PER_MM;
  long targetY = y * STEPS_PER_MM;

  stepperX.moveTo(targetX);
  stepperY.moveTo(targetY);
  Serial.print("Moveto X:");
  Serial.print(targetX);
  Serial.print(" Y:");
  Serial.println(targetY);
  currentX = x;
  currentY = y;
}

// "Line To" (PD) parancs feldolgozása
void lineTo(String coordinates) {
  int xIndex = coordinates.indexOf(',');
  if (xIndex == -1) return;

  float x = coordinates.substring(0, xIndex).toFloat();
  float y = coordinates.substring(xIndex + 1).toFloat();

  // Koordináták lépésekké konvertálása
  long targetX = x * STEPS_PER_MM;
  long targetY = y * STEPS_PER_MM;

  stepperX.moveTo(targetX);
  stepperY.moveTo(targetY);
   Serial.print("LineTo X:");
  Serial.print(targetX);
  Serial.print(" Y:");
  Serial.println(targetY);
  currentX = x;
  currentY = y;
}