#pragma region CPL License
/*
Nuclex Native Framework
Copyright (C) 2002-2015 Nuclex Development Labs

This library is free software; you can redistribute it and/or
modify it under the terms of the IBM Common Public License as
published by the IBM Corporation; either version 1.0 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
IBM Common Public License for more details.

You should have received a copy of the IBM Common Public
License along with this library
*/
#pragma endregion // CPL License

#ifndef NUCLEX_GEOMETRY_LINES_INTERPOLATORS_CUBICINTERPOLATOR_H
#define NUCLEX_GEOMETRY_LINES_INTERPOLATORS_CUBICINTERPOLATOR_H

#include "Nuclex/Geometry/Config.h"
#include "Nuclex/Geometry/Point2.h"

#include <cstddef>

namespace Nuclex { namespace Geometry { namespace Lines { namespace Interpolators {

  // ------------------------------------------------------------------------------------------- //

  #if 0

  /// <summary>Example implementation of the TVertexTraits type</summary>
  struct ExampleVertexTraits {

    /// <summary>Type that is used to measure distance between vertices</summary>
    public: typedef float TDistance;
    /// <summary>Type that is used to specify the interpolation interval</summary>
    public: typedef float TScalar;

    /// <summary>A vertex with a value of zero</summary>
    public: static const float ZeroVertex = 0.0f;

    /// <summary>Measures the distance between two vertices</summary>
    /// <param name="first">First vertex for the distance calculation</param>
    /// <param name="second">Second vertex for the distance calculation</param>
    /// <returns>The distance between the two vertices</returns>
    public: static TDistance DistanceBetween(float first, float second) {
      return std::abs(second - first),
    }

    /// <summary>Rounds a scalar / interval value to the closest integer</summary>
    /// <param name="t">Scalar / interval value that will be rounded</param>
    /// <returns>The closest integer to the specified scalar / interval value</returns>
    public: static std::size_t RoundScalarToInteger(float t) {
      return static_cast<std::size_t>(t + 0.5f);
    }

    /// <summary>Truncates a scalar / interval value to the next lower integer</summary>
    /// <param name="t">Scalar / interval value that will be truncated</param>
    /// <returns>The next lower integer to the specified scalar / interval value</returns>
    public: static std::size_t TruncateScalarToInteger(float t) {
      return static_cast<std::size_t>(t);
    }

    /// <summary>Sums a vertex value with another vertex</summary>
    /// <param name="base">Vertex to which another vertex wlll be added</param>
    /// <param name="amount">Other vertex that will be added to the vertex</param>
    public: static void Add(float &base, float amount) {
      base += amount;
    }

    /// <summary>Subtracts another vertex from a vertex</summary>
    /// <param name="base">Point from which another vertex wlll be subtracted</param>
    /// <param name="amount">Other vertex that will be substracted from the vertex</param>
    public: static void Subtract(float &base, float amount) {
      base -= amount;
    }

    /// <summary>Scales a vertex by a factor</summary>
    /// <param name="base">Vertex  that will be scaled by a factor</param>
    /// <param name="factor">Factor the Vertex will be scaled by</param>
    public: static void Scale(float &base, float factor) {
      base *= factor;
    }

  };

  #endif

  // ------------------------------------------------------------------------------------------- //

  /// <summary>Interpolator that generates a smooth path along vertices</summary>
  /// <typeparam name="TVertex">Type of the interpolated values</typeparam>
  /// <typeparam name="TVertexTraits">Additional information about the vertex type</typeparam>
  template <typename TVertex, typename TVertexTraits>
  struct CubicInterpolator {

    /// <summary>
    ///   Performs cubic interpolation between the vertices at the specified time index
    /// </summary>
    /// <typeparam name="TVertexIterator">Type of the iterator used to find vertices</typeparam>
    /// <param name="first">Iterator pointing to the first vertex</param>
    /// <param name="onePastLast">Iterator pointing one past the last vertex</param>
    /// <param name="t">Interpolation time index, equivalent to vertex index</param>
    /// <returns>The cubic-interpolated position at the specified time index</returns>
    public: template<typename TVertexIterator>
    static TVertex InterpolateByTime(
      TVertexIterator first, TVertexIterator onePastLast, typename TVertexTraits::TScalar t
    ) {

      // Empty vertex list? Return default value
      if(onePastLast == first) {
        return TVertexTraits::ZeroVertex;
      }

      // Asking for a time before the first vertex? Clamp to first vertex
      if(t < 0) {
        return *first;
      }

      // Determine the control points. If the interpolation point touches
      // the beginning or end, duplicate the outermost points to ease in + out
      TVertex controls[4];
      {
        // Truncate to obtain left vertex and right vertex index
        std::size_t leftVertexIndex = TVertexTraits::TruncateScalarToInteger(t);
        std::size_t rightVertexIndex = leftVertexIndex + 1;  

        // Obtain the final two vertices (or clamp if right vertex is past the end)
        std::size_t count = onePastLast - first;
        if(rightVertexIndex > count - 1) {
          return *(onePastLast - 1);
        } else if(rightVertexIndex < count - 1) {
          controls[2] = *(first + rightVertexIndex);
          controls[3] = *(first + (rightVertexIndex + 1));
        } else {
          controls[2] = *(first + rightVertexIndex);
          controls[3] = controls[2];
        }

        // Obtain the first two vertices
        if(leftVertexIndex == 0) {
          controls[0] = *first;
          controls[1] = controls[0];
        } else {
          controls[0] = *(first + (leftVertexIndex - 1));
          controls[1] = *(first + leftVertexIndex);
        }

        t -= leftVertexIndex;
      }

      // At this point we have 4 neighboring vertices we can interpolate between
      return interpolateCubic(controls, t);
    }

    /// <summary>
    ///   Performs cubic interpolation between the vertices at the specified distance
    /// </summary>
    /// <typeparam name="TVertexIterator">Type of the iterator used to find vertices</typeparam>
    /// <param name="first">Iterator pointing to the first vertex</param>
    /// <param name="onePastLast">Iterator pointing one past the last vertex</param>
    /// <param name="distance">Distance along the path to interpolate</param>
    /// <returns>The cubic-interpolated position at the specified distance</returns>
    public: template<typename TVertexIterator>
    static TVertex InterpolateByDistance(
      TVertexIterator first, TVertexIterator onePastLast,
      typename TVertexTraits::TDistance distance
    ) {

      // Empty vertex list? Return default value
      if(onePastLast == first) {
        return TVertexTraits::ZeroVertex;
      }

      // If the distance is zero or less, return the starting point
      if(distance < 0) {
        return *first;
      }

      // Determine the control points. If the interpolation point touches
      // the beginning or end, duplicate the outermost points to ease in + out
      TVertex controls[4];
      {
        // Initialize beginning with duplicated start point (for ease in + out)
        controls[0] = *first;

        // If there is only one point, it is the clamped interpolation result
        ++first;
        if(first == onePastLast) {
          return controls[0];
        } else { // Two points or more, perform cubic interpolation
          controls[1] = controls[0];
          controls[2] = *first;

          ++first;
          while (first != onePastLast) {
            controls[3] = *first;
            
            typename TVertexTraits::TDistance segmentLength = measure(controls);
            if(distance < segmentLength) {
              return interpolateCubic(controls, distance / segmentLength);
            }
            distance -= segmentLength;

            // Shift all control points one to the left so we can append the new one
            controls[0] = controls[1];
            controls[1] = controls[2];
            controls[2] = controls[3];

            ++first;
          }
        }
        
        // See if the distance is within the final segment with the last point duplicated
        // (for ease in + out)
        controls[3] = controls[2];
        typename TVertexTraits::TDistance segmentLength = measure(controls);
        if(distance < segmentLength) {
          return interpolateCubic(controls, distance / segmentLength);
        }
      }

      // Specified distance was beyond the end of the path, return the lastmost
      // vertex we processed (thereby clamping to the end of the path)
      return controls[3];

    }

    /// <summary>Estimates the length of a path when cubic-interpolated</summary>
    /// <typeparam name="TVertexIterator">Type of the iterator used to find vertices</typeparam>
    /// <param name="first">Iterator pointing to the first vertex</param>
    /// <param name="onePastLast">Iterator pointing one past the last vertex</param>
    /// <returns>The length of the path</returns>
    public: template<typename TVertexIterator>
    static typename TVertexTraits::TDistance EstimateLength(
      TVertexIterator first, TVertexIterator onePastLast
    ) {
      
      // Empty vertex list? The path has a length of 0
      if(onePastLast == first) {
        return 0;
      }

      // Perform the measurement by summing the lengths of the individual path segments
      typename TVertexTraits::TDistance distance = 0;
      {
        TVertex controls[4];

        // Initialize beginning with duplicated start point (for ease in + out)
        controls[0] = *first;

        // If there is only one point, the length is 0
        ++first;
        if(first == onePastLast) {
          return 0;
        } else { // Two points or more, measure intermediate segments
          controls[1] = controls[0];
          controls[2] = *first;

          ++first;
          while(first != onePastLast) {
            controls[3] = *first;
            distance += measure(controls);

            // Shift all control points one to the left so we can append the new one
            controls[0] = controls[1];
            controls[1] = controls[2];
            controls[2] = controls[3];

            ++first;
          }
        }

        // Measure the final segment with the last point duplicated (for ease in + out)
        controls[3] = controls[2];
        distance += measure(controls);
      }

      return distance;

    }

    /// <summary>Measures the length of the specified curve</summary>
    /// <param name="controls">Control points for the interpolaton curve</param>
    /// <returns>The approximate length of the curve</returns>
    /// <remarks>All contents of the <see cref="controls" /> array will be destroyed</remarks>
    private: static typename TVertexTraits::TDistance measure(TVertex controls[4]) {
      const std::size_t Resolution = 100; // Resolution per iterative measurement

      typename TVertexTraits::TDistance distance = 0;

      // Sample the curve in small increments to obtain an approximation of
      // its overall length.
      TVertex previous = controls[1];
      for(std::size_t index = 1; index <= Resolution; ++index) {
        typename TVertexTraits::TScalar t = (
          static_cast<typename TVertexTraits::TScalar>(index) /
          static_cast<typename TVertexTraits::TScalar>(Resolution)
        );

        TVertex temp[4] = { controls[0], controls[1], controls[2], controls[3] };
        TVertex current = interpolateCubic(temp, t);

        distance += TVertexTraits::DistanceBetween(previous, current);
        previous = current;
      }

      return distance;
    }

    /// <summary>Performs cubic interpolation over the specified vertices</summary>
    /// <param name="controls">Control points for the interpolaton curve</param>
    /// <param name="t">Interpolation time between the second and third vertex</param>
    /// <returns>The interpolation vertex</returns>
    /// <remarks>All contents of the <see cref="controls" /> array will be destroyed</remarks>
    private: static TVertex interpolateCubic(
      TVertex controls[4], typename TVertexTraits::TScalar t
    ) {
      // Formula:
      //
      //   TVertex a0 = c3 - c2 - c0 + c1;
      //   TVertex a1 = c0 - c1 - a0;
      //   TVertex a2 = c2 - c0;
      //   TVertex a3 = c1;
      //
      //   TScalar t2 = t * t;
      //   return (a0*t*t2 + a1*t2 + a2*t + a3);
      //

      // Calculate the intermediate terms (in-place)
      //
      //   3 - 2 - 0 + 1
      //   2 - 0
      //   0 - 1 - (3)
      //   1
      {
        TVertexTraits::Subtract(controls[3], controls[2]);
        TVertexTraits::Subtract(controls[3], controls[0]);
        TVertexTraits::Add(controls[3], controls[1]);

        TVertexTraits::Subtract(controls[2], controls[0]);

        TVertexTraits::Subtract(controls[0], controls[1]);
        TVertexTraits::Subtract(controls[0], controls[3]);
      }

      // Scale and sum them as demanded by the formula
      {
        typename TVertexTraits::TScalar t2 = t * t;
        TVertexTraits::Scale(controls[2], t); // Scale by T
        TVertexTraits::Scale(controls[0], t2); // Scale by T*T
        TVertexTraits::Scale(controls[3], t2 * t); // Scale by T*T*T

        TVertexTraits::Add(controls[0], controls[1]);
        TVertexTraits::Add(controls[0], controls[2]);
        TVertexTraits::Add(controls[0], controls[3]);
      }

      return controls[0];
    }

  };

  // ------------------------------------------------------------------------------------------- //

}}}} // namespace Nuclex::Geometry::Lines::Interpolators

#endif // NUCLEX_GEOMETRY_LINES_INTERPOLATORS_CUBICINTERPOLATOR_H