#include <Arduino.h>
#include <AccelStepper.h>

// 定义参数数组
#define ARRAY_SIZE 20
double receivedData[ARRAY_SIZE];

bool stopMotors = true; // 用于标记是否停止电机转动

// 定义步进电机每圈步数
const int lap = 6400;

// 定义电机步进控制引脚
const int stepPin1 = 18; // 1 左前轮
const int dirPin1 = 19;
//const int enablePin1 = 21;
const int stepPin2 = 13; // 2 右前轮
const int dirPin2 = 12;
//const int enablePin2 = 14;

// 定义电机转速和转动步数
// float stepper_speed1 = 0; // setMaxSpeed函数参数类型为float
// float stepper_speed2 = 0;
// long stepper_steps1 = 0; // move函数参数类型为long
// long stepper_steps2 = 0;

AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, dirPin1);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2);

void runCmd(double receivedData[ARRAY_SIZE]){
  for (int i = 0; i < ARRAY_SIZE; i+=5) {
    
    // 每个轮子转动的步数
    long stepper_steps1 = receivedData[i];
    long stepper_steps2 = receivedData[i+1];
    float stepper_speed1 = receivedData[i+2];
    float stepper_speed2 = receivedData[i+3];
    int yanchi = receivedData[i+4];

    Serial.print("stepper_steps1: ");
    Serial.println(stepper_steps1);
    Serial.print("stepper_steps2: ");
    Serial.println(stepper_steps2);
    Serial.print("stepper_speed1: ");
    Serial.println(stepper_speed1);
    Serial.print("stepper_speed2: ");
    Serial.println(stepper_speed2);

    stepper1.setMaxSpeed(stepper_speed1);
    stepper2.setMaxSpeed(stepper_speed2);

    stepper1.move(stepper_steps1);
    stepper2.move(stepper_steps2);

    while (stopMotors && stepper1.isRunning() || stepper2.isRunning()) {
      stepper1.run();
      stepper2.run();
    }

    delay(yanchi * 1000);
  }
}

void xTask1(void *xTask1){
  Serial.begin(115200);
  delay(10);
  Serial.println("xTask1: hello,world");

  while(1){
    if (Serial.available() > 0) {
      String input = Serial.readString(); // 读取串口数据

      if (input.startsWith("stop")) {
        stopMotors = false;
        Serial.println(stopMotors);
      }
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(10);
  Serial.println("hello,world");

  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(1000.0);
  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(1000.0);

  xTaskCreate(xTask1,"Task1",1024,NULL,2,NULL);
}

void loop() {
  if (Serial.available() > 0) {
    String input = Serial.readString(); // 读取串口数据

    // if (input.startsWith("stop")) { // 电机正在运转时，while循环阻塞程序，stop命令不被接受
    //   stopMotors = false;
    //   Serial.println(stopMotors);
    // } 
    // else {
    // 将数据拆分成单个数字，存储到数组中
    char* pch;
    int i = 0;
    pch = strtok(const_cast<char*>(input.c_str()), " ");
    while (pch != NULL && i < ARRAY_SIZE) {
      // receivedData[i] = atof(pch); // 精度差，最后计算出的电机速度和步数差别很大
      receivedData[i] = strtod(pch, NULL); // 转换为double型，但串口输出上与float没差；输入的线速度方向 PI值精度对计算步数影响很大
      pch = strtok(NULL, " ");
      i++;
    }

    // 输出接收到的数据
    Serial.println("Received data:");
    for (int i = 0; i < ARRAY_SIZE; i++) {
      Serial.print(receivedData[i]);
      Serial.print(" ");
    }
    Serial.println();

    // 执行程序
    runCmd(receivedData);
    // }
  }
}