#include "Motor.h"

Motor::Motor(int dirPin, int stepPin, int HIGHLimit, int LOWLimit, float stepDistance, float speed) {
  this->dirPin = dirPin;
  this->stepPin = stepPin;
  this->HIGHLimit = HIGHLimit;
  this->LOWLimit = LOWLimit;
  this->stepDistance = stepDistance;
  this->speed = speed;
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(HIGHLimit, INPUT_PULLUP);
  pinMode(LOWLimit, INPUT_PULLUP);
  stepper = AccelStepper(AccelStepper::DRIVER, stepPin, dirPin);
  //  stepper.setMaxSpeed(speed); //馬達最大速度
  //  stepper.setSpeed(speed);    //馬達初始化速度
  //  stepper.setAcceleration(speed * speed); // 馬達加速度
  float steps = speed / stepDistance;
  int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5);  // 四舍五入
  stepper.setMaxSpeed(stepsOut); //馬達最大速度
  stepper.setSpeed(stepsOut);    //馬達初始化速度
  stepper.setAcceleration(stepsOut * stepsOut); // 馬達加速度
  //  stepper.setMinPulseWidth(minPulseWidth);
  _currentPosition = 0;
  _targetPosition = 0;
}

// 移動指定距離
void Motor::move(float distance) {
  float steps = distance / stepDistance;
  int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5);  // 四舍五入
  stepper.move(stepsOut);
  while (stepper.distanceToGo() != 0) {
    if (distance > 0)
    {
      if (digitalRead(LOWLimit) == LOW) {
        stepper.move(-5);
      }
    }
    else
    {
      if (digitalRead(HIGHLimit) == LOW)
      {
        stepper.move(10);
      }
    }
    stepper.run();
  }
  _currentPosition = stepper.currentPosition() * stepDistance;
}

// 停止馬達運動
void Motor::stop() {
  stepper.stop();
}

//獲取馬達目前位置
float Motor::getCurrentPosition() {
  return _currentPosition;
}

//重新設置馬達零點
void Motor::resetPosition() {
  _currentPosition = 0;
  _targetPosition = 0;
  stepper.setCurrentPosition(0);
}

void Motor::moveDis(float distance) {
  float steps = distance / stepDistance;
  int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5);  // 四舍五入
  stepper.move(stepsOut);
  while (stepper.distanceToGo() != 0) {
    if (digitalRead(HIGHLimit) == LOW && steps < 0) {
      stepper.move(10);
    }
    if (digitalRead(LOWLimit) == LOW && steps > 0) {
      stepper.move(-10);
    }
    stepper.run();
  }
  _currentPosition = stepper.currentPosition() * stepDistance;
}

//碰撞上極限 每次移動-0.01mm
void Motor::moveToHighLim() {
  while (digitalRead(HIGHLimit) == HIGH ) {
    float steps = 1 / stepDistance;
    int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5);  // 四舍五入
    stepper.move(-stepsOut); //-0.01mm
    while (stepper.distanceToGo() != 0) {
      stepper.run();
    }
    stepper.stop();
  }
  _currentPosition = stepper.currentPosition() * stepDistance;
}
void Motor::motorMove(float distance) {
  int steps = distance / stepDistance;
  stepper.move(steps);
  while (stepper.distanceToGo() != 0) {
    stepper.run();
  }
  _currentPosition = stepper.currentPosition() * stepDistance;
}
void Motor::changeSpeed(int speed) {
  float steps = speed / stepDistance;
  int stepsOut = static_cast<int>(steps > 0 ? steps + 0.5 : steps - 0.5);  // 四舍五入
  stepper.setMaxSpeed(stepsOut); //馬達最大速度
  stepper.setSpeed(stepsOut);    //馬達初始化速度
  stepper.setAcceleration(stepsOut * stepsOut); // 馬達加速度
}