/// \file CNCShieldServer.ino
/// \ingroup CNCShieldServer
/// \brief Simplified hardware I/O server for the CNC Arduino Shield using a simple message protocol.

/// \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 example is intended as a starting point for creating custom
///          firmware for the CNC Shield with A4988 stepper motor drivers.  It
///          includes a simple ASCII protocol for sending motion commands to
///          ease connecting to dynamic code (e.g. Python) running on a laptop
///          or Raspberry Pi.

// This program assumes that:

//  1. three A4988 stepper motor drivers are connected following the CNC Shield pin conventions.
//  2. the TimerOne library is installed

#include "cnc_shield.h"
#include "CncStepper.h"
#include "TimerOne.h"

/****************************************************************/
/**** ASCII messaging scheme ************************************/
/****************************************************************/

// The message protocol is based on commands encoded as a sequence of string
// tokens and integers 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 argument integers.  The output formats are
// similar but include more general debugging output with a variable number of
// tokens.

// The following message formats are recognized by this program.

// Command	Arguments		Meaning
// ping                                 query whether the server is running
// poll         <value>                 set the input polling rate, value is milliseconds
// enable       <value>                 enable or disable driver output, value is 0 or non-zero
// goto         <x> <y> <z>             move to the absolute position in steps
// x            <position>              absolute X axis move
// y            <position>              absolute Y axis move
// z            <position>              absolute Z axis move
// dx           <offset>                incremental X axis move
// dy           <offset>                incremental Y axis move
// dz           <offset>                incremental Z axis move
// sx           <speed>                 set the X axis speed in steps/second
// sy           <speed>                 set the Y axis speed in steps/second
// sz           <speed>                 set the Z axis speed in steps/second
// led		<value>			controls the built-in LED, value is 0 or non-zero
// pwm		<pin> <value>		PWM control on given pin
// dig		<pin> <value>		digital output on given pin, value is 0 or non-zero

// Additional messages can be added by inserting code in the user_message_#() functions below.

// This program generates the following messages:

// Command	Arguments		Meaning
// awake                                initialization has completed or ping was received
// txyz         <usec> <x> <y> <z>      Arduino clock time in microseconds, followed by absolute step position
// dbg		<value-or-token>+	debugging message to print for user
// led		<value>			reply with current LED state

/****************************************************************/
/**** 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 milliseconds between status messages.
static unsigned int hardware_polling_interval = 200; // 5 Hz message rate to start

// Some versions of the Arduino IDE don't correctly define this symbol for an
// Arduino Uno.  Note that this pin definition is potentially shared with
// SPINDLE_DIR_PIN.
#ifndef LED_BUILTIN
#define LED_BUILTIN 13
#endif

/// 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 CncStepper x_axis( X_AXIS_STEP_PIN, X_AXIS_DIR_PIN );
static CncStepper y_axis( Y_AXIS_STEP_PIN, Y_AXIS_DIR_PIN );
static CncStepper z_axis( Z_AXIS_STEP_PIN, Z_AXIS_DIR_PIN );

/// 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;

/****************************************************************/
/****************************************************************/
/// 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);
}

/****************************************************************/
/****************************************************************/
/// 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[])
{
  // Interpret the first token as a command symbol.
  char *command = argv[0];

  /* -- process zero-argument commands --------------------------- */
  if (argc == 1) {
    if ( string_equal( command, "ping" )) {
      send_message("awake");
    }
  }

  /* -- process one-argument commands --------------------------- */
  else if (argc == 2) {
    long value = atol(argv[1] );

    // Process the 'led' command.
    if ( string_equal( command, "led" )) {
#ifdef LED_BUILTIN
      // turn on the LED if that value is true, then echo it back as a handshake
      digitalWrite(LED_BUILTIN, (value != 0) ? HIGH : LOW);
#endif
      send_message( "led", value );
    }
    
    else if ( string_equal( command, "enable" )) {
      // Enable or disable the stepper motor drivers.  The output is active-low,
      // so this inverts the sense.
      digitalWrite( STEPPER_ENABLE_PIN, (value != 0) ? LOW : HIGH );
    }

    // Set the individual absolute position targets.
    else if ( string_equal( command, "x" ))  x_axis.setTarget(value);
    else if ( string_equal( command, "y" ))  y_axis.setTarget(value);
    else if ( string_equal( command, "z" ))  z_axis.setTarget(value);

    // Apply incremental motion offsets to the stepper target positions.
    else if ( string_equal( command, "dx" ))  x_axis.incrementTarget(value);
    else if ( string_equal( command, "dy" ))  y_axis.incrementTarget(value);
    else if ( string_equal( command, "dz" ))  z_axis.incrementTarget(value);

    // Set the speed for each channel.
    else if ( string_equal( command, "sx" )) x_axis.setSpeed(value);
    else if ( string_equal( command, "sy" )) y_axis.setSpeed(value);
    else if ( string_equal( command, "sz" )) z_axis.setSpeed(value);

    else if ( string_equal( command, "poll" )) {
      // set the hardware polling interval in milliseconds
      if (value > 0)  hardware_polling_interval = value;
      else send_debug_message("invalid poll value");
    }
  } 

  /* -- process two-argument commands --------------------------- */
  else if (argc == 3) {
    int pin   = atoi(argv[1] );
    int value = atoi(argv[2] );

    // Process the 'pwm' command to generate a variable duty-cycle PWM signal on
    // any digital pin.  The value must be between 0 and 255.
    if ( string_equal( command, "pwm" )) {
      analogWrite( pin, value );
      return;
    }

    // Process the 'dig' command to set a pin to output mode and control its level.
    else if ( string_equal( command, "dig" )) {
      pinMode( pin, OUTPUT );
      digitalWrite( pin, value );
      return;
    }
  }

  /* -- process three-argument commands --------------------------- */
  else if (argc == 4 ) {
    long x = atol(argv[1]);
    long y = atol(argv[2]);
    long z = atol(argv[3]);
    
    // Set the absolute target position.
    if ( string_equal( command, "goto" )) {
      x_axis.setTarget(x);
      y_axis.setTarget(y);
      z_axis.setTarget(z);
    }
  }
}

/****************************************************************/
/// Polling function to read and send specific input values at periodic
/// intervals.

// N.B. The timing calculation could be improved to reduce jitter.

static void hardware_input_poll(void)
{
  static unsigned long last_time = 0;
  unsigned long now = millis();

  if ((now - last_time) > hardware_polling_interval) {
    last_time = now;

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

/****************************************************************/
/**** 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( X_AXIS_DIR_PIN, OUTPUT );
  pinMode( Y_AXIS_DIR_PIN, OUTPUT );
  pinMode( Z_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 );

  // additional hardware configuration can go here


  // 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)
{
  serial_input_poll();
  hardware_input_poll();

  // other polled tasks can go here
}

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