#include "stepper.h"
// Дефиниране на таблицата, която реализира функцията за движение до позиця с променлива скорост.
#define tableSize 200 // Дефиниране на размера на таблицата. Трябва да е кратен на 100, и да не е по-голям от 1000.
#define tableResolution 128 // Дефиниране, колко стъпки да има между два елемента от таблицата. Трябва да е степен на 2. ...64, 128, 256... и т.н.
  #if tableResolution <= 0 || ((tableResolution & (tableResolution - 1)) != 0)  //Проверка на горното условие.
    #error tableResolution must be an exact power of 2! ...64, 128, 256... etc.
  #endif
#define N1(index) (512 / tan((3.141 / 2) * ((index + 1) / (float)_tableSize))) //Функцията за елементите на таблицата. В случая е тангенс от 0 до п/2.
// Попълване на самата таблица (масива). Прави се по време на компилация и стои в ROM паметта. Благодаря на miro_atc за идеята.
#define N10(X)  N1(X+0), N1(X+1), N1(X+2), N1(X+3), N1(X+4), N1(X+5), N1(X+6), N1(X+7), N1(X+8), N1(X+9)
#define N100(X)  N10(X+0), N10(X+10), N10(X+20), N10(X+30), N10(X+40), N10(X+50), N10(X+60), N10(X+70), N10(X+80), N10(X+90)
#if tableSize == 100
  #define N1000(X) N100(X+0)
#elif tableSize == 200
  #define N1000(X) N100(X+0), N100(X+100)
#elif tableSize == 300
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200)
#elif tableSize == 400
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300)
#elif tableSize == 500
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400)
#elif tableSize == 600
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400), N100(X+500)
#elif tableSize == 700
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400), N100(X+500), N100(X+600)
#elif tableSize == 800
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400), N100(X+500), N100(X+600), N100(X+700)
#elif tableSize == 900
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400), N100(X+500), N100(X+600), N100(X+700), N100(X+800)
#elif tableSize == 1000
  #define N1000(X) N100(X+0), N100(X+100), N100(X+200), N100(X+300), N100(X+400), N100(X+500), N100(X+600), N100(X+700), N100(X+800), N100(X+900)
#else
  #error The size of the table must be between 100 and 1000, and it must be a multiple of 100!
#endif

// В следващия масив е изнесена формулата, по която се изчислява всеки следващ ипулс при ускорение и спиране.
// Заема 30% (300kB)от флаша на ESP32, но по този начин се увеличава драстично скоростта на изчисление.
// При тази реализация се налага следното ограничение:
// Максималната скорост на квадрат делено на два пъти ускорението да е по-малко от 100000.
// Скоростта е в съпки за секунда. Ускорението в съпки за секунда за зекунда.
#define SQR1(index) (sqrt(((index) +1)*1e12) - sqrt((index)*1e12)) //SQRT(_iProcess)Table function
#define SQR10(X)  SQR1(X+0), SQR1(X+1), SQR1(X+2), SQR1(X+3), SQR1(X+4), SQR1(X+5), SQR1(X+6), SQR1(X+7), SQR1(X+8), SQR1(X+9)
#define SQR100(X)  SQR10(X+0), SQR10(X+10), SQR10(X+20), SQR10(X+30), SQR10(X+40), SQR10(X+50), SQR10(X+60), SQR10(X+70), SQR10(X+80), SQR10(X+90)
#define SQR1000(X)  SQR100(X+0), SQR100(X+100), SQR100(X+200), SQR100(X+300), SQR100(X+400), SQR100(X+500), SQR100(X+600), SQR100(X+700), SQR100(X+800), SQR100(X+900)
#define SQR10000(X)  SQR1000(X+0), SQR1000(X+1000), SQR1000(X+2000), SQR1000(X+3000), SQR1000(X+4000), SQR1000(X+5000), SQR1000(X+6000), SQR1000(X+7000), SQR1000(X+8000), SQR1000(X+9000)
#define SQR100000(X)  SQR10000(X+0), SQR10000(X+10000), SQR10000(X+20000), SQR10000(X+30000), SQR10000(X+40000), SQR10000(X+50000), SQR10000(X+60000), SQR10000(X+70000), SQR10000(X+80000), SQR10000(X+90000)

static const int _tableSize = tableSize;
static const int _shiftTblSpd = 0x9; // Необходим е при изчисляване на скоростта при движение по таблица.
static const int _andValue = tableResolution - 1; // Необходим е при изчисляване на импулса при движение по таблица.
static const int _shiftValue = log2(tableResolution); // Необходим е при изчисляване на импулса при движение по таблица.
static const int _lastTableElement = _tableSize - 1;
static const int _table[_tableSize] = {N1000(0)}; // Създаване на таблицата реализираща функцията за движение с променлива скорост.
static const float _sqrTable[100000] = {SQR100000(0)}; // Създаване на таблицата реализираща формулата, по която се изчислява всеки следващ ипулс при ускорение и спиране.

// Конструктор.
// Задава пиновете за стъпка и посока.
// Инициализира някои променливи, необходими за работата на функциите.
stepper::stepper(uint stepPin, uint dirPin, float acc, float maxSpeed) {
  _stepPin = stepPin;
  _dirPin = dirPin;
  pinMode(_stepPin, OUTPUT);
  pinMode(_dirPin, OUTPUT);
  _pulseDurationMin = 1e6 / maxSpeed;
  _acc = sqrt(2 / acc);
  _pAccZero = 1e6 * _acc;
  _pSet = 1000;
  _iProcess = 0;
  _processStart = false;
  _currentPosition = 0;
  _newPosition = 0;
  _run = false;
  _modeDec = false;
}

// Това е сърцето на програмата.
// Тук се изчислява дължината на всеки текущ импулс.
inline void stepper::_calculateTime() {
  if (_pSet < _pulseDurationMin) { _pSet = _pulseDurationMin; } // проверка дали зададената скорост надвишава максимално допустимата.
  if (_pSet < _pAccZero || _iProcess) { // Проверка дали при зададената скорост е необходимо ускорение. (Например много ниска зададена скорост.)
    if (_pSet < _pAcc) {   //................................... През тази част се минава, ако сме в режим на ускорение.
      if (_pSetOld < _pAcc || _pSet > _pAcc) {_iProcess++;}
      _pAcc = _acc * _sqrTable[_iProcess];
      _p = (_pSet > _pAcc) ? _pSet : _pAcc;
    } else {               //................................... През тази част се минава, ако сме в режим на спиране.
      if ((_pSetOld > _pAcc || _pSet < _pAcc) && _iProcess) {_iProcess--;}
      _pAcc = _acc * _sqrTable[_iProcess];
      _p = (_pSet < _pAcc) ? _pSet : _pAcc;
    }
  } else {              // От тук се минава, ако при зададената скорост не е необходимо ускорение.
    _p = _pSet;
  }
  _pSetOld = _pSet;
  _currentPosition += _positionAddend;
  if (_modeStepSpdTbl != SPEED) { _stepCount--; }
  if (_modeStepSpdTbl == TABLE) { //..През тази част се минава, ако се движим до позиция с променлива скорост зададена от таблица.
    if (_currentPosition >= _lastTableElement << _shiftValue) {// Ако сме достигнали последния елемент на таблицата, скоростта остава такава каквато е зададена от него.
      _pSet = *(_table + _lastTableElement) * _pulseTblCoeff >> _shiftTblSpd;
    } else if (_currentPosition < 0) {// Ако сме достигнали първия елемент на таблицата, скоростта остава такава каквато е зададена от него.
      _pSet = *_table * _pulseTblCoeff >> _shiftTblSpd;
    } else { // От тук се минава, ако сме някъде между първия и последния елемент на таблицата.
      _tablePoint = (_currentPosition >> _shiftValue);
      _pSet = (*(_table + _tablePoint) + (((*(_table + _tablePoint + 1) - *(_table + _tablePoint)) * (_currentPosition & _andValue)) >> _shiftValue)) * _pulseTblCoeff >> _shiftTblSpd;
    }
  }
  if (_stepCount <= _iProcess) { // Проверка дали не е време да започне спиране.
    _pSet = 0x6FFFFFFF; // Ако сме в този режим задаваме максимално малка скорост, до която програмата се стреми да достигне по време на спиране.
    _modeDec = true;
  }
}

// Тук се манипулира пина на който се изкарват импулсите.
// Следи се времето и изчислената дължина на импулса от _calculateTime()
void stepper::refreshPulse() {
  _timeNow = micros();
  if (_stepCount && _processStart) { // От тук минаваме, ако току що е стартиран мотора. Вдигаме пина в единица.
    _run = true;
    digitalWrite(_stepPin, 1);
    _processStart = false;
    _highPulse = true;
    _timeBefore = _timeNow;
    _iProcess = 0;
    _pSetOld = 0x6FFFFFFF;
    _pAcc1 = _pAccZero;
    _pAcc0 = 0;
    _pAcc = _pAcc1;
  }
  if ((_timeNow - _timeBefore) >= _hiPulseWidth && _highPulse) { // От тук минаваме, ако времето да бъде в единица е изтекло.
    digitalWrite(_stepPin, 0); // Сваляме пина в нула.
    _calculateTime();
    _highPulse = false;
  }
  if ((_timeNow - _timeBefore) >= _p && !_highPulse) { // От тук минаваме, ако времето да бъде в нула е изтекло.
    if (_stepCount) {
      digitalWrite(_stepPin, 1);  // Вдигаме пина в единица.
      _highPulse = true;
      _timeBefore = _timeNow;
    } else { // От тук минаваме, ако в момента се изпълнява последния импулс преди спиране.
      _run = false;
      _iProcess = 0;
      _modeDec = false;
    }
  }
}

// Функция, с която се задава скоростта на въртене. Стъпки за секунда.
bool stepper::setSpeed(float speed) {
  if (!speed) { return false; }  // Проверка делене на нула!
  _pulseDuration = 1000000 / speed;
  if (_modeDec || _modeStepSpdTbl == TABLE) { return false; }
  _pSet = _pulseDuration;
  return true;
}

// От тук се задава относителна скорост, която се използва при движение до позиция с променлива скорост.
// Т.е. с колко да се увеличи или намали дължината на ипулсите изчислени в таблицата.
bool stepper::setTblSpeed(int tableSpeedCoeff) {
  if (!tableSpeedCoeff) { return false; }  // Dividing by zero check!
  _pulseTblCoeff = 8192 / tableSpeedCoeff;
  return true;
}

// Тук се задава текущата позиция на мотора.
bool stepper::setCurrentPosition(int currentPosition) {
  if (_run) { return false; }
  _currentPosition = currentPosition;
  return true;
}

// Провелка, дали моторът е в движение?
bool stepper::isRunning() {
  return _run;
}

// Спиране на мотора, независимо в какъв режим работи.
// Спирането е плавно с необходимото орицателно ускорение.
void stepper::stop() {
  _modeStepSpdTbl = STEP;
  _stepCount = _iProcess;
}

// Функция, реализираща работа на мотора с постоянна скорост, докато не срещне stop().
// Задава се посока на въртене и скорост.
// Скоростта не е задължително да се задава. В този случай се използва последната зададена.
bool stepper::runWithSpeed(bool dir, float speed) {
  if (_run) { return false; }
  if (speed) { _pulseDuration = 1000000 / speed; }
  digitalWrite(_dirPin, dir);
  if (dir) {
    _positionAddend = 1;
  } else {
    _positionAddend = -1;
  }
  _pSet = _pulseDuration;
  _modeStepSpdTbl = SPEED;
  _processStart = true;
  _stepCount = 0xFFFFFF;
  return true;
}

// Функция, реализираща въртене на мотора до зададена позиция с постоянна скорост.
// Може да се спре и със stop(). В този случай се запомня до коя позиция сме достигнали.
// Задава се позиция и скорост (не задължителен параметър).
bool stepper::runToPosition(int newPosition, float speed) {
  if (_run) { return false; }
  if (speed) { _pulseDuration = 1000000 / speed; }
  if (newPosition == _currentPosition) { return true; }
  _newPosition = newPosition;
  _manipulateDir();
  _pSet = _pulseDuration;
  _modeStepSpdTbl = STEP;
  _processStart = true;
  return true;
}

// Функция, реализираща въртене на мотора до зададена позиция с променлива скорост зададена от таблица.
// Може да се спре и със stop(). В този случай се запомня до коя позиция сме достигнали.
// Задава се позиция и относителна скорост (не задължителен параметър).
bool stepper::runToPositionUseTable(int newPosition, int tableSpeedCoeff) {
  if (_run) { return false; }
  if (tableSpeedCoeff) { _pulseTblCoeff = 8192 / tableSpeedCoeff; }
  if (newPosition == _currentPosition) { return true; }
  _newPosition = newPosition;
  _manipulateDir();
  _tablePoint = (_currentPosition >> _shiftValue);
  if (_tablePoint >= _lastTableElement) {
    _pSet = *(_table + _lastTableElement) * _pulseTblCoeff >> _shiftTblSpd;
  } else if (_tablePoint < 0) {
    _pSet = *_table * _pulseTblCoeff >> _shiftTblSpd;
  } else {
    _pSet = (*(_table + _tablePoint) + (((*(_table + _tablePoint + 1) - *(_table + _tablePoint)) * (_currentPosition & _andValue)) >> _shiftValue)) * _pulseTblCoeff >> _shiftTblSpd;
  }
  _modeStepSpdTbl = TABLE;
  _processStart = true;
  return true;
}

// inline функция използвана от runToPosition и runToPositionUseTable
inline void stepper::_manipulateDir () {
  if (_newPosition > _currentPosition) {
    digitalWrite(_dirPin, 1);
    _stepCount = _newPosition - _currentPosition;
    _positionAddend = 1;
  } else {
    digitalWrite(_dirPin, 0);
    _stepCount = _currentPosition - _newPosition;
    _positionAddend = -1;
  }
}








//