/// \file ValveControl.ino
///
/// \brief Hardware I/O driver for controlling pneumatic valves using a simple message protocol.
///
/// \copyright Copyright (c) 2014-2020, Garth Zeglin.  All rights
///            reserved. Licensed under the terms of the BSD 3-clause license.
///
/// \details This example is intended as a starting point for creating custom
///          firmware for driving one or more pneumatic actuators. It includes a
///          simple ASCII protocol for sending motion commands to ease
///          connecting to dynamic code (e.g. Max or Python) running on a laptop
///          or Raspberry Pi.

#include "Pneumatic.h"
#include "Joint.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 controller is running
// stop					set all valve channels to a neutral state
// led		<value>			controls the built-in LED, value is 0 or non-zero
// speed	<axis> <value>		sets the PWM rate and direction; axis is an integer from 1 to N, value ranges over -100 to 100
// pos          <axis> <value>		sets the joint position target; axis is an integer from 1 to N, value is in degrees

// This program generates the following messages:

// Command	Arguments		Meaning
// awake                                initialization has completed or ping was received
// 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

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

/****************************************************************/
/// Property specification for each pneumatic axis.  This is used to define an
/// initialization table for the specific connected hardware.
struct axis_config_t {
  int8_t fill, empty;
  enum Pneumatic::pneumatic_config_t config;
};

#if 1

// Declare the hardware configuration for a valve card in the valve stack rack.
static struct axis_config_t config_table[] = {
  { 2, 3, Pneumatic::FILL_EMPTY },
  { 4, 5, Pneumatic::FILL_EMPTY },
  { 6, 7, Pneumatic::FILL_EMPTY },
  { 8, 9, Pneumatic::FILL_EMPTY }
};
#endif

#if 0
// Declare the hardware configuration for the 'instructor station'.
static struct axis_config_t config_table[] = {
  // { 3, 5, Pneumatic::PROP_FILL_PROP_EMPTY },
  // { 6, 9, Pneumatic::PROP_FILL_PROP_EMPTY },

  { 3, 5, Pneumatic::FILL_EMPTY },
  { 6, 9, Pneumatic::FILL_EMPTY },
  
  { 2, 4, Pneumatic::FILL_EMPTY },
  { 7, -1, Pneumatic::FILL_THREEWAY },
  { 8, -1, Pneumatic::FILL_THREEWAY }
};
#endif

/// Compute the number of entries in the axis table in use.
#define NUM_CHANNELS (sizeof(config_table) / sizeof(struct axis_config_t))

/// Control object for each pneumatic channel. 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 Pneumatic channel[NUM_CHANNELS];

/// Define the number of physical axes to control.
#define NUM_JOINTS 2

/// Control object for each closed-loop joint controller.  These objects manage
/// the sensor input for each joint and can generate control signals to one or
/// more Pneumatic objects.  The declaration statically initializes the global
/// state objects for the axes.  Note that this does not initialize the
/// hardware; that is performed in setup().
static Joint controller[NUM_JOINTS];

/****************************************************************/
/****************************************************************/
/// Process an input message received from the USB port.
/// Unrecognized commands are silently ignored.

/// \param argc		number of argument tokens
/// \param argv		array of pointers to strings, one per token

static void vc_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");

    } else if (string_equal(command, "stop")) {
      // stop all feedback control and close off all manifolds
      for (int i = 0; i < NUM_JOINTS; i++)   controller[i].setMode(Joint::IDLE);
      for (int i = 0; i < NUM_CHANNELS; i++) channel[i].setDutyCycle(0.0);

    } else if (string_equal(command, "empty")) {
      // stop all feedback control and empty all manifolds
      for (int i = 0; i < NUM_JOINTS; i++)   controller[i].setMode(Joint::IDLE);
      for (int i = 0; i < NUM_CHANNELS; i++) channel[i].setDutyCycle(-1.0);
    }
  }

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

  /* -- process two-argument commands --------------------------- */
  else if (argc == 3) {
    
    if ( string_equal( command, "speed" )) {
      int axis = atoi(argv[1]);
      int value = atoi(argv[2]);
      // takes an integer value between -100 and 100
      if (axis > 0 && axis <= NUM_CHANNELS) channel[axis-1].setDutyCycle( 0.01 * value );
    }
    else if ( string_equal( command, "pos" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      int value = atoi(argv[2]);  // in degrees
      if (axis > 0 && axis <= NUM_JOINTS) {
	controller[axis-1].setMode(Joint::POSITION);
	controller[axis-1].setTargetPosition((float) value);
      }
    }
    else if ( string_equal( command, "offset" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      int value = atoi(argv[2]);  // in raw ADC units
      if (axis > 0 && axis <= NUM_JOINTS) controller[axis-1].setOffset(value);
    }

    else if ( string_equal( command, "scale" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      float value = atof(argv[2]);  // in degree/raw ADC units
      if (axis > 0 && axis <= NUM_JOINTS) controller[axis-1].setScale(value);
    }
  }
}

/****************************************************************/
/// Polling function to update all background control tasks.

static void vc_hardware_poll(long interval)
{
  for (int i = 0; i < NUM_JOINTS; i++) {
    controller[i].update(interval);
  }
  for (int i = 0; i < NUM_CHANNELS; i++) {
    channel[i].update(interval);
  }
}

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

/// Standard Arduino initialization function to configure the system.  This code
/// will need to be modified to initialize the channel[] array with Pneumatic
/// objects matching the specific valve and manifold configuration, and the
/// controller[] array with the physical axis specifications.
void setup(void)
{
  // Initialize the array of axis control objects and valve I/O as soon as possible.
  for (int i = 0; i < NUM_CHANNELS; i++) {
    channel[i] = Pneumatic(config_table[i].fill, config_table[i].empty, config_table[i].config);
    channel[i].begin();
  }

  // Initialize a closed loop controller. The scale and offset were calculated empirically and
  // will need to be adjusted for every individual joint.

  // single-ended test:
  // controller[0] = Joint(Joint::SINGLE_FILL_EMPTY, &channel[0], NULL, A0, -0.338, 608);

  // Our default joints are double-ended:
  controller[0] = Joint(Joint::DUAL_FILL_EMPTY, &channel[0], &channel[1], A0, 0.338, 512);
  controller[1] = Joint(Joint::DUAL_FILL_EMPTY, &channel[2], &channel[3], A1, 0.338, 512);
  
#ifdef LED_BUILTIN
  pinMode( LED_BUILTIN, OUTPUT );
#endif

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

  // 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 function is called repeatedly as fast as possible from within the
/// built-in library to poll program events.  This loop should never be allowed
/// to stall or block so that all tasks can be constantly serviced.
void loop()
{
  // The timestamp in microseconds for the last polling cycle, used to compute
  // the exact interval between output updates.
  static unsigned long last_update_clock = 0;
  
  // Read the microsecond 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_update_clock;
  last_update_clock = now;

  // Begin the polling cycle.
  vc_hardware_poll(interval);
  vc_serial_input_poll(interval);
  vc_status_output_poll(interval);
  
  // other polled tasks can go here
}

/****************************************************************/
void vc_status_output_poll(long interval)
{
  static long timer = 0;
  const long period = 100000;  // 10 Hz
  
  // Wait for the next status output event time point.
  timer -= interval;
  if (timer <= 0) {
    timer += period;

    // turn off data stream
#if 0
    controller[0].send_status();
    Serial.print(" ");
    controller[1].send_status();
    Serial.println();
#endif
    
  }
}
/****************************************************************/