using System;
using System.Collections.Generic;

using UnityEngine;

namespace Framework.Geometry.Volumes.Fitting {

  /// <summary>Fits 3D planes to meshes</summary>
  public static class Plane3Fitter {

    #region enum VectorComponent

    /// <summary>Individual comopnents of a 3D vector</summary>
    public enum VectorComponent {

      /// <summary>The X coordinate of a vector</summary>
      X,
      /// <summary>The Y coordinate of a vector</summary>
      Y,
      /// <summary>The Z coordinate of a vector</summary>
      Z

    }

    #endregion // enum VectorComponent

    /// <summary>Fits an axis aligned plane to a set of points</summary>
    /// <param name="points">Points to which the axis aligned plane will be fitted</param>
    /// <param name="axis">Axis of the plane's normal vector</param>
    /// <param name="maximumOutlierDelta">
    ///   Distance beyond which outliers will be incrementally eliminated
    /// </param>
    /// <returns>The offset of the plane on the specified axis</returns>
    public static float FitAxisAlignedPlane(
      IList<Vector3> points, VectorComponent axis, float maximumOutlierDelta = 0.01f
    ) {

      // Extract the component on which the plane should be fitted.
      // The other vector components quite simply don't matter :)
      ArraySegment<float> sortedValues;
      {
        float[] values = demuxVectorComponent(points, axis);
        Array.Sort(values);
        sortedValues = new ArraySegment<float>(values, 0, values.Length);
      }

      return findHighestDensity(sortedValues, maximumOutlierDelta);

    }

    /// <summary>Finds the highest density of values within the specified values</summary>
    /// <param name="sortedValues">
    ///   Values of which the point of highest density will be found
    /// </param>
    /// <param name="maximumOutlierDelta">
    ///   Distance beyond which outliers will be incrementally eliminated
    /// </param>
    /// <returns>The point of highest density</returns>
    private static float findHighestDensity(
      ArraySegment<float> sortedValues, float maximumOutlierDelta = 0.01f
    ) {
      int startIndex = sortedValues.Offset;
      int endIndex = startIndex + sortedValues.Count - 1;

      // Calculate the sum of all remaining values. We will use this to find the mean
      // of the values so we can see which outliers to eliminate.
      float sum = sortedValues.Array[startIndex];
      for(int index = startIndex + 1; index <= endIndex; ++index) {
        sum += sortedValues.Array[index];
      }

      // Now remove outliers until all of the remaining points are within the maximum
      // outlier distance, then return the median of those points.
      float median;
      for(;;) {
        median = sum / (endIndex - startIndex + 1);

        // Each iteration, eliminate the furthest point. We need to go point-by-point
        // because if enough points are outliers, this would move the median in
        // the first iterations so far that the point of highest density would be
        // an outlier, too.
        float leftDistance = Math.Abs(median - sortedValues.Array[startIndex]);
        float rightDistance = Math.Abs(sortedValues.Array[endIndex] - median);

        if(leftDistance > rightDistance) {
          if(leftDistance < maximumOutlierDelta) {
            break;
          }

          sum -= sortedValues.Array[startIndex];
          ++startIndex;
        } else {
          if(rightDistance < maximumOutlierDelta) {
            break;
          }

          sum -= sortedValues.Array[endIndex];
          --endIndex;
        }
      }

      //Debug.Log("Median averaged on " + (endIndex - startIndex + 1) + " samples.");
      return median;
    }

#if false
    /// <summary>
    ///   Removes elements from a sorted list until the remaining values fall within
    ///   a specified range
    /// </summary>
    /// <param name="sortedValues">List from which elements will be removed</param>
    /// <param name="min">Minimum value to keep in the list (inclusive)</param>
    /// <param name="max">Maximum value to keep in the list (exclusive)</param>
    private static void Trim(
      ref ArraySegment<float> sortedValues, float min, float max
    ) {
      int startIndex = sortedValues.Offset;
      int endIndex = startIndex + sortedValues.Count - 1;

      // Remove values that are smaller than the minimum value
      while(sortedValues.Array[startIndex] < min) {
        ++startIndex;
        if(startIndex > endIndex) {
          sortedValues = new ArraySegment<float>(sortedValues.Array, sortedValues.Offset, 0);
          return;
        }
      }

      // Remove values that are larger than the maximum value
      while(sortedValues.Array[endIndex] >= max) {
        --endIndex;
        if(endIndex < startIndex) {
          sortedValues = new ArraySegment<float>(sortedValues.Array, sortedValues.Offset, 0);
          return;
        }
      }

      sortedValues = new ArraySegment<float>(
        sortedValues.Array, startIndex, (endIndex - startIndex + 1)
      );
    }
#endif

    /// <summary>Demuxes an individual component from a set of vectors</summary>
    /// <param name="vectors">Vector that will have one of their components extracted</param>
    /// <param name="component">Vector component that will be demuxed</param>
    /// <returns>An array of all the values of the specified vector component</returns>
    private static float[] demuxVectorComponent(
      IList<Vector3> vectors, VectorComponent component
    ) {
      int vectorCount = vectors.Count;

      var values = new float[vectorCount];
      switch(component) {
        case VectorComponent.X: {
          for(int index = 0; index < vectorCount; ++index) {
            values[index] = vectors[index].x;
          }
          break;
        }
        case VectorComponent.Y: {
          for(int index = 0; index < vectorCount; ++index) {
            values[index] = vectors[index].y;
          }
          break;
        }
        case VectorComponent.Z: {
          for(int index = 0; index < vectorCount; ++index) {
            values[index] = vectors[index].z;
          }
          break;
        }
        default: {
          throw new ArgumentException("Unsupported vector component index", "component");
        }
      }

      return values;
    }

  }

} // namespace Framework.Geometry.Volumes.Fitting