#include <ESP32Servo.h>  // Use ESP32-specific servo library
#include <Preferences.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED display settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Preferences object
Preferences preferences;

// Constants for robot geometry (in mm)
float LEFT_LINKAGE_1 = 50.0;  // Left shoulder to elbow
float LEFT_LINKAGE_2 = 50.0;  // Left elbow to end effector
float RIGHT_LINKAGE_1 = 50.0; // Right shoulder to elbow
float RIGHT_LINKAGE_2 = 50.0; // Right elbow to end effector
const float SHOULDER_DISTANCE = 30.0;

// Paper size
float PAPER_WIDTH = 76.2;
float PAPER_HEIGHT = 76.2;

// Pen offset variables
float penOffsetX = 0.0;
float penOffsetY = 0.0;

// Servo pin definitions (aligned with Nano ESP32 official pinout)
#define LEFT_SHOULDER_PIN  17  // D8 (GPIO 17)
#define RIGHT_SHOULDER_PIN 10  // D7 (GPIO 10)
#define PEN_SERVO_PIN      18  // D9 (GPIO 18)

// Joystick pin definitions (aligned with Nano ESP32 official pinout)
#define JOY_X_PIN  1   // A0 (GPIO 1, ADC1_CH0)
#define JOY_Y_PIN  2   // A1 (GPIO 2, ADC1_CH1)
#define JOY_SW_PIN 3   // A2 (GPIO 3)

// Encoder pin definitions for KY-040 (aligned with Nano ESP32 official pinout)
#define ENC_A_PIN  4   // A3 (GPIO 4) - Connected to KY-040 CLK
#define ENC_B_PIN  11  // A4 (GPIO 11) - Connected to KY-040 DT
#define ENC_SW_PIN 12  // A5 (GPIO 12) - Connected to KY-040 SW

// Toggle switch pin definition (aligned with Nano ESP32 official pinout)
#define TOGGLE_PIN 7   // A6 (GPIO 7)

// OLED I2C pins (aligned with Nano ESP32 official pinout)
// SDA: GPIO 21 (Nano ESP32 pin D10), SCL: GPIO 38 (Nano ESP32 pin D11)
#define OLED_SDA_PIN 21  // D10
#define OLED_SCL_PIN 38  // D11

// Servo objects using ESP32Servo
Servo leftShoulder;
Servo rightShoulder;
Servo penServo;

// Current position variables
float currentX = 0.0;
float currentY = 0.0;
bool isPenDown = false;  // Renamed from penDown to avoid conflict

float moveSpeed = 0.5;
unsigned long lastMoveTime = 0;
const unsigned long MOVE_DELAY = 20;

String currentFunction = "Initializing";
bool inMenu = false;
int menuIndex = 0;
int lastEncValue = 0;
bool lastEncSWState = HIGH;
const String menuItems[] = {"Programs", "Record", "Settings", "Calibration", "System Info"};
const int menuSize = 5;

// Function prototypes
void setup();
void loop();
void updateDisplay();
void handleMenu();
void updateMenuDisplay();
void selectMenuItem();
void programsMenu();
void calibrationMenu();
void setLinkageLengths();
void setPaperSpace();
void setPenOffset();  // Prototype for setPenOffset
void drawSquare();
void drawCircle();
void wobble1();
void displaySystemInfo();
void inverseKinematics(float x, float y, float& leftAngle, float& rightAngle);
void moveTo(float x, float y);
void penUp();
void penDown();
void processGCode();
void joystickControl();

void setup() {
  Serial.begin(115200);
  
  Wire.begin(OLED_SDA_PIN, OLED_SCL_PIN);
  
  preferences.begin("calibration", false);
  PAPER_WIDTH = preferences.getFloat("paper_width", 76.2);
  PAPER_HEIGHT = preferences.getFloat("paper_height", 76.2);
  penOffsetX = preferences.getFloat("offset_x", 0.0);
  penOffsetY = preferences.getFloat("offset_y", 0.0);
  preferences.end();
  
  preferences.begin("geometry", false);
  LEFT_LINKAGE_1 = preferences.getFloat("left_l1", 50.0);
  LEFT_LINKAGE_2 = preferences.getFloat("left_l2", 50.0);
  RIGHT_LINKAGE_1 = preferences.getFloat("right_l1", 50.0);
  RIGHT_LINKAGE_2 = preferences.getFloat("right_l2", 50.0);
  preferences.end();
  
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();
  
  leftShoulder.attach(LEFT_SHOULDER_PIN);
  rightShoulder.attach(RIGHT_SHOULDER_PIN);
  penServo.attach(PEN_SERVO_PIN);
  
  pinMode(JOY_SW_PIN, INPUT_PULLUP);
  pinMode(ENC_A_PIN, INPUT_PULLUP);  // KY-040 CLK
  pinMode(ENC_B_PIN, INPUT_PULLUP);  // KY-040 DT
  pinMode(ENC_SW_PIN, INPUT_PULLUP); // KY-040 SW
  pinMode(TOGGLE_PIN, INPUT_PULLUP);
  
  randomSeed(analogRead(0));
  
  moveTo(PAPER_WIDTH/2, PAPER_HEIGHT/2);
  penUp();
  
  currentFunction = "Joystick";
  updateDisplay();
}

void loop() {
  inMenu = (digitalRead(TOGGLE_PIN) == HIGH);
  
  if (inMenu) {
    handleMenu();
  } else {
    if (Serial.available() > 0) {
      String command = Serial.readStringUntil('\n');
      command.trim();
      if (command == "SET_PAPER") {
        currentFunction = "Set Paper";
        updateDisplay();
        setPaperSpace();
        currentFunction = "Joystick";
        updateDisplay();
      } else {
        currentFunction = "G-code";
        updateDisplay();
        processGCode();
        currentFunction = "Joystick";
        updateDisplay();
      }
    } else {
      currentFunction = "Joystick";
      joystickControl();
    }
  }
}

void updateDisplay() {
  if (!inMenu) {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Mode: ");
    display.println(currentFunction);
    display.print("X: ");
    display.print(currentX, 1);
    display.print(" mm");
    display.setCursor(0, 20);
    display.print("Y: ");
    display.print(currentY, 1);
    display.println(" mm");
    display.display();
  }
}

void handleMenu() {
  int encA = digitalRead(ENC_A_PIN);  // KY-040 CLK
  if (encA != lastEncValue) {
    if (digitalRead(ENC_B_PIN) != encA) {  // KY-040 DT
      menuIndex = (menuIndex + 1) % menuSize;  // Clockwise
    } else {
      menuIndex = (menuIndex - 1 + menuSize) % menuSize;  // Counterclockwise
    }
    lastEncValue = encA;
    updateMenuDisplay();
  }
  
  bool encSW = digitalRead(ENC_SW_PIN);  // KY-040 SW
  if (encSW == LOW && lastEncSWState == HIGH) {
    selectMenuItem();
    delay(200);  // Debounce
  }
  lastEncSWState = encSW;
}

void updateMenuDisplay() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Menu:");
  for (int i = 0; i < menuSize; i++) {
    display.setCursor(10, 10 + i * 10);
    if (i == menuIndex) {
      display.print("> ");
    } else {
      display.print("  ");
    }
    display.println(menuItems[i]);
  }
  display.display();
}

void selectMenuItem() {
  switch (menuIndex) {
    case 0:  // Programs
      programsMenu();
      break;
    case 1:  // Record
      Serial.println("Record selected (not implemented)");
      break;
    case 2:  // Settings
      Serial.println("Settings selected (not implemented)");
      break;
    case 3:  // Calibration
      calibrationMenu();
      break;
    case 4:  // System Info
      displaySystemInfo();
      break;
  }
}

void programsMenu() {
  const String progItems[] = {"Draw Square", "Draw Circle", "Wobble 1"};
  const int progSize = 3;
  int progIndex = 0;
  
  while (digitalRead(TOGGLE_PIN) == HIGH) {
    int encA = digitalRead(ENC_A_PIN);
    if (encA != lastEncValue) {
      if (digitalRead(ENC_B_PIN) != encA) {
        progIndex = (progIndex + 1) % progSize;
      } else {
        progIndex = (progIndex - 1 + progSize) % progSize;
      }
      lastEncValue = encA;
      
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("Programs:");
      for (int i = 0; i < progSize; i++) {
        display.setCursor(10, 10 + i * 10);
        if (i == progIndex) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(progItems[i]);
      }
      display.display();
    }
    
    bool encSW = digitalRead(ENC_SW_PIN);
    if (encSW == LOW && lastEncSWState == HIGH) {
      inMenu = false;
      if (progIndex == 0) {
        drawSquare();
      } else if (progIndex == 1) {
        drawCircle();
      } else if (progIndex == 2) {
        wobble1();
      }
      inMenu = true;
      delay(200);
    }
    lastEncSWState = encSW;
  }
  updateMenuDisplay();
}

void calibrationMenu() {
  const String calibItems[] = {"Set Paper", "Pen Offset", "Set Linkages"};
  const int calibSize = 3;
  int calibIndex = 0;
  
  while (digitalRead(TOGGLE_PIN) == HIGH) {
    int encA = digitalRead(ENC_A_PIN);
    if (encA != lastEncValue) {
      if (digitalRead(ENC_B_PIN) != encA) {
        calibIndex = (calibIndex + 1) % calibSize;
      } else {
        calibIndex = (calibIndex - 1 + calibSize) % calibSize;
      }
      lastEncValue = encA;
      
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("Calibration:");
      for (int i = 0; i < calibSize; i++) {
        display.setCursor(10, 10 + i * 10);
        if (i == calibIndex) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(calibItems[i]);
      }
      display.display();
    }
    
    bool encSW = digitalRead(ENC_SW_PIN);
    if (encSW == LOW && lastEncSWState == HIGH) {
      inMenu = false;
      if (calibIndex == 0) {
        currentFunction = "Set Paper";
        updateDisplay();
        setPaperSpace();
      } else if (calibIndex == 1) {
        setPenOffset();
      } else if (calibIndex == 2) {
        setLinkageLengths();
      }
      inMenu = true;
      delay(200);
    }
    lastEncSWState = encSW;
  }
  updateMenuDisplay();
}

void setLinkageLengths() {
  float linkages[] = {LEFT_LINKAGE_1, LEFT_LINKAGE_2, RIGHT_LINKAGE_1, RIGHT_LINKAGE_2};
  const String linkageNames[] = {"Left L1", "Left L2", "Right L1", "Right L2"};
  int linkageIndex = 0;
  
  currentFunction = "Set Linkages";
  updateDisplay();
  
  while (digitalRead(TOGGLE_PIN) == HIGH) {
    int encA = digitalRead(ENC_A_PIN);
    if (encA != lastEncValue) {
      if (digitalRead(ENC_B_PIN) != encA) {
        linkageIndex = (linkageIndex + 1) % 4;
      } else {
        linkageIndex = (linkageIndex - 1 + 4) % 4;
      }
      lastEncValue = encA;
    }
    
    unsigned long currentTime = millis();
    if (currentTime - lastMoveTime >= MOVE_DELAY) {
      int joyY = analogRead(JOY_Y_PIN);
      float yMove = map(joyY, 0, 4095, -100, 100) / 100.0;
      if (abs(yMove) < 0.2) yMove = 0;
      
      linkages[linkageIndex] += yMove * 0.1;
      linkages[linkageIndex] = constrain(linkages[linkageIndex], 10, 100);
      
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("Set Linkages:");
      for (int i = 0; i < 4; i++) {
        display.setCursor(10, 10 + i * 10);
        if (i == linkageIndex) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.print(linkageNames[i]);
        display.print(": ");
        display.print(linkages[i], 1);
        display.println(" mm");
      }
      display.display();
      
      int joySW = digitalRead(JOY_SW_PIN);
      if (joySW == LOW) {
        LEFT_LINKAGE_1 = linkages[0];
        LEFT_LINKAGE_2 = linkages[1];
        RIGHT_LINKAGE_1 = linkages[2];
        RIGHT_LINKAGE_2 = linkages[3];
        
        preferences.begin("geometry", false);
        preferences.putFloat("left_l1", LEFT_LINKAGE_1);
        preferences.putFloat("left_l2", LEFT_LINKAGE_2);
        preferences.putFloat("right_l1", RIGHT_LINKAGE_1);
        preferences.putFloat("right_l2", RIGHT_LINKAGE_2);
        preferences.end();
        
        Serial.println("Linkage lengths saved:");
        Serial.print("Left L1: "); Serial.println(LEFT_LINKAGE_1);
        Serial.print("Left L2: "); Serial.println(LEFT_LINKAGE_2);
        Serial.print("Right L1: "); Serial.println(RIGHT_LINKAGE_1);
        Serial.print("Right L2: "); Serial.println(RIGHT_LINKAGE_2);
        break;
      }
      
      lastMoveTime = currentTime;
    }
  }
  currentFunction = "Joystick";
  updateDisplay();
}

void setPaperSpace() {
  float corners[4][2];
  int cornerIndex = 0;
  
  Serial.println("Entering paper space calibration mode...");
  Serial.println("Use joystick to move to bottom-left corner, then press button.");
  
  penUp();
  
  while (cornerIndex < 4 && digitalRead(TOGGLE_PIN) == LOW) {
    unsigned long currentTime = millis();
    
    if (currentTime - lastMoveTime >= MOVE_DELAY) {
      int joyX = analogRead(JOY_X_PIN);
      int joyY = analogRead(JOY_Y_PIN);
      int joySW = digitalRead(JOY_SW_PIN);
      
      float xMove = map(joyX, 0, 4095, -100, 100) / 100.0;
      float yMove = map(joyY, 0, 4095, -100, 100) / 100.0;
      
      if (abs(xMove) < 0.2) xMove = 0;
      if (abs(yMove) < 0.2) yMove = 0;
      
      float newX = currentX + (xMove * moveSpeed);
      float newY = currentY + (yMove * moveSpeed);
      
      newX = constrain(newX, -20, 100);
      newY = constrain(newY, 0, 100);
      
      if (newX != currentX || newY != currentY) {
        moveTo(newX, newY);
      }
      
      if (joySW == LOW) {
        corners[cornerIndex][0] = currentX;
        corners[cornerIndex][1] = currentY;
        Serial.print("Corner ");
        Serial.print(cornerIndex + 1);
        Serial.print(" set at: X=");
        Serial.print(currentX);
        Serial.print(", Y=");
        Serial.println(currentY);
        
        cornerIndex++;
        
        if (cornerIndex < 4) {
          switch (cornerIndex) {
            case 1: Serial.println("Move to bottom-right corner, then press button."); break;
            case 2: Serial.println("Move to top-right corner, then press button."); break;
            case 3: Serial.println("Move to top-left corner, then press button."); break;
          }
        }
        delay(500);
      }
      
      lastMoveTime = currentTime;
    }
  }
  
  if (cornerIndex == 4) {
    float minX = min(min(corners[0][0], corners[1][0]), min(corners[2][0], corners[3][0]));
    float maxX = max(max(corners[0][0], corners[1][0]), max(corners[2][0], corners[3][0]));
    float minY = min(min(corners[0][1], corners[1][1]), min(corners[2][1], corners[3][1]));
    float maxY = max(max(corners[0][1], corners[1][1]), max(corners[2][1], corners[3][1]));
    
    PAPER_WIDTH = maxX - minX;
    PAPER_HEIGHT = maxY - minY;
    
    preferences.begin("calibration", false);
    preferences.putFloat("paper_width", PAPER_WIDTH);
    preferences.putFloat("paper_height", PAPER_HEIGHT);
    preferences.end();
    
    currentX -= minX;
    currentY -= minY;
    moveTo(currentX, currentY);
    
    Serial.print("New paper space set: ");
    Serial.print(PAPER_WIDTH);
    Serial.print(" x ");
    Serial.println(PAPER_HEIGHT);
  }
}

void setPenOffset() {
  penUp();
  currentFunction = "Pen Offset";
  updateDisplay();
  
  Serial.println("Adjust pen offset with joystick. Press button to save.");
  
  while (digitalRead(TOGGLE_PIN) == HIGH) {
    unsigned long currentTime = millis();
    if (currentTime - lastMoveTime >= MOVE_DELAY) {
      int joyX = analogRead(JOY_X_PIN);
      int joyY = analogRead(JOY_Y_PIN);
      int joySW = digitalRead(JOY_SW_PIN);
      
      float xMove = map(joyX, 0, 4095, -100, 100) / 100.0;
      float yMove = map(joyY, 0, 4095, -100, 100) / 100.0;
      
      if (abs(xMove) < 0.2) xMove = 0;
      if (abs(yMove) < 0.2) yMove = 0;
      
      penOffsetX += xMove * moveSpeed;
      penOffsetY += yMove * moveSpeed;
      
      penOffsetX = constrain(penOffsetX, -20, 20);
      penOffsetY = constrain(penOffsetY, -20, 20);
      
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("Pen Offset:");
      display.print("X: ");
      display.print(penOffsetX, 1);
      display.println(" mm");
      display.print("Y: ");
      display.print(penOffsetY, 1);
      display.println(" mm");
      display.display();
      
      if (joySW == LOW) {
        preferences.begin("calibration", false);
        preferences.putFloat("offset_x", penOffsetX);
        preferences.putFloat("offset_y", penOffsetY);
        preferences.end();
        Serial.print("Pen offset saved: X=");
        Serial.print(penOffsetX);
        Serial.print(", Y=");
        Serial.println(penOffsetY);
        break;
      }
      
      lastMoveTime = currentTime;
    }
  }
  currentFunction = "Joystick";
  updateDisplay();
}

void drawSquare() {
  currentFunction = "Drawing Square";
  updateDisplay();
  
  float size = 30.0;
  float centerX = PAPER_WIDTH / 2;
  float centerY = PAPER_HEIGHT / 2;
  float halfSize = size / 2;
  
  if (centerX - halfSize < 0 || centerX + halfSize > PAPER_WIDTH ||
      centerY - halfSize < 0 || centerY + halfSize > PAPER_HEIGHT) {
    Serial.println("Error: Square exceeds paper bounds");
    return;
  }
  
  penUp();
  moveTo(centerX - halfSize, centerY - halfSize);
  penDown();
  moveTo(centerX + halfSize, centerY - halfSize);
  moveTo(centerX + halfSize, centerY + halfSize);
  moveTo(centerX - halfSize, centerY + halfSize);
  moveTo(centerX - halfSize, centerY - halfSize);
  penUp();
  
  moveTo(centerX, centerY);
  currentFunction = "Joystick";
  updateDisplay();
}

void drawCircle() {
  currentFunction = "Drawing Circle";
  updateDisplay();
  
  float radius = 15.0;
  float centerX = PAPER_WIDTH / 2;
  float centerY = PAPER_HEIGHT / 2;
  const int segments = 16;
  
  if (centerX - radius < 0 || centerX + radius > PAPER_WIDTH ||
      centerY - radius < 0 || centerY + radius > PAPER_HEIGHT) {
    Serial.println("Error: Circle exceeds paper bounds");
    return;
  }
  
  penUp();
  moveTo(centerX + radius, centerY);
  penDown();
  
  for (int i = 0; i <= segments; i++) {
    float angle = 2.0 * PI * i / segments;
    float x = centerX + radius * cos(angle);
    float y = centerY + radius * sin(angle);
    moveTo(x, y);
  }
  
  penUp();
  moveTo(centerX, centerY);
  currentFunction = "Joystick";
  updateDisplay();
}

void wobble1() {
  currentFunction = "Wobble 1";
  updateDisplay();
  
  float ellipseWidth = 64.0;
  float ellipseHeight = 44.0;
  float centerX = PAPER_WIDTH / 2;
  float centerY = PAPER_HEIGHT / 2;
  float halfWidth = ellipseWidth / 2;
  float halfHeight = ellipseHeight / 2;
  
  if (centerX - halfWidth < 0 || centerX + halfWidth > PAPER_WIDTH ||
      centerY - halfHeight < 0 || centerY + halfHeight > PAPER_HEIGHT) {
    Serial.println("Error: Ellipse exceeds paper bounds");
    return;
  }
  
  penUp();
  
  while (digitalRead(TOGGLE_PIN) == LOW) {
    float angle = random(0, 360) * PI / 180.0;
    float r = random(0, 100) / 100.0;
    float startX = centerX + (r * halfWidth * cos(angle));
    float startY = centerY + (r * halfHeight * sin(angle));
    
    float swipeLength = random(9000, 59001) / 1000.0;
    bool leftToRight = random(0, 2) == 0;
    
    moveTo(startX, startY);
    penDown();
    delay(100);
    
    float endX = leftToRight ? startX + swipeLength : startX - swipeLength;
    float endY = startY;
    float midX = (startX + endX) / 2;
    float midY = startY - 5.0;
    
    moveTo(midX, midY);
    moveTo(endX, endY);
    
    penUp();
    
    long delaySeconds = random(33, 334);
    for (long i = 0; i < delaySeconds * 1000 / MOVE_DELAY && digitalRead(TOGGLE_PIN) == LOW; i++) {
      delay(MOVE_DELAY);
      joystickControl();
    }
  }
  
  moveTo(centerX, centerY);
  currentFunction = "Joystick";
  updateDisplay();
}

void displaySystemInfo() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("System Info:");
  display.print("Paper: ");
  display.print(PAPER_WIDTH, 1);
  display.print("x");
  display.println(PAPER_HEIGHT, 1);
  display.print("Pos: (");
  display.print(currentX, 1);
  display.print(",");
  display.print(currentY, 1);
  display.println(")");
  display.print("Offset: (");
  display.print(penOffsetX, 1);
  display.print(",");
  display.print(penOffsetY, 1);
  display.println(")");
  display.print("L1: ");
  display.print(LEFT_LINKAGE_1, 1);
  display.print(" L2: ");
  display.println(LEFT_LINKAGE_2, 1);
  display.print("R1: ");
  display.print(RIGHT_LINKAGE_1, 1);
  display.print(" R2: ");
  display.println(RIGHT_LINKAGE_2, 1);
  display.println("Firmware: v1.0");
  display.display();
  delay(2000);
  updateMenuDisplay();
}

void inverseKinematics(float x, float y, float& leftAngle, float& rightAngle) {
  float adjustedX = x - penOffsetX;
  float adjustedY = y - penOffsetY;
  
  float xLeft = adjustedX - SHOULDER_DISTANCE/2;
  float xRight = adjustedX + SHOULDER_DISTANCE/2;
  
  float dLeft = sqrt(xLeft * xLeft + adjustedY * adjustedY);
  float dRight = sqrt(xRight * xRight + adjustedY * adjustedY);
  
  float cosLeft1 = (LEFT_LINKAGE_1 * LEFT_LINKAGE_1 + dLeft * dLeft - 
                    LEFT_LINKAGE_2 * LEFT_LINKAGE_2) / (2 * LEFT_LINKAGE_1 * dLeft);
  float cosRight1 = (RIGHT_LINKAGE_1 * RIGHT_LINKAGE_1 + dRight * dRight - 
                     RIGHT_LINKAGE_2 * RIGHT_LINKAGE_2) / (2 * RIGHT_LINKAGE_1 * dRight);
  
  float baseLeft = atan2(adjustedY, xLeft);
  float baseRight = atan2(adjustedY, -xRight);
  
  leftAngle = (baseLeft + acos(cosLeft1)) * 180.0 / PI;
  leftAngle = 90 - leftAngle;
  
  rightAngle = (baseRight + acos(cosRight1)) * 180.0 / PI;
  rightAngle = 90 + rightAngle;
}

void moveTo(float x, float y) {
  if (x < 0 || x > PAPER_WIDTH || y < 0 || y > PAPER_HEIGHT) {
    Serial.println("Error: Position out of bounds");
    return;
  }
  
  float leftAngle, rightAngle;
  inverseKinematics(x, y, leftAngle, rightAngle);
  
  leftShoulder.write(constrain(leftAngle, 0, 180));
  rightShoulder.write(constrain(rightAngle, 0, 180));
  
  currentX = x;
  currentY = y;
  
  updateDisplay();
  delay(50);
}

void penUp() {
  penServo.write(90);
  isPenDown = false;
  delay(100);
}

void penDown() {
  penServo.write(0);
  isPenDown = true;
  delay(100);
}

void processGCode() {
  String line = Serial.readStringUntil('\n');
  line.trim();
  
  if (line.startsWith("G0") || line.startsWith("G1")) {
    float newX = currentX;
    float newY = currentY;
    
    int xIndex = line.indexOf('X');
    int yIndex = line.indexOf('Y');
    
    if (xIndex != -1) {
      newX = line.substring(xIndex + 1).toFloat();
    }
    if (yIndex != -1) {
      newY = line.substring(yIndex + 1).toFloat();
    }
    
    if (line.startsWith("G0")) {
      penUp();
    } else {
      penDown();
    }
    
    moveTo(newX, newY);
    Serial.println("OK");
  } else if (line == "M3") {
    penDown();
    Serial.println("OK");
  } else if (line == "M5") {
    penUp();
    Serial.println("OK");
  } else if (line == "G28") {
    moveTo(PAPER_WIDTH/2, PAPER_HEIGHT/2);
    penUp();
    Serial.println("OK");
  }
}

void joystickControl() {
  unsigned long currentTime = millis();
  
  if (currentTime - lastMoveTime >= MOVE_DELAY) {
    int joyX = analogRead(JOY_X_PIN);
    int joyY = analogRead(JOY_Y_PIN);
    int joySW = digitalRead(JOY_SW_PIN);
    
    float xMove = map(joyX, 0, 4095, -100, 100) / 100.0;
    float yMove = map(joyY, 0, 4095, -100, 100) / 100.0;
    
    if (abs(xMove) < 0.2) xMove = 0;
    if (abs(yMove) < 0.2) yMove = 0;
    
    float newX = currentX + (xMove * moveSpeed);
    float newY = currentY + (yMove * moveSpeed);
    
    newX = constrain(newX, 0, PAPER_WIDTH);
    newY = constrain(newY, 0, PAPER_HEIGHT);
    
    if (newX != currentX || newY != currentY) {
      moveTo(newX, newY);
    }
    
    if (joySW == LOW) {
      if (isPenDown) {
        penUp();
      } else {
        penDown();
      }
      delay(200);
    }
    
    lastMoveTime = currentTime;
  }
}