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


class Stepper {
  private:
    enum pulseState {UP = 1, NO_PULSE = 0, DOWN = -1} state;
    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
    unsigned long pulseStartTime;

    bool pulseActive = false;

    static const unsigned long PULSE_WIDTH = 1000; // microseconds

  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(){
      state = NO_PULSE;
      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 setPosition(long position){
      targetPos = currentPos = position;
    }

    void setCurrentPosition(long position){
      targetPos = currentPos = position;
      n = 0;
      stepInterval = 0;
      speed = 0.0;
    }

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

    bool isRunning(){
      return (distanceToGo() != 0);
    }
    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;
      }
    }
    void updatePulse() {
      if (pulseActive) {
        if(state == UP){
          digitalWrite(stepPin, HIGH);
        }else if(state == DOWN){
          digitalWrite(stepPin, LOW);
        }

        unsigned long currentTime = micros();
        unsigned long timDiff = currentTime - pulseStartTime;
        if (timDiff >= PULSE_WIDTH) {
          digitalWrite(stepPin, LOW);
          state = NO_PULSE;
          pulseActive = false;
        }

      }

    }
    bool runSpeed(){
        // Dont do anything unless we actually have a step interval
      updatePulse();
      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(){
      if(distanceToGo() != 0)
        runSpeed();
      // Serial.print("S: ");
      // Serial.print(speed);
      // Serial.print(", dist: ");
      // Serial.print(distanceToGo());
      // Serial.print(", bool: ");
      // Serial.println((speed != 0.0 || distanceToGo() != 0));
      return 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);
      // digitalWrite(stepPin, HIGH); 
      if(state == NO_PULSE){
        state = UP;
        pulseStartTime = micros(); 
        pulseActive = true;
        updatePulse();
      }
    }
};