#import <Arduino.h>
#define DIRECTION_CW 1
#define DIRECTION_CCW 0


class Stepper {
  private:
    short enablePin = 8;
    long currentPos = 0;
    long targetPos = 0;
    float speed = 0;
    float maxSpeed = 1000;
    float acceleration = 0;
    short stepPin = 0;
    short dirPin = 0;
    long n;
    float c0;
    float cn;
    float cmin; 
    unsigned long stepInterval;
    unsigned long lastStepTime;
    unsigned long minPulseWidth = 1;
    bool direction = DIRECTION_CW; // 1 == CW

  public:
    Stepper(short sP, short dP) {
      stepPin = sP;
      dirPin = dP;
    }
    Stepper(short sP, short dP, short en) {
      stepPin = sP;
      dirPin = dP;
      enablePin = en;
    }
    void setSpeed(float sp){
      if(sp > maxSpeed){
        speed = maxSpeed;
        return;
      }
      speed = sp;
      return;
    }
    void setMaxSpeed(float sp){
      maxSpeed = sp;
      return;
    }

    float getSpeed(){
      return speed;
    }
    float getMaxSpeed(){
      return maxSpeed;
    }
    void init(){
      pinMode(enablePin, OUTPUT);
      pinMode(stepPin, OUTPUT); 
      pinMode(dirPin, OUTPUT);
      enableOutputs();
      // digitalWrite(enablePin, LOW);
    }
    void disableOutputs(){
      digitalWrite(enablePin, HIGH);
    }
    void enableOutputs(){
      digitalWrite(enablePin, LOW);
    }
    long distanceToGo()
    {
      return targetPos - currentPos;
    }

    long targetPosition()
    {
      return targetPos;
    }

    long currentPosition()
    {
      return currentPos;
    }
    void blockingRunToTarget(){
      while(abs(distanceToGo()) > 0){
        run();
      }
    }
    void setCurrentPosition(long position)
    {
      targetPos = currentPos = position;
      n = 0;
      stepInterval = 0;
      speed = 0.0;
    }

    void stop()
    {
      targetPos = currentPos;
      n = 0;
      speed = 0.0;
    //   move(0);
    }

    bool isRunning()
    {
      return !(speed == 0.0 && targetPos == currentPos);
    }
    void moveTo(long absolute)
    {
      if (targetPos != absolute)
      {
        targetPos = absolute;
        stepInterval = fabs(1000000.0 / speed);
      }
    }

    void move(long relative)
    {
      if(0 < relative){
        direction = DIRECTION_CW;
      }else {
        direction = DIRECTION_CCW;
      }
      moveTo(currentPos + relative);
    }

    void setDirection(int dir){
      if(0 < dir){
        direction = DIRECTION_CW;
      }else {
        direction = DIRECTION_CCW;
      }
    }

    bool runSpeed()
    {
        // Dont do anything unless we actually have a step interval
      if (!speed)
        return false;
      stepInterval = fabs(1000000.0 / speed);

      unsigned long time = micros();   
      if (time - lastStepTime >= stepInterval)
      {
        if (direction == DIRECTION_CW)
        {
          // Clockwise
          currentPos += 1;
        }
        else
        {
          // Anticlockwise  
          currentPos -= 1;
        }
        step();

        lastStepTime = time; // Caution: does not account for costs in step()

        return true;
      }else
      {
        return false;
      }
    }

    bool run()
    {
      runSpeed();
      return speed != 0.0 || distanceToGo() != 0;
    }

    void step()
    {
      if(direction == DIRECTION_CW)
        digitalWrite(dirPin, HIGH);
      else 
        digitalWrite(dirPin, LOW);
      digitalWrite(stepPin,HIGH);
      delayMicroseconds(400);
      digitalWrite(stepPin,LOW);
      delayMicroseconds(400);
    }
};