/// \file SplinePath.h
/// \brief Spline 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 __SPLINE_PATH_H_INCLUDED__
#define __SPLINE_PATH_H_INCLUDED__

#include <math.h>

#define MAX_SPLINE_SEGMENTS 20

// ================================================================
class SplinePath {

private:
  float q;   	   ///< current model position in dimensionless units
  float qd;   	   ///< current model velocity in units/sec
  float origin;    ///< model position origin for offsetting knot values

  float u;    	   ///< current spline position parameter, ranges over 0.0 to 1.0
  float rate;      ///< spline path velocity, units are (1/sec)

  float knots[3*MAX_SPLINE_SEGMENTS+1];  ///< current Bezier knot points in dimensionless units (e.g. step or encoder counts)
  int pts;         ///< current number of valid knot points; always at least four

  /// Internal method to recalculate model values at current path position.
  void recalculate();

public:

  /// Main constructor.
  SplinePath();

  /// Set the spline 'tempo' in units of segments/minute.  The default is 60 or
  /// one segment/second.
  void setTempo(int spm) { rate = spm / 60.0; };

  /// Set the spline segment duration in units of milliseconds per segment.
  void setDuration(long msec_per_seg) { rate = 1000.0 / msec_per_seg; };

  /// Path calculation 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);

  /// Set the absolute target position.   The units are dimensionless
  /// 'steps'.  If using a microstepping driver, these may be less than a
  /// physical motor step.
  void setTarget(long position);

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

  /// Reset the knot origin to the end of the current path.  Affects
  /// interpretation of subsequent knots.
  void setOrigin(void) { origin = knots[pts-1]; };

  /// Append another knot to the spline buffer.  Each additional three knots
  /// extends the previous spline by one segment.  Knots exceeding the buffer
  /// length are silently dropped.
  void addKnot(long position);

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

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

#endif //__SPLINE_PATH_H_INCLUDED__