#include "A4988.h"
#include "SyncDriver.h"
#define MOTOR_STEPS 200 //全步脉冲
#define MICROSTEPPING 16 //细分

#define SM1_DIR 5
#define SM1_STEP 2
A4988 stepper1(MOTOR_STEPS, SM1_DIR, SM1_STEP);

#define SM2_DIR 6
#define SM2_STEP 3
A4988 stepper2(MOTOR_STEPS, SM2_DIR, SM2_STEP);

#define SM3_DIR 7
#define SM3_STEP 4
A4988 stepper3(MOTOR_STEPS, SM3_DIR, SM3_STEP);

#define SM4_DIR 13
#define SM4_STEP 12
A4988 stepper4(MOTOR_STEPS, SM4_DIR, SM4_STEP);

#include <Arduino_FreeRTOS.h>
#include "task.h"

TaskHandle_t   SM1_task_handler;           //定义任务句柄（结构体指针）
TaskHandle_t   SM2_task_handler;           //定义任务句柄（结构体指针）
TaskHandle_t   SM3_task_handler;           //定义任务句柄（结构体指针）
TaskHandle_t   SM4_task_handler;           //定义任务句柄（结构体指针）

void SM1_task(void* args);
void SM2_task(void* args);
void SM3_task(void* args);
void SM4_task(void* args);

//菜单

String SMSTR[]={
  "转速", //RPM_转速          0
  "加速度", //加速度            1
  "减速度", //减速度            2
  "反转脉冲", //反转脉冲          3
  "步进1正转脉冲", //步进1 正转脉冲   4
  "步进2正转脉冲", //步进2 正转脉冲   5
  "步进3正转脉冲", //步进3 正转脉冲   6
  "步进4正转脉冲", //步进4 正转脉冲   7
};

int SMARG[]={
  100, //RPM_转速          0
  500, //加速度            1
  500, //减速度            2
  800, //反转脉冲          3
  32000, //步进1 正转脉冲   4
  32000, //步进2 正转脉冲   5
  32000, //步进3 正转脉冲   6
  32000, //步进4 正转脉冲   7
};


//EEPROM
#include <EEPROM.h>
void SettingRead();
void SettingWrite();


//独立按钮
#include <Bounce2.h>
#define Button_ST A3
Bounce2::Button stButton = Bounce2::Button();



bool RUNTAGGER1=false;
bool RUNTAGGER2=false;
bool RUNTAGGER3=false;
bool RUNTAGGER4=false;
void stButtonClicked() {//运行按钮
  if (RUNTAGGER1 || RUNTAGGER2 || RUNTAGGER3 || RUNTAGGER4){
    RUNTAGGER1=false;RUNTAGGER2=false;RUNTAGGER3=false;RUNTAGGER4=false;
    vTaskDelete(SM1_task_handler);
    vTaskDelete(SM2_task_handler);
    vTaskDelete(SM3_task_handler);
    vTaskDelete(SM4_task_handler);
    Serial.println("急停");
  }
  else{
    Serial.println("启动");
    xTaskCreate(SM1_task, "SM1_task", 80, NULL, 0, &SM1_task_handler);//运行电机任务
    xTaskCreate(SM2_task, "SM2_task", 80, NULL, 0, &SM2_task_handler);//运行电机任务
    xTaskCreate(SM3_task, "SM3_task", 80, NULL, 0, &SM3_task_handler);//运行电机任务
    xTaskCreate(SM4_task, "SM4_task", 80, NULL, 0, &SM4_task_handler);//运行电机任务
  }
}

void SettingRead(){
  uint8_t MenuCount=(sizeof(SMARG) / sizeof(SMARG[0]));
  int t_SMARG[int(8)];//手动填写菜单count
  memcpy(t_SMARG,SMARG,sizeof(SMARG));
  bool BADEEPROM=false;
  EEPROM.get(0, t_SMARG);
  for (uint8_t i=0;i<MenuCount;i++){
    if (t_SMARG[i]==-1){//如果有任意-1值
      BADEEPROM=true;
      break;
    }
  }
  if (BADEEPROM==true){
    EEPROM.update(0, SMARG);
  }
  else{
    memcpy(SMARG,t_SMARG,sizeof(t_SMARG));
  }
}
void SettingWrite(){
  EEPROM.update(0, SMARG);
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  SettingRead();
  
  stButton.attach(Button_ST, INPUT_PULLUP);

  stepper1.begin(SMARG[0],MICROSTEPPING);
  stepper1.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, SMARG[1], SMARG[2]);//设置加速度

  stepper2.begin(SMARG[0],MICROSTEPPING);
  stepper2.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, SMARG[1], SMARG[2]);//设置加速度

  stepper3.begin(SMARG[0],MICROSTEPPING);
  stepper3.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, SMARG[1], SMARG[2]);//设置加速度

  stepper4.begin(SMARG[0],MICROSTEPPING);
  stepper4.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, SMARG[1], SMARG[2]);//设置加速度
  
}

void loop() {
  // put your main code here, to run repeatedly:
   stButton.update();
   if(stButton.rose()){
    stButtonClicked();
  }
  serialSetting();//监控串口参数设置
}
void serialShowSetting(uint8_t index){
  uint8_t MenuCount=(sizeof(SMARG) / sizeof(SMARG[0]));
  if(index==255){//显示所有
    for(uint8_t i=0;i<MenuCount;i++){
      Serial.print(SMSTR[i]+":");
      Serial.println(SMARG[i]);
    }
  }
}
void serialSetting(){

}
//电机1任务
static void SM1_task(void* pvParameters) {
  while (1) {
    RUNTAGGER1=true;
    Serial.println("1运行");
    //vTaskDelay(500);
    stepper1.move(SMARG[4]);
    stepper1.move(-SMARG[3]);
    Serial.println("1停止");
    RUNTAGGER1=false;
    vTaskDelete(NULL);//执行后删除任务
  }
}

//电机2任务
static void SM2_task(void* pvParameters) {
  while (1) {
    RUNTAGGER2=true;
    Serial.println("2运行");
    //vTaskDelay(500);
    stepper2.move(SMARG[5]);
    stepper2.move(-SMARG[3]);
    Serial.println("2停止");
    RUNTAGGER2=false;
    vTaskDelete(NULL);//执行后删除任务
  }
}

//电机3任务
static void SM3_task(void* pvParameters) {
  while (1) {
    RUNTAGGER3=true;
    Serial.println("3运行");
    //vTaskDelay(500);
    stepper3.move(SMARG[6]);
    stepper3.move(-SMARG[3]);
    Serial.println("3停止");
    RUNTAGGER3=false;
    vTaskDelete(NULL);//执行后删除任务
  }
}

//电机1任务
static void SM4_task(void* pvParameters) {
  while (1) {
    RUNTAGGER4=true;
    Serial.println("4运行");
    //vTaskDelay(500);
    stepper4.move(SMARG[7]);
    stepper4.move(-SMARG[3]);
    Serial.println("4停止");
    RUNTAGGER4=false;
    vTaskDelete(NULL);//执行后删除任务
  }
}