#include "MotorControl.h"

MotorControl motor(2, 3, 0);

void taskMotorRun(void *pvParam)
{
  while (true)
  {
    motor.run();
    vTaskDelay(1);
  }
}

void taskParseCommand(void *pvParam)
{
  while (true)
  {
    if (Serial.available() > 0)
    {
      // 读取一行指令
      String commandString = Serial.readStringUntil('\n');
      // 去除字符串首尾空格
      commandString.trim();
      // 获取指令
      char command = commandString.charAt(0);
      // 获取参数
      int value = commandString.substring(1).toInt();
      switch (command)
      {
      // 停止电机
      case 'S':
      {
        motor.emergencyStop();
        break;
      }
      // 释放电机急停
      case 'R':
      {
        motor.releaseEmergencyStop();
        break;
      }
      // 设置速度
      case 's':
      {
        motor.setSpeed(value);
        break;
      }

      case 'A':
      {
        value = constrain(value, 0, 1000 * 100);
        Serial.println("forward: " + String(value));
        motor.setTarget(value);
        break;
      }

      case 'D':
      {
        value = constrain(value, 0, 1000 * 100);
        Serial.println("backward: " + String(value));
        motor.setTarget(-value);
        break;
      }

      case 'J':
      {
        // 前进直到触发限位开关A，停止运动
        Serial.println("forward until button A");
        motor.goHome(true);
        break;
      }

      case 'K':
      {
        // 后退直到触发限位开关A，停止运动
        Serial.println("backward until button A");
        motor.goHome(false);
        break;
      }

      default:
        break;
      }
    }
    vTaskDelay(1);
  }
}

void taskPrintStatus(void *pvParam)
{
  while (true)
  {
    // 输出电机运动状态，限位开关触发状态，电机当前位置，目标位置
    Serial.println("isRunning: " + String(motor.isRunning()) + ", isLimitTriggered: " + String(motor.getLimitState()) + ", position: " + String(motor.getPosition()) + ", target: " + String(motor.getTarget()));
    vTaskDelay(500);
  }
}

void setup()
{
  Serial.begin(115200);
  Serial.println("setup");
  // 初始化电机
  motor.init();
  // 设置电机运动速度
  motor.setSpeed(255);
  // 设置限位开关触发电平
  motor.setLimitTrigger(HIGH);


  xTaskCreatePinnedToCore(taskMotorRun, "taskMotorRun", 1024 * 4, NULL, 1, NULL, 1);
  delay(100);
  xTaskCreatePinnedToCore(taskParseCommand, "taskParseCommand", 1024 * 4, NULL, 1, NULL, 1);
  delay(100);
  xTaskCreatePinnedToCore(taskPrintStatus, "taskPrintStatus", 1024 * 4, NULL, 1, NULL, 1);
}

void loop()
{
  vTaskDelete(NULL);
}