const int pin = 17;
const int potPin = 34;
long delayMicros = 30000;
long delayTime = 0;

/// Current direction motor is spinning in
/// Protected because some peoples subclasses need it to be so
boolean _direction; // 1 == CW

/// The current interval between steps in microseconds.
/// 0 means the motor is currently stopped with _speed == 0
unsigned long  _stepInterval;

/// The current absolution position in steps.
long           _currentPos;    // Steps

/// The target position in steps. The AccelStepper library will move the
/// motor from the _currentPos to the _targetPos, taking into account the
/// max speed, acceleration and deceleration
long           _targetPos;     // Steps

/// The current motos speed in steps per second
/// Positive is clockwise
float          _speed;         // Steps per second

/// The target motor speed in steps per second
/// Positive is clockwise
float          _targetSpeed;         // Steps per second

/// The maximum permitted speed in steps per second. Must be > 0.
float          _maxSpeed;

/// The acceleration to use to accelerate or decelerate the motor in steps
/// per second per second. Must be > 0
float          _acceleration;
float          _sqrt_twoa; // Precomputed sqrt(2*_acceleration)

/// The last step time in microseconds
unsigned long  _lastStepTime;

/// The minimum allowed pulse width in microseconds
unsigned int   _minPulseWidth;


/// The step counter for speed calculations
long _n;

/// Initial step size in microseconds
float _c0;

/// Last step size in microseconds
float _cn;

/// Min step size in microseconds based on maxSpeed
float _cmin; // at max speed

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(pin, OUTPUT);
  setMaxSpeed(8000);

}

void loop() {
  pingPin();
  checkPot();
}

void pingPin() {
  static uint32_t last = 0;
  if (_stepInterval > 0) {
    uint32_t now = micros();
    if (now - last > _stepInterval) {
      delayTime = now - last;
      last = now;
      digitalWrite(pin, HIGH);
      digitalWrite(pin, LOW);
    }
  }
}

void checkPot() {
  static uint32_t last = 0;
  static int lastPot;
  uint32_t now = millis();
  if (now - last > 100) {
    Serial.print('.');
    last = now;
    int nowPot = analogRead(potPin);
    if (abs(nowPot - lastPot) > 10) {
      lastPot = nowPot;
      // 38us >  7894rpm, 380us 789RPM, 600us 500RPM 60000us > 5RPM
      delayMicros = map(constrain(nowPot, 200, 3500), 200, 3500, 600, 10000);
      _stepInterval = delayMicros;
      Serial.print("lastPeriodUs:"); Serial.println(delayTime);
      Serial.print("newRPM:");
      Serial.println(1.0e6 / delayMicros / 200 * 60, 5);
    }
  }
}



// Stuff copied from AccelStepper
// https://github.com/waspinator/AccelStepper/blob/master/src/AccelStepper.cpp
// to support smoothly changing the _stepInterval

void setMaxSpeed(float speed)
{
// speed in steps/sec, so: 

    if (speed < 0.0)
       speed = -speed;
    if (_maxSpeed != speed)
    {
	_maxSpeed = speed;
	_cmin = 1000000.0 / speed;
	// Recompute _n from current speed and adjust speed if accelerating or cruising
	if (_n > 0)
	{
	    _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
	    computeNewSpeed();
	}
    }
}



void setAcceleration(float acceleration)
{
  if (acceleration == 0.0)
    return;
  if (acceleration < 0.0)
    acceleration = -acceleration;
  if (_acceleration != acceleration)
  {
    // Recompute _n per Equation 17
    _n = _n * (_acceleration / acceleration);
    // New c0 per Equation 7, with correction per Equation 15
    _c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15
    _acceleration = acceleration;
    computeNewSpeed();
  }
}

long distanceToGo() {
  return _targetPos - _currentPos;
}

enum MotorMode {POSITION, SPEED} motorMode = POSITION;
typedef enum
{
  DIRECTION_CCW = 0,  ///< Counter-Clockwise
  DIRECTION_CW  = 1   ///< Clockwise
} Direction;


unsigned long computeNewSpeed()
{
  long distanceTo = distanceToGo(); // +ve is clockwise from curent location

  long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16

  if (motorMode == POSITION && distanceTo == 0 && stepsToStop <= 1)
  {
    // We are at the target and its time to stop
    _stepInterval = 0;
    _speed = 0.0;
    _n = 0;
    return _stepInterval;
  }

  if (distanceTo > 0)
  {
    // We are anticlockwise from the target
    // Need to go clockwise from here, maybe decelerate now
    if (_n > 0)
    {
      // Currently accelerating, need to decel now? Or maybe going the wrong way?
      if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
        _n = -stepsToStop; // Start deceleration
    }
    else if (_n < 0)
    {
      // Currently decelerating, need to accel again?
      if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
        _n = -_n; // Start accceleration
    }
  }
  else if (distanceTo < 0)
  {
    // We are clockwise from the target
    // Need to go anticlockwise from here, maybe decelerate
    if (_n > 0)
    {
      // Currently accelerating, need to decel now? Or maybe going the wrong way?
      if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
        _n = -stepsToStop; // Start deceleration
    }
    else if (_n < 0)
    {
      // Currently decelerating, need to accel again?
      if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
        _n = -_n; // Start accceleration
    }
  }

  // Need to accelerate or decelerate
  if (_n == 0)
  {
    // First step from stopped
    _cn = _c0;
    _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
  }
  else
  {
    // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
    _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
    _cn = max(_cn, _cmin);
  }
  _n++;
  _stepInterval = _cn;
  _speed = 1000000.0 / _cn;
  if (_direction == DIRECTION_CCW)
    _speed = -_speed;

#if 1
  Serial.println(_speed);
  Serial.println(_acceleration);
  Serial.println(_cn);
  Serial.println(_c0);
  Serial.println(_n);
  Serial.println(_stepInterval);
  Serial.println(distanceTo);
  Serial.println(stepsToStop);
  Serial.println("-----");
#endif
  return _stepInterval;
}