#region License
// Easy Jiggle Bone
//
// Authors: Markus Ewald (Cygon), Michael Cook (Fishypants), Rodrigo Pegorari
// License: Free to use. Any credit would be nice :)
// Upodate: 2013-06-01 (ymd)
#endregion // License

//#define HAVE_DAMN_MATH_PROBLEM_SOLVED

using System;

using UnityEngine;

namespace Framework.Support {

  /// <summary>Simulates a wobbly, bouncy or jiggly part of a rigid body</summary>
  /// <remarks>
  ///   <para>
  ///     This is a modified version of the jiggle bone script which can handle any
  ///     arbitrary rotation of the bone to be jiggled. This is particularly useful if
  ///     you use one of the modeling tools where the asset import causes the bones'
  ///     neutral orientations to end up being weird angles.
  ///   </para>
  ///   <para>
  ///     How to use (example of breast physics):
  ///   </para>
  ///   <para>
  ///     Preparations: your character model should have one bone per breast and
  ///     ideally your game would be in a stage that already lets you move around.
  ///     In the initial pose (the one the jiggle bone script will find when
  ///     it activates) the breast bones should be in the "rest position" to which
  ///     they will return when the model stops moving.
  ///   </para>
  ///   <para>
  ///     <list type="number">
  ///       <item>
  ///         <!-- Step 1 -->
  ///         Select the character model in the Hierarchy window, go down the bone
  ///         tree to locate the breast bones. Attach this jiggle bone script to each
  ///         of them.
  ///       </item>
  ///       <item>
  ///         <!-- Step 2 -->
  ///         The jiggle bone script has a property "Mass Offset" that specifies where,
  ///         relative to the jiggle bone's base, the center of mass is located. The
  ///         scene editor will show this point as a small green sphere with a line.
  ///         Tweak the "Mass Offset" until the green line goes right through the center
  ///         of mass. Actually, make it a bit longer to achieve a more stable simulation.
  ///       </item>
  ///       <item>
  ///         <!-- Step 3 -->
  ///         Check the results. If during acceleration, the blue sphere gets left behind
  ///         and twists the breasts into extreme angles, move the "Mass Offset"
  ///         a bit further out. If it doesn't settle quickly enough, increase the
  ///         stiffness. If it moves too slowly, decrease the mass.
  ///       </item>
  ///       <item>
  ///         <!-- Step 4 -->
  ///         (Additional step required until I can solve a silly math issue).
  ///         The bone will currently end up with a rather arbitrary roll angle. To fix
  ///         this, click on "Play" and tweak the RestAngle into all possible directions
  ///         until the hook on the green line points into the same direction as
  ///         the hook on the red line. Remember the RestAngle you used, click on "Stop"
  ///         and re-enter it (as Unity will reset any changes performed during playback).
  ///       </item>
  ///     </list>
  ///   </para>
  /// </remarks>
  public class JiggleBone : MonoBehaviour {

    /// <summary>Offset the center of mass has from the bone</summary>
    /// <remarks>
    ///   <para>
    ///     If the jiggle bone is, for example, used for breast physics, the breast bones
    ///     would typically be somewhere inside of the breasts and have arbitrary
    ///     orientations (depending on the modeling application used) that cannot be relied
    ///     on to estimate the bounce in response to object translation changes.
    ///   </para>
    ///   <para>
    ///     The mass offset specifies the offset that of the center of mass has from
    ///     the origin of the bone.
    ///   </para>
    /// </remarks>
    public Vector3 MassOffset = Vector3.forward;

    /// <summary>Maximum angle the jiggle bone can bend</summary>
    public float AngleLimit = 45.0f;

    /// <summary>How heavy the jiggling mass is</summary>
    public float Mass = 0.90f;

    /// <summary>How strongly the jiggling mass follows the jiggle bone</summary>
    public float Stiffness = 0.15f;

    /// <summary>How much the mass resists movements</summary>
    public float Damping = 0.75f;

    /// <summary>Whether gravity influences the jiggle bone</summary>
    public bool ApplyGravity = false;

    /// <summary>
    ///   If this jiggle bone might be turned 90 degrees, fixes an unexplained error
    /// </summary>
    public bool EnableCrawlingHack = false;

#if !HAVE_DAMN_MATH_PROBLEM_SOLVED
    /// <summary>Direction at which the jiggle bone has no roll</summary>
    public Vector3 RestAngle = Vector3.left;
#endif

    /// <summary>
    ///   Stops any jiggling immediately. Use after warping to another position.
    /// </summary>
    public void Stabilize() {
      this.jiggleOrientation = transform.parent.rotation * this.originalOrientation;
      this.dynamicPosition = massRestPosition;
      this.dynamicVelocity = Vector3.zero;
    }

    /// <summary>Called when the script gets loaded into a game component</summary>
    protected virtual void Awake() {
      this.originalOrientation = transform.localRotation;

#if HAVE_DAMN_MATH_PROBLEM_SOLVED
      this.up = getOrthogonalUp(Vector3.Normalize(this.MassOffset));
#endif

      // Because we allow for an arbitrary initial rotation of the jiggle bone,
      // we will need to transform all effects that are based on the center of mass
      // by the jiggle bone's orientation so that the rest position equals the bind
      // pose and impulses calculated on the center of mass can be transformed into
      // this coordinate frame.
      this.massToOriginalOrientation = Quaternion.Inverse(
        massRestOrientation
      ) * this.originalOrientation;
      this.originalToMassOrientation = Quaternion.Inverse(
        this.massToOriginalOrientation
      );

      Stabilize();
    }

    /// <summary>Called after Unity has finished its physics processing</summary>
    protected virtual void LateUpdate() {
      updateDynamicPosition(massRestPosition);

      // Orient the jiggle bone towards the dynamic point
      Vector3 dynamicDirection = Vector3.Normalize(this.dynamicPosition - transform.position);

      // HACK LookRotation uses up vector in global coordinate system
      //   The up vector should be transformed into the local coordinate frame
      //   so that no matter which way the model is facing, up stays up. Due to
      //   acute brain meltdown, hardcoded standing and crawling orientations here.
      Vector3 bestUp;
      {
        float deltaX = massRestPosition.x - transform.position.x;
        float deltaY = massRestPosition.y - transform.position.y;
        bool isProbablyCrawling = Math.Abs(deltaY) > Math.Abs(deltaX);
        if(isProbablyCrawling && this.EnableCrawlingHack) {
          bestUp = new Vector3(-Mathf.Sign(deltaX), 0.0f, 0.0f);
        } else {
          bestUp = Vector3.up;
        }
      }

      this.jiggleOrientation = Quaternion.LookRotation(dynamicDirection, bestUp);
      this.jiggleOrientation *= this.massToOriginalOrientation;

      applyJiggleOrientation();
    }

    /// <summary>Simulates the dynamic point towards which the jiggle bone is oriented</summary>
    /// <param name="target">Rest position that the dynamic point tries to reach</param>
    /// <remarks>
    ///   Formula based on the original Unity jiggle bone by Michael Cook, which in turn
    ///   uses the math from Rodrigo Pegorari's rubber simulation script.
    /// </remarks>
    private void updateDynamicPosition(Vector3 target) {
      Vector3 force = (target - this.dynamicPosition) * this.Stiffness;
      if(this.ApplyGravity) {
        force += Physics.gravity / 1000.0f;
      }
      Vector3 acceleration = force / this.Mass;
      dynamicVelocity += acceleration * (1.0f - this.Damping);

      this.dynamicPosition += dynamicVelocity + force;
    }

    /// <summary>
    ///   Draws some debug gizmos when the component is shown in the scene editor
    /// </summary>
    protected virtual void OnDrawGizmos() {
      float massOffsetLength = this.MassOffset.magnitude;

      // Red marker to the jiggle bone's actual orientation
      {
        Vector3 jiggleForward =
          this.jiggleOrientation * this.originalToMassOrientation * Vector3.forward;
        jiggleForward *= massOffsetLength;

        Gizmos.color = Color.red;
        Gizmos.DrawLine(transform.position, transform.position + jiggleForward * 2.0f);
        Gizmos.DrawSphere(transform.position + jiggleForward, massOffsetLength / 25.0f);

        Vector3 jiggleUp =
          this.jiggleOrientation * this.originalToMassOrientation * Vector3.up;
        jiggleUp *= massOffsetLength / 5.0f;
        Gizmos.DrawLine(
          transform.position + jiggleForward * 2.0f,
          (transform.position + jiggleForward * 2.0f) + jiggleUp
        );
      }

      // Green marker to the rest position (the orientation the jiggle bone is chasing).
      {
        Vector3 restForward =
          transform.parent.rotation * massRestOrientation * Vector3.forward;
        restForward *= massOffsetLength;

        Gizmos.color = Color.green;
        Gizmos.DrawLine(transform.position, transform.position + restForward);
        Gizmos.DrawSphere(transform.position + restForward, massOffsetLength / 25.0f);

        Vector3 restUp =
          transform.parent.rotation * massRestOrientation * Vector3.up;
        restUp *= massOffsetLength / 5.0f;
        Gizmos.DrawLine(
          transform.position + restForward,
          (transform.position + restForward) + restUp
        );
      }

      // Blue marker for the dynamic position
      Gizmos.color = Color.cyan;
      Gizmos.DrawWireSphere(this.dynamicPosition, massOffsetLength / 10.0f);
    }

    /// <summary>Applies the jiggle orientation to the bone</summary>
    private void applyJiggleOrientation() {

      // The jiggle orientation is in the global coordinate frame, but the bone's
      // rotation is expressed relative to its parent, thus we need to transform
      // the jiggle orientation into the same coordinate frame.
      Quaternion parentOrientation = transform.parent.rotation;
      transform.localRotation = Quaternion.Inverse(parentOrientation) * this.jiggleOrientation;

    }

    /// <summary>
    ///   Orientation of the mass' rest position in the coordinate frame of the parent bone
    /// </summary>
    private Quaternion massRestOrientation {
      get {
        Vector3 massDirection = Vector3.Normalize(this.MassOffset);

        // HACK: Up vector of bone bind position points right for some reason :-((
        // I don't understand the problem sufficiently, the roll of the bone should
        // not matter (the massToOriginalOrientation should take care of that), but it
        // does. So now I'm using the left vector here...
#if HAVE_DAMN_MATH_PROBLEM_SOLVED
        return Quaternion.LookRotation(massDirection, this.up);
#else
        return Quaternion.LookRotation(massDirection, this.RestAngle);
#endif
      }
    }

    /// <summary>Rest position of the center of mass in world space</summary>
    private Vector3 massRestPosition {
      get {
        Vector3 forward = transform.parent.rotation * this.MassOffset;
        return transform.position + forward;
      }
    }

    /// <summary>Returns the orthogonal up vector for the provided direction</summary>
    /// <param name="direction">Direction of which the up vector will be found</param>
    /// <returns>The up vector for the provided direction</returns>
    private static Vector3 getOrthogonalUp(Vector3 direction) {
      switch(getLongestAxis(direction)) {
        case 0: { // X axis is longest
          if(direction.x > 0.0f) {
            return new Vector3(direction.z, direction.x, direction.y);
          } else {
            return new Vector3(-direction.z, -direction.x, -direction.y);
          }
        }
        case 1: { // Y axis is longest
          if(direction.y > 0.0f) {
            return new Vector3(direction.z, direction.x, direction.y);
          } else {
            return new Vector3(-direction.z, -direction.x, -direction.y);
          }
        }
        case 2: { // Z axis is longest
          if(direction.z > 0.0f) {
            return new Vector3(direction.y, direction.z, direction.x);
          } else {
            return new Vector3(-direction.y, -direction.z, -direction.x);
          }
        }
        default: {
          throw new InvalidOperationException("getLongestAxis() malfunctioned");
        }
      }
    }

    /// <summary>Returns the index of the vector's longest axis</summary>
    /// <param name="direction">Direction in which the longest axis will be found</param>
    /// <returns>The index of the longest axis in the vector</returns>
    private static int getLongestAxis(Vector3 direction) {
      float absX = Math.Abs(direction.x);
      float absY = Math.Abs(direction.y);
      float absZ = Math.Abs(direction.z);

      if(absX > absY) {
        if(absX > absZ) {
          return 0;
        } else {
          return 2;
        }
      } else {
        if(absY > absZ) {
          return 1;
        } else {
          return 2;
        }
      }
    }

    /// <summary>Transforms from the mass orientation to the jiggle bone orientation</summary>
    private Quaternion massToOriginalOrientation;
    /// <summary>Transforms from the jiggle bone orientation to the mass orientation</summary>
    private Quaternion originalToMassOrientation;

    /// <summary>Original orientation of the bone with the mass at its rest position</summary>
    /// <remarks>
    ///   This orientation is specified in the coordinate frame of the jiggle bone's parent
    ///   because otherwise, the original local rotation of the jiggle bone would be lost.
    /// </remarks>
    private Quaternion originalOrientation;

    /// <summary>Current orientation of the bone</summary>
    /// <remarks>
    ///   Specified in the global coordinate frame. This is so because when the character
    ///   rotates, the jiggle bone will not turn with it (only its stiffness cause it
    ///   to follow). The local rotation of the jiggle bone will be this orientation
    ///   transformed into the coordinate frame of the jiggle bone's parent.
    /// </remarks>
    private Quaternion jiggleOrientation;

#if HAVE_DAMN_MATH_PROBLEM_SOLVED
    /// <summary>Direction that is upwards to the mass offset</summary>
    private Vector3 up;
#endif
    /// <summary>Dynamic position towards which the simulated mass is oriented</summary>
    private Vector3 dynamicPosition;
    /// <summary>Velocity of the dynamic position</summary>
    private Vector3 dynamicVelocity;

  }

} // namespace Framework.Support