#include <AccelStepper.h>
#include <Servo.h>

bool movingToTarget = false;
long targetStepperPos = 0;
int targetRearArmAngle = 90;
int targetFrontArmAngle = 45;

// 使用 A4988 驱动（2引脚模式）
#define STEP_PIN 8
#define DIR_PIN 9
AccelStepper myStepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Servo instances
Servo servoRearArm;
Servo servoFrontArm;
const int servoRearPin = 4;
const int servoFrontPin = 3;

// JoystickA pins
const int X_pin_A = A0;  
const int Y_pin_A = A1;

// JoystickB pins
const int X_pin_B = A8;
const int Y_pin_B = A9;

// Joystick thresholds
const int THRESHOLD_LOW = 400;
const int THRESHOLD_HIGH = 600;
const int CENTER_THRESHOLD = 50;

// Initial angles
int rearArmAngle = 135;
int frontArmAngle = 45;

// Remeber points matrix
const int MAX_POINTS = 10;
String pointNames[MAX_POINTS];//字符串类型的数组记录输入点的名字
int pointPositions[MAX_POINTS][3]; // 0: stepper pos, 1: rearArm, 2: frontArm
int pointCount = 0;

void handleSerialCommands(); // 串口处理函数声明

void setup() {
  Serial.begin(9600);

  // Servo 初始化
  servoRearArm.attach(servoRearPin);
  servoFrontArm.attach(servoFrontPin);
  servoRearArm.write(rearArmAngle);
  servoFrontArm.write(frontArmAngle);

  // 步进电机初始化
  myStepper.setMaxSpeed(1000.0);
  // myStepper.setAcceleration(500); // 如有需要可启用

  pinMode(X_pin_A, INPUT);
  pinMode(Y_pin_A, INPUT); 
  pinMode(X_pin_B, INPUT);
  pinMode(Y_pin_B, INPUT); 
}

void loop() {
  if (!movingToTarget) {
    checkJoystickA();
    checkJoystickB();
  }

  handleMoveToTarget();
  myStepper.run();
  handleSerialCommands();
}

void checkJoystickA() {
  int xValue = analogRead(X_pin_A);
  int centeredx = xValue - 512;          
  int deadzone = 50;

  int yValue = analogRead(Y_pin_A);

  if (abs(centeredx) < deadzone) {
    myStepper.setSpeed(0);               
  } else {
    float speed = (float)centeredx * 500.0 / 512.0;
    myStepper.setSpeed(speed);
    myStepper.runSpeed(); // A4988 驱动需要连续调用
    Serial.print("Position of Stepper: ");
    Serial.println(myStepper.currentPosition());
  }

  if (yValue > THRESHOLD_HIGH && rearArmAngle < 180) {
    rearArmAngle++;
    servoRearArm.write(rearArmAngle);
    Serial.print("Direction of rear arm: ");
    Serial.println(rearArmAngle);
  } else if (yValue < THRESHOLD_LOW && rearArmAngle > 0) {
    rearArmAngle--;
    servoRearArm.write(rearArmAngle);
    Serial.print("Direction of rear arm: ");
    Serial.println(rearArmAngle);
  }
}

void checkJoystickB() {
  int yValue = analogRead(Y_pin_B);

  if (yValue > THRESHOLD_HIGH && frontArmAngle < 180) {
    frontArmAngle++;
    servoFrontArm.write(frontArmAngle);
  } else if (yValue < THRESHOLD_LOW && frontArmAngle > 0) {
    frontArmAngle--;
    servoFrontArm.write(frontArmAngle);
  }

  delay(15); // 舵机响应时间
}

void handleSerialCommands() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input.startsWith("remember(") && input.endsWith(")")) {
      String label = input.substring(9, input.length() - 1);//
      remember(label);
    } else if (input.startsWith("giveme(") && input.endsWith(")")) {
      String label = input.substring(7, input.length() - 1);
      giveme(label);
    } else if (input.startsWith("moveto(") && input.endsWith(")")) {
      String label = input.substring(7, input.length() - 1);
      moveto(label);
    }
    else {
      Serial.println("Invalid command.");
    }
  }
}

void remember(String label) {
  if (pointCount < MAX_POINTS) {
    pointNames[pointCount] = label;                               //???
    pointPositions[pointCount][0] = myStepper.currentPosition();
    pointPositions[pointCount][1] = rearArmAngle;
    pointPositions[pointCount][2] = frontArmAngle;

    Serial.print("Recorded point '");
    Serial.print(label);
    Serial.println("' successfully.");

    pointCount++;
  } else {
    Serial.println("Maximum number of points reached.");
  }
}

void giveme(String label) {
  bool found = false;

  for (int i = 0; i < pointCount; i++) {
    if (pointNames[i] == label) {
      Serial.print("Point '");
      Serial.print(label);
      Serial.print("': ");
      Serial.print(pointPositions[i][0]); Serial.print(", ");
      Serial.print(pointPositions[i][1]); Serial.print(", ");
      Serial.println(pointPositions[i][2]);
      found = true;
      break;
    }
  }

  if (!found) {
    Serial.print("Point '");
    Serial.print(label);
    Serial.println("' not found.");
  }
}

void moveto(String label) {
  for (int i = 0; i < pointCount; i++) {
    if (pointNames[i] == label) {
      targetStepperPos = pointPositions[i][0];
      targetRearArmAngle = pointPositions[i][1];
      targetFrontArmAngle = pointPositions[i][2];

      myStepper.moveTo(targetStepperPos);
      movingToTarget = true;

      Serial.print("Moving to point '");
      Serial.print(label);
      Serial.println("'...");
      return;
    }
  }

  Serial.print("Point '");
  Serial.print(label);
  Serial.println("' not found.");
}

void handleMoveToTarget() {
  if (!movingToTarget) return;

  // 舵机逐步移动
  if (rearArmAngle < targetRearArmAngle) {
    rearArmAngle++;
    servoRearArm.write(rearArmAngle);
  } else if (rearArmAngle > targetRearArmAngle) {
    rearArmAngle--;
    servoRearArm.write(rearArmAngle);
  }

  if (frontArmAngle < targetFrontArmAngle) {
    frontArmAngle++;
    servoFrontArm.write(frontArmAngle);
  } else if (frontArmAngle > targetFrontArmAngle) {
    frontArmAngle--;
    servoFrontArm.write(frontArmAngle);
  }

  // 判断是否已全部到位
  if (myStepper.distanceToGo() == 0 &&
      rearArmAngle == targetRearArmAngle &&
      frontArmAngle == targetFrontArmAngle) {
    movingToTarget = false;
    Serial.println("Reached target point.");
  }

  delay(10); // 可调节平滑程度
}