/// \file SplinePath.cpp
/// \brief Step and path generator for a single motor.
///
/// \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/>.

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

#include "SplinePath.h"

//================================================================
SplinePath::SplinePath()
{
  q    = 0.0;
  qd   = 0.0;
  rate = 1.0;
  memset(knots, 0, sizeof(knots));
  origin = 0.0;

  // invariant: always leave one segment in buffer
  pts  = 4;
  u    = 1.0;
}

//================================================================
/// 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 SplinePath::pollForInterval(unsigned long interval)
{
  // calculate the current number of spline segments
  int segs = (pts-1)/3;

  // integrate the path position one time step
  float dt = 1e-6 * interval;
  u += rate*dt;

  // Determine whether to roll over to the next segment or not.  This will
  // always leave one segment remaining.
  if (segs > 1) {
    if (u >= 1.0) {
      // u has advanced to next segment, drop the first segment by dropping the first three knots
      pts -= 3;
      for (int i = 0; i < pts; i++) knots[i] = knots[i+3];
      u -= 1.0;
    }
  }
  // always limit u to the first active segment
  u = constrain(u, 0.0, 1.0);

  // update q and qd
  this->recalculate();
}

//================================================================
/// Apply the De Casteljau algorithm to calculate the position and velocity of
/// the spline at the current path point.
void SplinePath::recalculate()
{
  // unroll the De Casteljau iteration and save intermediate points
  float beta[3];
  for (int k = 0; k < 3; k++)  beta[k] = knots[k] * (1 - u) + knots[k+1] * u;
  for (int k = 0; k < 2; k++)  beta[k] =  beta[k] * (1 - u) +  beta[k+1] * u;
  qd = 3 * (beta[1] - beta[0]);          // s'(u)
  q  = beta[0] * (1 - u) + beta[1] * u;   // s(u)
}

//================================================================
/// Set the absolute target position in dimensionless units.
void SplinePath::setTarget(long position)
{
  // reset the spline knots to move to the target
  knots[0] = q;  // start at the current position and velocity
  knots[1] = knots[0] + (qd * (1.0/3.0));

  // set the endpoint to the target position
  knots[3] = (float) position;

  // set a zero velocity boundary condition
  knots[2] = knots[3];

  // reset the origin for safety
  origin = knots[3];

  // reset the path position to start the new segment
  u = 0.0;

  // reset the spline length to one segment
  pts = 4;

  // recalculate current values
  this->recalculate();
}

//================================================================
/// 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 SplinePath::incrementTarget(long offset)
{
  // reset the spline knots to move to the relative target
  knots[0] = q;  // start at the current position and velocity
  knots[1] = knots[0] + (qd * (1.0/3.0));

  // set the endpoint to the target position
  knots[3] += (float) offset;

  // set a zero velocity boundary condition
  knots[2] = knots[3];

  // reset the origin for safety
  origin = knots[3];

  // reset the spline length to one segment
  pts = 4;

  // reset the path position to start the new segment
  u = 0.0;

  // recalculate current values
  this->recalculate();
}

//================================================================
/// 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 SplinePath::addKnot(long position)
{
  if (pts < (3*MAX_SPLINE_SEGMENTS+1)) {
    knots[pts] = (float) position + origin;
    pts++;
  }
}
//================================================================