/// \file StepperWinch.ino
/// \brief Driver for four-channel stepper-motor capstan winch system.
///
/// \copyright Written over 2014-2018 by Garth Zeglin <garthz@cmu.edu>.  To the
/// extent possible under law, the author has dedicated all copyright and
/// related and neighboring rights to this software to the public domain
/// worldwide.  This software is distributed without any warranty.  You should
/// have received a copy of the CC0 Public Domain Dedication along with this
/// software.  If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
///
/// \details This sketch is designed to support the expressive gestural motion
/// of a kinetic fabric sculpture rather than perform precision trajectory
/// tracking as would be typical for CNC applications.  The communicates over
/// the serial port to receive motion commands and report status.  The protocol
/// is event-driven and implements parameterized gestural motions; it is
/// patterned after MIDI or OSC conventions.  This sketch assumes the following:
///
/// 1. The TimerOne library is installed in the Arduino IDE.
/// 2. Four A4988 stepper motor drivers are connected following the CNC Shield pin conventions.
/// 3. If using a CNC Shield board, A-axis jumpers are installed to connect A-DIR to D13 (also SpinDir) and A-STEP to D12 (also SpinEnable)..
/// 4. The serial communication baud rate is 115200 bps.
///
/// Typically, for 200 step/rev (1.8 deg) stepper motors the drivers are
/// configured for 1/4 step microstepping (MS2 pulled high).  However, the
/// protocol commands use integer step units so the code does not depend on
/// this.

// ================================================================
// Import the TimerOne library to support timer interrupt processing.
#include "TimerOne.h"

// Include the other modules from this sketch.
#include "cnc_shield.h"
#include "Stepper.h"
#include "Path.h"

// ================================================================
// Communication protocol.

// The message protocol is based on plain-text commands sent as keywords and
// values in a line of text.  One line is one message.  All the input message
// formats begin with a string naming a specific command or destination followed
// by one or two arguments.  The output formats are similar but include more
// general debugging output with a variable number of tokens.

// ----------------------------------------------------------------
// The following global messages are not channel-specific.

// Command	Arguments		Meaning
// ping                                 query whether the server is running
// version				query the identity of the sketch
// srate        <value>                 set the status reporting interval in milliseconds
// enable       <value>                 enable or disable all driver outputs, value is 0 or non-zero

// ----------------------------------------------------------------
// The following messages include a token representing the flag set specifying
// the affected axes.  The flag set should include one or more single-letter
// channel specifiers (regex form: "[xyza]+").  Note that the flag set cannot be
// empty.

// --------------------------------

// Absolute move. There should be an integer target value corresponding to each
// included channel; each controller target is set to the specified position.
// The motion is not coordinated; different channels may finish at different
// times.  Note that this command will enable all drivers.

//   a <flags> <offset>+
//
// Examples:
//   a xyza 100 120 -200 -50	move the axes to the specified locations
//   a x 50			move the X axis to +50

// --------------------------------

// Relative move. There should be an offset value corresponding to each included
// channel; each controller target is incremented by the specified amount.  The
// motion is not coordinated; different channels may finish at different times.
// Note that this command will enable all drivers.
//
//   d <flags> <offset>+
//
// Examples:
//   d xyza 10 10 -10 -10	move all axes ten steps (two forward, two backward)
//   d x 50			move the X axis fifty steps forward

// --------------------------------

// Set velocity. There should be an integer velocity value corresponding to each
// included channel; each controller target velocity is set to the amount
// specified in units/sec.  Note that this command will enable all drivers.
//
//   v <flags> <value>+
//
// Examples:
//   v xyza 10 10 -10 -10	set all axes to drift 10 steps per second (two forward, two backward)
//   v x 500			set the X axis to constantly move forward at roughly half speed

// --------------------------------
// Set second-order gains.  The same dynamic parameters are applied to all included channels.
//   g <flags> <frequency (Hz)> <damping-ratio>
//
// Examples:
//   g xyza 2.0 1.0		set all channels to 1 Hz natural frequency with critical damping
//   g xyza 0.1 0.5		set all channels to 0.1 Hz natural frequency and underdamping

// --------------------------------
// Set velocity and acceleration limits.  The same dynamic parameters are applied to all included channels.
//   l <flags> <maximum velocity (steps/sec)> <maximum acceleration (steps/sec/sec)>
//
// Examples:
//   l xyza 4000 40000		set all channels to 4000 steps/sec and 40000 steps/sec/sec

// ----------------------------------------------------------------
// This program generates the following messages:

// Command	Arguments		Meaning
// awake                                initialization has completed or ping was received
// txyza        <usec> <x> <y> <z> <a>  Arduino clock time in microseconds, followed by absolute step position
// dbg		<value-or-token>+	debugging message to print for user
// id		<tokens>+		tokens identifying the specific sketch

// ================================================================
// Global variables and constants.

// The baud rate is the number of bits per second transmitted over the serial port.
#define BAUD_RATE 115200

// Interval in microseconds between status messages.
static unsigned long status_poll_interval = 200000; // 5 Hz message rate to start

/// Control objects for the stepper channels.  The declaration statically
/// initializes the global state objects for the channels.  Note that this does
/// not initialize the hardware; that is performed in setup().
static Stepper x_axis(X_AXIS_STEP_PIN, X_AXIS_DIR_PIN);
static Stepper y_axis(Y_AXIS_STEP_PIN, Y_AXIS_DIR_PIN);
static Stepper z_axis(Z_AXIS_STEP_PIN, Z_AXIS_DIR_PIN);
static Stepper a_axis(A_AXIS_STEP_PIN, A_AXIS_DIR_PIN);

/// Path generator object for each channel.
static Path x_path, y_path, z_path, a_path;

/// The timestamp in microseconds for the last polling cycle, used to compute
/// the exact interval between stepper motor updates.
static unsigned long last_interrupt_clock = 0;

/// Identification string.
static const char version_string[] = "id StepperWinch " __DATE__;

// ================================================================
/// Enable or disable the stepper motor drivers.  The output is active-low,
/// so this inverts the sense.
static inline void set_driver_enable(int value)
{
  digitalWrite(STEPPER_ENABLE_PIN, (value != 0) ? LOW : HIGH);
}

// ================================================================
/// Interrupt handler to update all the stepper motor channels.  Note that this
/// is called from a timer interrupt context, so it should take as little time as
/// feasible and cannot use serial I/O (i.e. no debugging messages).
void stepper_output_interrupt(void)
{
  // read the clock
  unsigned long now = micros();

  // Compute the time elapsed since the last poll.  This will correctly handle wrapround of
  // the 32-bit long time value given the properties of twos-complement arithmetic.
  unsigned long interval = now - last_interrupt_clock;
  last_interrupt_clock = now;

  // Update all the stepper channels. This may emit step signals or simply
  // update the timing and state variables.
  x_axis.pollForInterval(interval);
  y_axis.pollForInterval(interval);
  z_axis.pollForInterval(interval);
  a_axis.pollForInterval(interval);
}

// ================================================================
/// Polling function called from the main event loop to update the path model
/// and update the step generators.
void path_poll(unsigned long interval)
{
  x_path.pollForInterval(interval);
  y_path.pollForInterval(interval);
  z_path.pollForInterval(interval);
  a_path.pollForInterval(interval);

  // update the step generator for new targets
  x_axis.setTarget(x_path.currentPosition());
  x_axis.setSpeed(abs(x_path.currentVelocity()));

  y_axis.setTarget(y_path.currentPosition());
  y_axis.setSpeed(abs(y_path.currentVelocity()));

  z_axis.setTarget(z_path.currentPosition());
  z_axis.setSpeed(abs(z_path.currentVelocity()));

  a_axis.setTarget(a_path.currentPosition());
  a_axis.setSpeed(abs(a_path.currentVelocity()));
}
// ================================================================
/// Return a Path object or NULL for each flag in the flag token.  As a side effect, updates
/// the source pointer, leaving it at the terminating null.
static Path *path_flag_iterator(char **tokenptr)
{
  char flag = **tokenptr;
  if (flag == 0) return NULL;
  else {
    (*tokenptr) += 1;
    switch (flag) {
    case 'x': return &x_path;
    case 'y': return &y_path;
    case 'z': return &z_path;
    case 'a': return &a_path;
    default: return NULL;
    }
  }
}

// ================================================================
/// Process an input message.  Unrecognized commands are silently ignored.
///   argc - number of argument tokens
///   argv - array of pointers to strings, one per token

static void parse_input_message(int argc, char *argv[])
{
  if (argc == 0) return;

  // Interpret the first token as a command symbol.
  char *command = argv[0];

  if (string_equal(command, "enable")) {
    if (argc > 1) set_driver_enable(atoi(argv[1]));

  } else if (string_equal(command, "a")) {
    if (argc > 2) {
      set_driver_enable(1);
      char *flags = argv[1];
      int channel = 0;
      while (*flags) {
	Path *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->setTarget(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "d")) {
    if (argc > 2) {
      set_driver_enable(1);
      char *flags = argv[1];
      int channel = 0;
      while (*flags) {
	Path *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->incrementTarget(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "v")) {
    if (argc > 2) {
      set_driver_enable(1);
      char *flags = argv[1];
      int channel = 0;
      while (*flags) {
	Path *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->setVelocity(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "g")) {
    if (argc > 3) {
      char *flags = argv[1];
      float frequency = atof(argv[2]);
      float damping_ratio = atof(argv[3]);
      while (*flags) {
	Path *p = path_flag_iterator(&flags);
	if (p) p->setFreqDamping(frequency, damping_ratio);
      }
    }
  } else if (string_equal(command, "l")) {
    if (argc > 3) {
      char *flags = argv[1];
      float qdmax = atof(argv[2]);
      float qddmax = atof(argv[3]);
      while (*flags) {
	Path *p = path_flag_iterator(&flags);
	if (p) p->setLimits(qdmax, qddmax);
      }
    }
  } else if (string_equal(command, "version")) {
    send_message(version_string);

  } else if (string_equal(command, "ping")) {
    send_message("awake");

  } else if (string_equal(command, "srate")) {
    if (argc > 1) {
      long value = atol(argv[1]);
      // set the reporting interval (milliseconds -> microseconds)
      if (value > 0)  status_poll_interval = 1000*value;
      else send_debug_message("invalid srate value");
    }
  }
}

/****************************************************************/
/// Polling function to send status reports at periodic intervals.
static void status_poll(unsigned long interval)
{
  static long timer = 0;
  timer -= interval;

  if (timer < 0) {
    timer += status_poll_interval;

    // send a time and position reading
    long clock = micros();
    long x = x_axis.currentPosition();
    long y = y_axis.currentPosition();
    long z = z_axis.currentPosition();
    long a = a_axis.currentPosition();
    send_message("txyza", clock, x, y, z, a);
  }
}

/****************************************************************/
/**** Standard entry points for Arduino system ******************/
/****************************************************************/

/// Standard Arduino initialization function to configure the system.
void setup(void)
{
  // set up the CNC Shield I/O
  digitalWrite(STEPPER_ENABLE_PIN, HIGH); // initialize drivers in disabled state
  pinMode(STEPPER_ENABLE_PIN, OUTPUT);

  pinMode(X_AXIS_STEP_PIN, OUTPUT);
  pinMode(Y_AXIS_STEP_PIN, OUTPUT);
  pinMode(Z_AXIS_STEP_PIN, OUTPUT);
  pinMode(A_AXIS_STEP_PIN, OUTPUT);

  pinMode(X_AXIS_DIR_PIN, OUTPUT);
  pinMode(Y_AXIS_DIR_PIN, OUTPUT);
  pinMode(Z_AXIS_DIR_PIN, OUTPUT);
  pinMode(A_AXIS_DIR_PIN, OUTPUT);

#ifdef LED_BUILTIN
  pinMode(LED_BUILTIN, OUTPUT);
#endif

  // initialize the Serial port
  Serial.begin(BAUD_RATE);

  // set up the timer1 interrupt and attach it to the stepper motor controls
  last_interrupt_clock = micros();
  Timer1.initialize(100); // 100 microsecond intervals, e.g. 10kHz
  Timer1.attachInterrupt(stepper_output_interrupt);

  // send a wakeup message
  send_message("awake");
}

/****************************************************************/
/// Standard Arduino polling function to handle all I/O and periodic processing.
/// This loop should never be allowed to stall or block so that all tasks can be
/// constantly serviced.
static unsigned long last_event_loop = 0;

void loop(void)
{
  // read the clock
  unsigned long now = micros();

  // Compute the time elapsed since the last polling cycle.  This will correctly handle wrapround of
  // the 32-bit long time value given the properties of twos-complement arithmetic.
  unsigned long interval = now - last_event_loop;
  last_event_loop = now;

  serial_input_poll();
  status_poll(interval);
  path_poll(interval);

  // other polled tasks can go here
}

/****************************************************************/
/****************************************************************/

/// \file Path.h
/// \brief Path generator for a single motor.
///
/// \copyright Written 2018 by Garth Zeglin <garthz@cmu.edu>.  To the extent
/// possible under law, the author has dedicated all copyright and related and
/// neighboring rights to this software to the public domain worldwide.  This
/// software is distributed without any warranty.  You should have received a
/// copy of the CC0 Public Domain Dedication along with this software.  If not,
/// see <http://creativecommons.org/publicdomain/zero/1.0/>.
///
/// \details This class implements several smooth path generators intended for
/// generating gestural motions on a single motor channel.  It assumes a
/// separate controller manages the step generator of closed-loop control of the
/// physical hardware.

#ifndef __PATH_H_INCLUDED__
#define __PATH_H_INCLUDED__

#include <math.h>

// ================================================================
class Path {

private:
  float q;    	   ///< current model position, in dimensionless units (e.g. step or encoder counts)
  float qd;   	   ///< current model velocity in units/sec
  float qdd;  	   ///< current model acceleration, in units/sec/sec
  float q_d;  	   ///< current model target position in dimensionless units
  float qd_d;  	   ///< current model target velocity in dimensionless units/sec
  float t;    	   ///< elapsed model time, in seconds
  float k;    	   ///< proportional feedback gain, in (units/sec/sec)/(units), which is (1/sec^2)
  float b;    	   ///< derivative feedback gain, in (units/sec/sec)/(units/sec), which is (1/sec)
  float qd_max;    ///< maximum allowable speed in units/sec
  float qdd_max;   ///< maximum allowable acceleration in units/sec/sec

public:

  /// Main constructor.
  Path();

  /// Path-integration polling function to be called as often as possible,
  /// typically from the main event loop.  The interval argument is the duration
  /// in microseconds since the last call.
  void pollForInterval(unsigned long interval);

  /// Add a signed offset to the target position.  The units are dimensionless
  /// 'steps'.  If using a microstepping driver, these may be less than a
  /// physical motor step.
  void incrementTarget(long offset) { q_d += offset; }

  /// Set the absolute target position in dimensionless units.
  void setTarget(long position) { q_d = position; }

  /// Set the target velocity in dimensionless units/second.
  void setVelocity(long velocity) { qd_d = velocity; }

  /// Return the current position in dimensionless units.
  long currentPosition(void) { return (long) q; }

  /// Return the current velocity in units/second.
  long currentVelocity(void) { return (long) qd; }

  /// Configure the second-order model gains.
  void setPDgains(float k_new, float b_new) { k = k_new; b = b_new; }

  /// Convenience function to set second order model gains in terms of natural frequency and damping ratio.
  /// The frequency is in Hz, the damping ratio is 1.0 at critical damping.
  void setFreqDamping(float freq, float damping) {
    k = freq * freq * 4 * M_PI * M_PI;
    b = 2 * sqrtf(k) * damping;
  }

  /// Configure the velocity and acceleration limits.
  void setLimits(float qdmax, float qddmax) { qd_max = qdmax; qdd_max = qddmax; }
};

#endif //__PATH_H_INCLUDED__

/// \file Path.cpp
/// \brief Step and path generator for a single path motor
///
/// \copyright Written over 2014-2018 by Garth Zeglin <garthz@cmu.edu>.  To the
/// extent possible under law, the author has dedicated all copyright and
/// related and neighboring rights to this software to the public domain
/// worldwide.  This software is distributed without any warranty.  You should
/// have received a copy of the CC0 Public Domain Dedication along with this
/// software.  If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.

#include <Arduino.h>
#include <math.h>
#include <stdint.h>

#include "Path.h"

//================================================================
Path::Path()
{
  q    = 0.0;
  qd   = 0.0;
  qdd  = 0.0;
  q_d  = 0.0;
  qd_d = 0.0;
  t    = 0;
  k    = 4*M_PI*M_PI;   // 1 Hz; freq = (1/2*pi) * sqrt(k/m); k = (freq*2*pi)^2
  b    = 1.0;           // damping (would be 2*sqrt(k) for critical damping)
  qd_max  = 3500.0;     // typical physical limit for 4x microstepping
  qdd_max = 35000.0;
}

//================================================================
// Path integration running from main event loop.
void Path::pollForInterval(unsigned long interval)
{
  float dt = 1e-6 * interval;

  // calcuate the derivatives
  float qdd = k * (q_d - q) + b * (qd_d - qd);

  // clamp the acceleration within range for safety
  qdd = constrain(qdd, -qdd_max, qdd_max);

  // integrate one time step
  q  += qd  * dt;
  qd += qdd * dt;
  q_d += qd_d * dt;  // integrate the target velocity into the target position
  t += dt;

  // clamp the model velocity within range for safety
  qd = constrain(qd, -qd_max, qd_max);
}
//================================================================

/// \file Stepper.h
/// \brief Step generator for a single stepper motor.
///
/// \copyright Written over 2014-2018 by Garth Zeglin <garthz@cmu.edu>.  To the
/// extent possible under law, the author has dedicated all copyright and
/// related and neighboring rights to this software to the public domain
/// worldwide.  This software is distributed without any warranty.  You should
/// have received a copy of the CC0 Public Domain Dedication along with this
/// software.  If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
///
/// \details This class implements fast constant-velocity stepping.  It is
/// possible to use this directly, but the overall sketch pairs this with a
/// interpolating path generator which frequently updates the position and
/// velocity setpoints.

#ifndef __STEPPER_H_INCLUDED__
#define __STEPPER_H_INCLUDED__

#include <stdint.h>

/// An instance of this class manages generation of step and direction signals
/// for one stepper motor.
class Stepper {

private:
  /****************************************************************/
  // The following instance variables may only be modified from a non-interrupt
  // context, i.e., not within poll().

  /// the I/O pins for this channel designated using the Arduino convention
  uint8_t step_pin, dir_pin;

  /// the target position in dimensionless step counts
  long target;

  /// the interval in microseconds between steps
  unsigned long step_interval;

  /****************************************************************/
  // The following instance variables may be modified within poll() from an interrupt context.

  /// the current position in dimensionless step counts
  long position;

  /// the time elapsed in microseconds since the last step occurred
  unsigned long elapsed;

  /****************************************************************/
public:

  /// Main constructor.  The arguments are the pin numbers for the step and
  /// direction outputs. Note: this does not initialize the underlying hardware.
  Stepper(uint8_t step_pin, uint8_t dir_pin);

  /// Step-generator polling function to be called as often as possible.  This
  /// is typically called from a timer interrupt.  The interval argument is the
  /// duration in microseconds since the last call.
  void pollForInterval(unsigned long interval);

  /// Add a signed offset to the target position.  The units are dimensionless
  /// 'steps'.  If using a microstepping driver, these may be less than a
  /// physical motor step.
  void incrementTarget(long offset) { target += offset; }

  /// Set the absolute target position.
  void setTarget(long position) { target = position; }

  /// Return the current position in dimensionless 'steps'.
  long currentPosition(void) { return position; }

  /// Set a constant speed in steps/second.  Note that the value must be
  /// non-zero and positive.  The maximum rate available is a function of the
  /// polling rate.
  void setSpeed(int speed) {
    // (1000000 microseconds/second) / (steps/second) = (microseconds/step)
    if (speed > 0) {
      step_interval = 1000000 / speed;
      if (step_interval == 0) step_interval = 1;
    }
  }

};

#endif //__STEPPER_H_INCLUDED__

/// \file Stepper.cpp
/// \brief Step and path generator for a single stepper motor
///
/// \copyright Written over 2014-2018 by Garth Zeglin <garthz@cmu.edu>.  To the
/// extent possible under law, the author has dedicated all copyright and
/// related and neighboring rights to this software to the public domain
/// worldwide.  This software is distributed without any warranty.  You should
/// have received a copy of the CC0 Public Domain Dedication along with this
/// software.  If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.

#include "Stepper.h"
#include <Arduino.h>

Stepper::Stepper(uint8_t _step_pin, uint8_t _dir_pin)
{
  step_pin = _step_pin;
  dir_pin  = _dir_pin;
  position = 0;
  target   = 0;
  elapsed  = 0;

  step_interval = 200;  // 200 microseconds = 5000 steps/sec
}
//================================================================
// Step generator running on fast timer interrupt.
void Stepper::pollForInterval(unsigned long interval)
{
  // Accumulated the time elapsed since the step.
  elapsed += interval;

  if (elapsed >= step_interval) {
    // reset the timer according to the target interval to produce a correct an
    // average rate even if extra time has passed
    elapsed -= step_interval;

    // check whether to emit a step
    if (position != target) {

      // always set the direction to match the sign of motion
      digitalWrite(dir_pin, (position < target) ? HIGH : LOW);

      // emit a step
      digitalWrite(step_pin, HIGH);

      // update the position count
      if (position < target) position++;
      else position--;
      digitalWrite(step_pin, LOW);
    }
  }
}
//================================================================