/// \file Joint.h
/// \brief Pneumatic axis controller for one joint.
/// \copyright Copyright (c) 2015-2016, Garth Zeglin.  All rights reserved. Licensed under the terms of the BSD 3-clause license.
/// \details Pneumatic axis controller for one joint, using separate fill and empty valves.

#ifndef __JOINT_H_INCLUDED__
#define __JOINT_H_INCLUDED__

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

#include "Pneumatic.h"

/// An instance of this class manages generation of fill and empty signals for
/// one joint axis.
class Joint {

public:
  /// Valve configuration for the axis.
  enum joint_config_t { SINGLE_FILL_EMPTY,        ///< single set of fill/empty valves, with gravity or spring return
			DUAL_FILL_EMPTY,          ///< double set of fill/empty valves to actively push and pull
			NUM_JOINT_CONFIGURATIONS  ///< the total number of defined valve configuration modes
  };

  /// Control modes for the axis.
  enum joint_state_t { IDLE,             ///< no control computed, no outputs updated
		       OPENLOOP,         ///< open loop pwm output
		       POSITION,         ///< proportional position control
		       NUM_JOINT_MODES   ///< the total number of axis control modes
  };

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

  /// The underlying Pneumatic objects manage generate valve switching signals.
  Pneumatic *push, *pull;

  /// The I/O pins for this channel designated using the Arduino convention.
  int8_t position_pin;  ///< pin designator for the analog position input (e.g. A0), or -1 if no sensor

  /// Configuration of the valve set for this axis.
  enum joint_config_t config;

  /// Countdown for the sensor measurement period, in microseconds.
  long sensor_timer;

  /// Duration of the sensor measurement cycle, in microseconds.
  long sensor_period;

  /// Countdown for the control period, in microseconds.
  long control_timer;

  /// Duration of the control cycle, in microseconds.
  long control_period;

  /// Raw position signal, in ADC units.
  int raw_position;

  /// Filtered position signal, in degrees.
  float position;

  /// Scaling between ADC units and degrees, units are degree/ADC unit.
  float scale;

  /// Offset subtracted from raw ADC signal prior to scaling, in ADC units.
  int offset;

  /// Current position target.
  float desired_position;

  /// Proportional position gain, units are 1/degree.
  float k_position;

  /// Current output PWM rate.
  float output_pwm;

  /// Current control mode for single axis.
  enum joint_state_t mode;

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

public:

  /// Default constructor called during array initialization.  Note that this is
  /// a no-op, since each object needs to be properly initalized with unique pin
  /// numbers later.
  Joint() { }

  /// Main constructor.  Note: this does not initialize the underlying hardware.
  Joint( enum joint_config_t _config, Pneumatic *_push, Pneumatic *_pull = NULL, int8_t _position_pin = -1, float _scale = 0.32, float _offset = 512 ) {
    push             = _push;
    pull             = _pull;
    position_pin     = _position_pin;
    config    	     = _config;
    control_timer    = 0;
    sensor_timer     = 0;
    sensor_period    = 5000; // 200Hz
    control_period   = 50000; // 20Hz
    raw_position     = 0;
    position         = 0.0;
    scale            = _scale;
    offset           = _offset;
    mode             = IDLE;
    desired_position = 0.0;
    k_position       = 1.0 / 30.0;   // full-on PWM at 30 degrees error
    output_pwm       = 0.0;
  }

  /// Hardware initialization.  This class follows the Arduino convention of
  /// initializing instance data in the constructor but not hardware.
  void begin(void) {
    // Read the analog input once to guarantee the ADC mode and initialize the filter.
    if (position_pin >= 0) {
      raw_position = analogRead(position_pin);
      position = (raw_position - offset) * scale;
    }
  }

  /// Main polling function to be called as often as possible.  The interval
  /// argument is the duration in microseconds since the last call.
  void update(long interval);

  /// Send a status output message to the serial port.  This should not be called too often to avoid stalling.
  void send_status(void);

  /// Set the position target.
  void setTargetPosition(float _degrees) {
    desired_position = _degrees;
  }

  /// Set a open-loop PWM output.
  void setPWM(float pwm) {
    output_pwm = pwm;
  }

  /// Set the control mode.
  void setMode(Joint::joint_state_t _mode) {
    mode = _mode;
  }

  /// Update the position sensor gain.
  void setScale(float _scale) {
    scale = _scale;
  }

  /// Update the position sensor offset.
  void setOffset(int _offset) {
    offset = _offset;
  }

};

#endif //__JOINT_H_INCLUDED__