AppExperiments

using Leap.Unity.PhysicalInterfaces;
using Leap.Unity.RuntimeGizmos;
using System;
using UnityEngine;

namespace Leap.Unity.Animation {

  [Serializable]
  public struct PoseSplineSequence : IIndexable<HermitePoseSpline>,
                                     ISpline<Pose, Movement>,
                                     ISpline<Vector3, Vector3> {
    public HermitePoseSpline[] splines;
    public bool allowExtrapolation;

    public PoseSplineSequence(HermitePoseSpline[] splines,
                              bool allowExtrapolation = false) {
      this.splines = splines;
      this.allowExtrapolation = allowExtrapolation;
    }

    public HermitePoseSpline this[int idx] {
      get { return splines[idx]; }
    }

    public int Count { get { return splines.Length; } }

    public Pose PoseAt(float t) {
      Pose pose;
      Movement unusedMovement;
      PoseAndMovementAt(t, out pose, out unusedMovement);
      return pose;
    }

    public Movement MovementAt(float t) {
      Pose unusePose;
      Movement movement;
      PoseAndMovementAt(t, out unusePose, out movement);
      return movement;
    }

    public void PoseAndMovementAt(float t, out Pose pose, out Movement movement) {
      var minT = splines[0].minT;
      var maxT = splines[splines.Length - 1].maxT;

      pose = Pose.identity;
      movement = Movement.identity;

      var dt = 0f;
      Pose poseOrigin; Movement extrapMovement;
      if (t < minT) {
        if (allowExtrapolation) {
          splines[0].PoseAndMovementAt(minT, out poseOrigin, out extrapMovement);
          dt = t - minT;
          pose = poseOrigin.Integrated(extrapMovement, dt);
          movement = extrapMovement;
          return;
        }
        else {
          t = minT;
        }
      }
      else if (t > maxT) {
        if (allowExtrapolation) {
          splines[splines.Length - 1].PoseAndMovementAt(maxT, out poseOrigin, out extrapMovement);
          dt = t - maxT;
          pose = poseOrigin.Integrated(extrapMovement, dt);
          movement = extrapMovement;
          return;
        }
        else {
          t = maxT;
        }
      }

      foreach (var spline in splines) {
        if (t >= spline.minT && t <= spline.maxT) {
          pose = spline.PoseAt(t);
          movement = spline.MovementAt(t);
          return;
        }
      }

      Debug.LogError("PoseSplineSequence couldn't evaluate T: " + t);
    }

    #region ISpline<Pose, Movement>

    public float minT {
      get {
        if (splines == null || splines.Length == 0) {
          return 0;
        }
        else {
          return splines[0].minT;
        }
      }
    }
    
    public float maxT {
      get {
        if (splines == null || splines.Length == 0) {
          return 0;
        }
        else {
          return splines[splines.Length - 1].maxT;
        }
      }
    }

    public Pose ValueAt(float t) {
      return PoseAt(t);
    }

    public Movement DerivativeAt(float t) {
      return MovementAt(t);
    }

    public void ValueAndDerivativeAt(float t, out Pose value, out Movement deltaValuePerSec) {
      PoseAndMovementAt(t, out value, out deltaValuePerSec);
    }

    #endregion

    #region ISpline<Vector3, Vector3>
    
    float ISpline<Vector3, Vector3>.minT { get { return minT; } }

    float ISpline<Vector3, Vector3>.maxT { get { return maxT; } }

    Vector3 ISpline<Vector3, Vector3>.ValueAt(float t) {
      return PoseAt(t).position;
    }

    Vector3 ISpline<Vector3, Vector3>.DerivativeAt(float t) {
      return MovementAt(t).velocity;
    }

    void ISpline<Vector3, Vector3>.ValueAndDerivativeAt(float t,
                                                        out Vector3 value,
                                                        out Vector3 deltaValuePerT) {
      Pose pose;
      Movement movement;
      PoseAndMovementAt(t, out pose, out movement);

      value = pose.position;
      deltaValuePerT = movement.velocity;
    }

    #endregion

  }

  public static class PoseSplineSequenceExtensions {
    public static void DrawPoseSplineSequence(this RuntimeGizmoDrawer drawer,
                                              PoseSplineSequence poseSplines,
                                              bool drawPoses = true,
                                              bool drawSegments = true) {
      for (int i = 0; i < poseSplines.Count; i++) {
        drawer.DrawPoseSpline(poseSplines[i],
                              drawPoses: drawPoses,
                              drawSegments: drawSegments);
      }
    }
  }

}