// Control a stepper motor with serial commands and move in an orbital fashion. A lot of this code is recycled from my automated camera rig, so it may not all be relevant.
// Designed for an Arduino Mega 2560. I'm writing digital signals directly to PORTC with direct port manipulation (digital pins 30-34). If you want to use another board, you'll need to look for the appropriate register to write to.

// Defines the four register states that we'll loop through to send steps to the driver.
#define POS_A B0110
#define POS_B B0101
#define POS_C B1001
#define POS_D B1010

#define DRIVE_LIMIT 3 // The limit switch pin which causes the motor to halt any motion and register its home.
#define DRIVE_ENABLE 29 // The pin which tells the stepper driver to switch on or off the current supply to motor, which stops it overheating when spending a long time at idle.

// Defines if the drive enable pin should go low or high to shut off the motor.
#define ENABLED LOW 
#define DISABLED HIGH

#define CLOCKWISE -1
#define ANTICLOCKWISE 1

// A bit like gcode, provides key codes for the types of message the Arduino will be send to enable message filtering on the other end.
#define STATUS "S"
#define ERROR "E"
#define POSITION "P"

// My basic stepper motor class with direct port manipulation.

// The motor is operated by setting a target. The motor is polled at high frequency for an available move with available(). 
// If the current position doesn't match the target position and you've waited a sufficient interval since taking the last step, then a step is available.

class Stepper{
  
  public:

    Stepper (long extent, int minInterval, int maxInterval, byte enablePin, bool reversed, bool disableOnIdle){

        _extent = extent; // Extent defines the total travel of the system in steps.

        // Interval defines the microsecond delay between pulses, i.e. the speed at which the stepper motor rotates.
        
        _minInterval = minInterval; // Min interval, speed = 1
        _maxInterval = maxInterval; // Max interval, speed = 0
        
        _reversed = reversed; // Reverse motor direction
        _enablePin = enablePin;
        _disableOnIdle = disableOnIdle; // Decide whether to apply a holding current or switch off the motor when it's inactive. The latter will prevent overheating.

        pinMode(_enablePin, OUTPUT);

        disable(true);
        
    }

    // Turn a speed parameter from 0.0 - 1.0 into a microsecond step interval.
    void setSpeed(float parameter) {
      
      _stepInterval = _minInterval + ((float) (_maxInterval - _minInterval)) * (1.0 - parameter);
      
    }

    // Set the target destination to a distance parameter from 0.0 (home) to 1.0 (extent).
    void setTargetParameter(float parameter) {

      _target = _extent * parameter;
      
    }

    // Set the target destination to a step index from 0 (home) to the extent.
    void setTargetPosition(long position) {

      _target = max(0, min(_extent, position));
      
    }

    // Set the extent to a new value, providing the rig is currently homed and at home.
    bool setExtent(long extent) {

      bool success = false;
      
      if (_position == 0) {
        _extent = extent;
        success = true;
      }

      return success;
      
    }


    
    float calculateParameter(long stepPosition) {

      return (float) stepPosition / (float) _extent;
      
    }

    // Return the current position as a parameter from 0.0 (home) to 1.0.
    float getParameter() {

      return (float) _position / (float) _extent;
      
    }

    // Return the current position in steps.
    long getPosition() {

      return _position;
      
    }

    // Return the appropriate register state to write to the register next.
    byte output() {

      switch(_stepIndex) {
        case 0:
          return POS_A;
        case 1:
          return POS_B;
        case 2:
          return POS_C;
        case 3:
          return POS_D;
      }
       
    }

    // Increment the step index and return the next regster state.
    byte step() {

      if (_direction == 1) {
        
        _stepIndex ++;
        if (_stepIndex == 4){
          _stepIndex = 0;
        }
      }
      
      else if (_direction == -1){
        if (_stepIndex == 0){
          _stepIndex = 4;
        }
        _stepIndex --;
      }

      return output();
      
    }

    // Check if a step is available by looking a delta, either because position doesn't match target, or you are in jogging mode.
    bool available() {

      bool available = false;
      bool countingSteps = true;
      
      int delta = 0;
      if (!_locked) {
        
        if (_position < _target) {
          delta = 1;
        }
        else if (_position > _target) {
          delta = -1;
        }
        else {
          delta = _jog;
          countingSteps = false;
        }

        if (delta != 0) {

          enable();
          
          if (!_reversed) {
            _direction = delta;
          }
          else {
            _direction = -delta;
          }

          unsigned long now = micros();
          if (now - _lastPolled >= _stepInterval) {

            if (countingSteps){
              _position += delta;
            }
            

            _lastPolled = now;
            available = true;
              
          }
      
        }
        else {

          _direction = 0;
          if (_disableOnIdle) {
            disable();
          }
          
        }
          
      }

      return available;
      
    }

    // Allow free movement in either direction without setting a specific target.
    void startJogging(int direction){
      
       _jog = direction;
       
    }

    // Stop moving.
    void stopJogging() {
      
      _jog = 0;
      
    }

    // Check if moving and in what direction.
    int isMoving() {
      return _direction;
    }

    // Check if the drive is locked.
    bool isLocked() {
      return _locked;
    }

    // Check if we're in jog mode.
    bool isJogging() {
      return _jog != 0;
    }

    void unlock() {
      _locked = false;
    }

    void lock() {
      _locked = true;
    }

    // Provide an event for the ISR, when the limit switch is hit.
    void onLimit() {

      // Ignore limit triggers if direction is away from limit switch. 
      if (_direction == -1) {
        reset();
      }
      
    }

    // Clear a movement target.
    void stop() {

      _target = _position;
      
    }

    // Clear all movement and set this position to be home.
    void reset() {
      _jog = 0;
      _position = 0;
      _target = 0;
      _direction = 0;
    }

    // Enable the motor drive.
    void enable(bool force=false) {

      if (!_enabled || force) {
         _enabled = true;
        digitalWrite(_enablePin, ENABLED);       
      }

    }

    // Disable the motor drive.
    void disable(bool force=false) {
      
      if (_enabled || force) {
         _enabled = false;
         digitalWrite(_enablePin, DISABLED);       
      }

    }

    void dump() {
      
    }
    
  private:

    int _minInterval = 20;
    int _maxInterval = 30000;

    bool _locked = false;
    bool _enabled = false;

    byte _enablePin; 

    bool _reversed = false;
    bool _disableOnIdle = true;
    
    long _position = 0;
    long _target = 0;
    long _extent = 0;
    
    int _jog = 0;
    int _direction = 0;
    
    unsigned long _lastPolled = 0;
    
    int _stepInterval = 30000;
    byte _stepIndex = 0;

    
        
  
};


String serialBuffer = "";

Stepper drive(78540, 100, 400, DRIVE_ENABLE, false, true);

void setup() {

  drive.setSpeed(0.5);

  boot();

  delay(500);

  runHoming();
  
}

void loop() {

  checkForInput(false); // Poll serial and handle commands.
  outputSteps(); // Write stepper output to the register
  
}

void boot() {

  Serial.begin(9600);

  pinMode(DRIVE_LIMIT, INPUT_PULLUP);
  
  DDRC = B11110000; // Set pins 30-34 as output

  // Do not allow booting if the carriage is already in contact with the homing switch.
  if (!digitalRead(DRIVE_LIMIT)) {
    Serial.print(ERROR);
    Serial.println("Cannot boot: limit hit.");
  }

  // Wait for the switch to be open
  while (!digitalRead(DRIVE_LIMIT)) {
    ;
  }

  // Notify that the rig is booted.
  Serial.print(STATUS);
  Serial.println("BOOT");
  
}

void runHoming() {

  Serial.print(STATUS);
  Serial.println("HOME_START");

  // Enable limit pin interrupt after boot.
  attachInterrupt(digitalPinToInterrupt(DRIVE_LIMIT), onLimit, FALLING);

  // Start moving towards the limit switch, stopping when it's reached.
  drive.startJogging(-1);
  while (drive.isJogging()) {
    
    outputSteps();

  }

  // Check for input, but ignore any movement commands sent before homing is complete.
  checkForInput(true);

  // Notify that the rig is homed.
  Serial.print(STATUS);
  Serial.println("HOME_SUCCESS");
  
}


// ISR for limit pin contact.
void onLimit() {

  drive.onLimit();
  
}


void outputSteps() {

  // If a step is available, get the new register state to be written
  if (drive.available()) {

    // Write data to the first four pins in the 8 pin register.
    byte outputA = drive.step() << 4;
    
    PORTC = outputA;
    
  }  
  
}

bool checkForInput(bool ignore) {

  bool dataAvailable = false;

  int incomingCount = Serial.available();
  if (incomingCount > 0) {

    dataAvailable = true;

    char incoming[incomingCount];
    Serial.readBytes(incoming, incomingCount);

    for (int i = 0; i < incomingCount; i++) {
      if (incoming[i] != '\n') {
        serialBuffer += incoming[i];
      }
      else{
        if (not ignore) {
          handleCommand(serialBuffer);
        }
        serialBuffer = "";
      }
    }
  }

  return dataAvailable;

}

// Serial commands are prefaced by an identifying character code, a bit like g-code. 
// Any characters after this code and before a line ending are inferred to be an accompanying float or long value.
void handleCommand(String commandStr) {

  if (commandStr != "") {

    char action = commandStr[0];
    commandStr.remove(0, 1);
    
    String value = commandStr;

    switch ((int) action) {

      // Run homing
      case 'h': 

        runHoming();
        break;

      // Toggle movement in a direction
      case 'm': {

        int direction = drive.isMoving();

        if (value == "c") {
          if (direction != CLOCKWISE) {
            drive.setTargetParameter(0.0);
          }
          else{
            drive.stop();
          }
        }
        else if (value == "a"){
          if (direction != ANTICLOCKWISE) {
            drive.setTargetParameter(1.0);
          }
          else{
            drive.stop();
          }
        }
        else{
          Serial.print(ERROR);
          Serial.println("Unrecognised motion direction.");
        }
        
        break;
        
      }

      // Stop any current movement, get position
      case 'p':

        drive.stop();
        Serial.print(POSITION);
        Serial.println(drive.getPosition());
        break;

      // Jog to a position
      case 'j': {

        long position = value.toInt();
        drive.setTargetPosition(position);
        break;
        
      }

      // Set speed
      case 's': {
        
        float parameter = max(0.0, min(value.toFloat(), 1.0));
        drive.setSpeed(parameter);
        break;   
            
      }

      // Set extent
      case 'e': {

        long extent = value.toInt();

        if (!drive.setExtent(extent)) {
          Serial.print(ERROR);
          Serial.println("Cannot set extent - position is non-zero!");
        }
        break;
        
      }

      // Lock drive
      case 'l':

        drive.lock();
        break;

      // Unlock drive
      case 'u':

        drive.unlock();
        break;

      //Unknown command
      default:

        Serial.print(ERROR);
        Serial.println("Unrecognised command!");
        break;
    }
    
  }
}