#ifndef _STEPPER_H
#define _STEPPER_H

#include <Arduino.h>
#include <Tone.h>
//#include <TimerOne.h>


class Stepper {
  public:
    virtual void begin(int dir_pin, int step_pin, int enable_pin);
    virtual void writeSpeed(uint16_t freq, bool dir) = 0;
    virtual void loop();
    virtual void stop() = 0;
    void enable();
    void disable();
    void setSpeed(int16_t speed);
    virtual void setSpeedDirect(int16_t speed);
    void setMaxSpeed(uint16_t max_speed);
    void setAcceleration(uint16_t accel);
    bool isAccelerating();
  protected:
    int dir_pin;
    int step_pin;
    int enable_pin;
    uint16_t maxSpeed = 4000;
    uint16_t acceleration = 2000;
    int16_t targetSpeed = 0;
    int16_t currentSpeed = 0;
    unsigned long lastAccelMillis = 0;
};

class TimerStepper : public Stepper {
  public:
    void begin(int dir_pin, int step_pin, int enable_pin) override;
    void writeSpeed(uint16_t freq, bool dir) override;
    void setSpeedDirect(int16_t speed) override;
    void stop() override;
    void loop() override;
  private:
    Tone tone;
};


void Stepper::begin(int dir_pin, int step_pin, int enable_pin) {
  this->dir_pin = dir_pin;
  this->step_pin = step_pin;
  this->enable_pin = enable_pin;

  pinMode(dir_pin, OUTPUT);
  pinMode(step_pin, OUTPUT);
  pinMode(enable_pin, OUTPUT);

  disable();
}

void Stepper::enable() {
  digitalWrite(enable_pin, LOW);
}

void Stepper::disable() {
  stop();
  digitalWrite(enable_pin, HIGH);
}

void Stepper::setSpeed(int16_t speed) {
  if ((speed > 0) && (speed > maxSpeed)) speed = maxSpeed;
  else if ((speed < 0) && (-speed > maxSpeed)) speed = -maxSpeed;
  else targetSpeed = speed;
}

void Stepper::setMaxSpeed(uint16_t max_speed) {
  maxSpeed = max_speed;
}

void Stepper::setAcceleration(uint16_t accel) {
  acceleration = accel;
}

bool Stepper::isAccelerating() {
  if (currentSpeed != targetSpeed) return true;
  else return false;
}

void TimerStepper::begin(int dir_pin, int step_pin, int enable_pin) {
  Stepper::begin(dir_pin, step_pin, enable_pin);
  tone.begin(step_pin);
}

void TimerStepper::writeSpeed(uint16_t freq, bool dir) {
  enable();
  digitalWrite(dir_pin, dir ? HIGH : LOW);
  if (freq > 0) tone.play(freq);
  else tone.stop();
}

void TimerStepper::setSpeedDirect(int16_t speed) {
  uint16_t freq = abs(speed);
  bool dir = (speed < 0);

  writeSpeed(freq, dir);
}

void TimerStepper::stop() {
  targetSpeed = 0;
  currentSpeed = 0;
  tone.stop();
}

void TimerStepper::loop() {
  int16_t speedDiff = targetSpeed - currentSpeed;
  int16_t increment = acceleration / 1000;

  if (speedDiff == 0) return;
  else if ( (speedDiff > 0) && ( speedDiff > increment)) currentSpeed += increment;
  else if ( (speedDiff < 0) && (-speedDiff > increment)) currentSpeed -= increment;
  else currentSpeed = targetSpeed;

/*
  int16_t accelDelta = (millis()-lastAccelMillis)*acceleration/1000;
  lastAccelMillis = millis();
  if (accelDelta > ACCEL_CAP_PER_STEP) accelDelta = ACCEL_CAP_PER_STEP;

  if (speedDiff > accelDelta) speedDiff = accelDelta;
  else if (speedDiff < -accelDelta) speedDiff = -accelDelta;
  currentSpeed += speedDiff;
*/
  uint16_t freq = abs(currentSpeed);
  bool dir = (currentSpeed < 0);

  writeSpeed(freq, dir);

}




#endif /* _STEPPER_H */