#include "Arduino.h"
#include "spline.h"
#include <math.h>

Spline::Spline(void) {
  _prev_point = 0;
}

Spline::Spline( float x[], float y[], int numPoints, int degree )
{
  setPoints(x, y, numPoints);
  setDegree(degree);
  _prev_point = 0;
}

Spline::Spline( float x[], float y[], float m[], int numPoints )
{
  setPoints(x, y, m, numPoints);
  setDegree(Hermite);
  _prev_point = 0;
}

void Spline::setPoints( float x[], float y[], int numPoints ) {
  _x = x;
  _y = y;
  _length = numPoints;
}

void Spline::setPoints( float x[], float y[], float m[], int numPoints ) {
  _x = x;
  _y = y;
  _m = m;
  _length = numPoints;
}

void Spline::setDegree( int degree ) {
  _degree = degree;
}

float Spline::value( float x )
{
  if ( _x[0] > x ) {
    return _y[0];
  }
  else if ( _x[_length - 1] < x ) {
    return _y[_length - 1];
  }
  else {
    for (int i = 0; i < _length; i++ )
    {
      int index = ( i + _prev_point ) % _length;

      if ( _x[index] == x ) {
        _prev_point = index;
        return _y[index];
      } else if ( (_x[index] < x) && (x < _x[index + 1]) ) {
        _prev_point = index;
        return calc( x, index );
      }
    }
  }
  return (0.0);
}

float Spline::calc( float x, int i )
{
  switch ( _degree ) {
    case 0:
      return _y[i];
    case 1:
      if ( _x[i] == _x[i + 1] ) {
        // Avoids division by 0
        return _y[i];
      } else {
        return _y[i] + (_y[i + 1] - _y[i]) * ( x - _x[i]) / ( _x[i + 1] - _x[i] );
      }
    case Hermite:
      return hermite( ((x - _x[i]) / (_x[i + 1] - _x[i])), _y[i], _y[i + 1], _m[i], _m[i + 1], _x[i], _x[i + 1] );
    case Catmull:
      if ( i == 0 ) {
        // x prior to spline start - first point used to determine tangent
        return _y[1];
      } else if ( i == _length - 2 ) {
        // x after spline end - last point used to determine tangent
        return _y[_length - 2];
      } else {
        float t = (x - _x[i]) / (_x[i + 1] - _x[i]);
        float m0 = (i == 0 ? 0 : catmull_tangent(i) );
        float m1 = (i == _length - 1 ? 0 : catmull_tangent(i + 1) );
        return hermite( t, _y[i], _y[i + 1], m0, m1, _x[i], _x[i + 1]);
      }
  }
  return (0.0);
}

float Spline::hermite( float t, float p0, float p1, float m0, float m1, float x0, float x1 ) {
  return (hermite_00(t) * p0) + (hermite_10(t) * (x1 - x0) * m0) + (hermite_01(t) * p1) + (hermite_11(t) * (x1 - x0) * m1);
}
float Spline::hermite_00( float t ) {
  return (2 * pow(t, 3)) - (3 * pow(t, 2)) + 1;
}
float Spline::hermite_10( float t ) {
  return pow(t, 3) - (2 * pow(t, 2)) + t;
}
float Spline::hermite_01( float t ) {
  return (3 * pow(t, 2)) - (2 * pow(t, 3));
}
float Spline::hermite_11( float t ) {
  return pow(t, 3) - pow(t, 2);
}

float Spline::catmull_tangent( int i )
{
  if ( _x[i + 1] == _x[i - 1] ) {
    // Avoids division by 0
    return 0;
  } else {
    return (_y[i + 1] - _y[i - 1]) / (_x[i + 1] - _x[i - 1]);
  }
}