/// \file StepperShow.ino
/// \brief Driver for four-channel stepper-motor system.
/// An adaptation of StepperSpline to include autonomous movement control.
///
/// \copyright Written over 2014-2023 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 expressive gestural motion using
/// cubic splines.  It assumes the host communicates over the serial port to
/// receive motion commands and report status.  The protocol is event-driven and
/// implements parameterized gestural motions.  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 "SplinePath.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
// tempo        <spm>                   set the spline execution tempo in segments per minute

// ----------------------------------------------------------------
// 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 path is set to the specified position by
// replacing the current spline with a single segment ending at this target.
// 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.  Each
// controller path is set to the specified position by replacing the current
// spline with a single segment ending at this relative target.  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 origin/start spline.  This defines the end of the current path as the origin
// for knot positions on the specified channels.
//
//   s <flags>
//
// Examples:
//   s xyza			set origin on all channels
//   s x			set origin on X axis

// Relative knot. There should be an integer target value corresponding to each
// included channel; a cubic Bezier spline knot is appended to each controller
// target with the specified position relative to the current origin.
//
//   k <flags> <offset>+
//
// Examples:
//   k xyza 100 120 -200 -50	append the given knot point to each axis
//   k x 50			add an X axis knot at 50

// --------------------------------
// Set spline rate.  There should be a rate value corresponding to each included
// channel.  Rates are specified as a segment duration in milliseconds per spline
// segment.
//
//   r <flags> <rate>+
//
// Examples:
//   r xyza 500 500 500 500	set all channels to 0.5 sec per segment
//   r x 1000			set X axis to 1.0 sec per spline segment

// ----------------------------------------------------------------
// 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 = 2000000; // 0.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 SplinePath 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 StepperSpline " __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);
}

// ================================================================
// Update a step generator for new targets.
static inline void update_step_generator(Stepper *axis, SplinePath *path)
{
  float q  = path->currentPosition();
  float qd = path->currentVelocity();
  long position = axis->currentPosition();

  // always set the step target according to the current spline position
  long target = (long) q;
  axis->setTarget(target);

  // Choose a speed which is the maximum of the spline velocity or a velocity
  // proportional to the error.  This allows the step generator to follow an
  // asymptotic approach if it falls behind.
  long error = abs(target - position);
  int speed  = 2 * error;
  axis->setSpeed(max(speed, (int)(abs(qd))));
}

// ================================================================
/// 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
  update_step_generator(&x_axis, &x_path);
  update_step_generator(&y_axis, &y_path);
  update_step_generator(&z_axis, &z_path);
  update_step_generator(&a_axis, &a_path);
}
// ================================================================
/// Return a SplinePath 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 SplinePath *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.
///
/// @param argc		number of argument tokens
/// @param argv		array of pointers to strings, one per token
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) {
	SplinePath *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->setTarget(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "k")) {
    if (argc > 2) {
      set_driver_enable(1);
      char *flags = argv[1];
      int channel = 0;
      while (*flags) {
	SplinePath *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->addKnot(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) {
	SplinePath *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->incrementTarget(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "s")) {
    if (argc > 1) {
      char *flags = argv[1];
      while (*flags) {
	SplinePath *p = path_flag_iterator(&flags);
	if (p) p->setOrigin();
      }
    }
  } else if (string_equal(command, "r")) {
    if (argc > 2) {
      char *flags = argv[1];
      int channel = 0;
      while (*flags) {
	SplinePath *p = path_flag_iterator(&flags);
	if (p) {
	  if (argc > (channel+2)) {
	    p->setDuration(atol(argv[channel+2]));
	    channel++;
	  }
	}
      }
    }
  } else if (string_equal(command, "tempo")) {
    if (argc > 1) {
      int value = atoi(argv[1]);
      if (value > 0 && value < 300) {
	x_path.setTempo(value);
	y_path.setTempo(value);
	z_path.setTempo(value);
	a_path.setTempo(value);
      }
    }
  } 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);

  script_setup();

  // 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.
void loop(void)
{
  static unsigned long last_event_loop = 0;

  // 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
  script_poll(interval);
}

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