using System;

using UnityEngine;

using Debug = System.Diagnostics.Debug;

namespace Framework.Support {

  /// <summary>Contains helper methods for dealing with IK problems</summary>
  public static class InverseKinematicsHelper {

    /// <summary>
    ///   Calculates the angle of two bones so the second bone ends at a desired location
    /// </summary>
    /// <param name="nearBone">Near bone in the the IK chain</param>
    /// <param name="farBone">Far bone in the IK chain</param>
    /// <param name="endBone">Bone to which the far bone is connected</param>
    /// <param name="targetPosition">
    ///   Position at the <paramref name="endBone" /> should be placed at
    /// </param>
    /// <param name="bendNormal">Normal of the 2D plane on which the bending takes place</param>
    /// <param name="weight">Factor by how much the IK should be applied (1.0 = full)</param>
    /// <remarks>>
    ///   Code by anonymous pastebin contributor.
    /// </remarks>
    public static void SolveTwoBoneIK(
      Transform nearBone, Transform farBone, Transform endBone,
      Vector3 targetPosition, Vector3 bendNormal, float weight
    ) {
      if(weight <= 0.0f) {
        return;
      }

      // Direction of the limb in solver
      targetPosition = Vector3.Lerp(endBone.position, targetPosition, weight);

      Vector3 dir = targetPosition - nearBone.position;

      // Distance between the first and the last node solver positions
      float length = dir.magnitude;
      if(length == 0.0f) {
        return;
      }

      float sqrMag1 = (farBone.position - nearBone.position).sqrMagnitude;
      float sqrMag2 = (endBone.position - farBone.position).sqrMagnitude;

      // Get the general world space bending direction
      Vector3 bendDir = Vector3.Cross(dir, bendNormal);

      // Get the direction to the trigonometrically solved position of the second node
      Vector3 toBendPoint = getDirectionToBendPoint(dir, length, bendDir, sqrMag1, sqrMag2);

      // Position the second node
      Quaternion q1 = Quaternion.FromToRotation(
        farBone.position - nearBone.position, toBendPoint
      );
      if(weight < 1.0f) {
        q1 = Quaternion.Lerp(Quaternion.identity, q1, weight);
      }

      nearBone.rotation = q1 * nearBone.rotation;

      Quaternion q2 = Quaternion.FromToRotation(
        endBone.position - farBone.position, targetPosition - farBone.position
      );
      if (weight < 1.0f) q2 = Quaternion.Lerp(Quaternion.identity, q2, weight);

      farBone.rotation = q2 * farBone.rotation;
    }

    /// <summary>
    ///   Calculates the angle of two bones so the second bone ends at a desired location
    /// </summary>
    /// <param name="solvePosAngle2">
    ///   Solve for positive angle 2 instead of negative angle 2?
    /// </param>
    /// <param name="length1">Length of bone 1. Assume to be >= zero</param>
    /// <param name="length2">Length of bone 2. Assume to be >= zero</param>
    /// <param name="target">Target position for the bones to reach</param>
    /// <param name="angle1">Receives the target angle of bone 1</param>
    /// <param name="angle2">Receives the target angle of bone 2</param>
    /// <returns>True when a valid solution was found</returns>
    /// <remarks>
    ///   <para>
    ///     Given a two bone chain located at the origin (bone1 is the parent of bone2), this
    ///     function will compute the bone angles needed for the end of the chain to line up
    ///     with a target position. If there is no valid solution, the angles will be set to
    ///     get as close to the target as possible.
    ///   </para>
    ///   <para>
    ///     Algorithm by Ryan Juckett
    ///   </para>
    ///   <para>
    ///     Copyright (c) 2008-2009 Ryan Juckett
    ///     http://www.ryanjuckett.com/
    ///   </para>
    ///   <para>
    ///     This software is provided 'as-is', without any express or implied
    ///     warranty. In no event will the authors be held liable for any damages
    ///     arising from the use of this software.
    ///   </para>
    ///   <para>
    ///     Permission is granted to anyone to use this software for any purpose,
    ///     including commercial applications, and to alter it and redistribute it
    ///     freely, subject to the following restrictions:
    ///   </para>
    ///   <para>
    ///     1. The origin of this software must not be misrepresented; you must not
    ///     claim that you wrote the original software. If you use this software
    ///     in a product, an acknowledgment in the product documentation would be
    ///     appreciated but is not required.
    ///   </para>
    ///   <para>
    ///     2. Altered source versions must be plainly marked as such, and must not be
    ///     misrepresented as being the original software.
    ///   </para>
    ///   <para>
    ///     3. This notice may not be removed or altered from any source
    ///     distribution.
    ///   </para>
    /// </remarks>
    public static bool SolveTwoBoneIK(
      bool solvePosAngle2,
      double length1, double length2,
      Vector2 target,
      out double angle1, out double angle2
    ) {
      Debug.Assert(length1 >= 0);
      Debug.Assert(length2 >= 0);

      const double epsilon = 0.0001; // used to prevent division by small numbers

      bool foundValidSolution = true;

      double targetDistSqr = (target.x * target.x + target.y * target.y);

      // Compute a new value for angle2 along with its cosine
      double sinAngle2, cosAngle2;

      double cosAngle2_denom = 2.0 * length1 * length2;
      if(cosAngle2_denom > epsilon) {

        cosAngle2 = (targetDistSqr - length1 * length1 - length2 * length2) / (cosAngle2_denom);

        // If our result is not in the legal cosine range, we can not find a
        // legal solution for the target
        if((cosAngle2 < -1.0) || (cosAngle2 > 1.0)) {
          // Check: Could this be solved by stretching the bones?
          foundValidSolution = false;
        }

        // Clamp our value into range so we can calculate the best
        // solution when there are no valid ones
        cosAngle2 = Math.Max(-1, Math.Min( 1, cosAngle2));

        // Second bone angle
        angle2 = Math.Acos( cosAngle2 );
        if(!solvePosAngle2) {
          angle2 = -angle2;
        }
        sinAngle2 = Math.Sin( angle2 );

      } else {
        // At least one of the bones had a zero length. This means our
        // solvable domain is a circle around the origin with a radius
        // equal to the sum of our bone lengths.
        double totalLenSqr = (length1 + length2) * (length1 + length2);
        if(
          targetDistSqr < (totalLenSqr-epsilon) ||
          targetDistSqr > (totalLenSqr+epsilon)
        ) {
          foundValidSolution = false;
        }

        // Only the value of angle1 matters at this point. We can just
        // set angle2 to zero.
        angle2    = 0.0;
        cosAngle2 = 1.0;
        sinAngle2 = 0.0;
      }

      //===
      // Compute the value of angle1 based on the sine and cosine of angle2
      double triAdjacent = length1 + length2 * cosAngle2;
      double triOpposite = length2 * sinAngle2;

      double tanY = target.y * triAdjacent - target.x * triOpposite;
      double tanX = target.x * triAdjacent + target.y * triOpposite;

      // Note that it is safe to call Atan2(0,0) which will happen if targetX and
      // targetY are zero
      angle1 = Math.Atan2(tanY, tanX);

      return foundValidSolution;
    }

    /// <summary>Calculates the bend direction based on the law of cosines</summary>
    /// <param name="direction">Direction from base of joint to end of second limb</param>
    /// <param name="directionMag">Distance from joint base to limb end</param>
    /// <param name="bendDirection">Normal vector of the plane on which the joint rotates</param>
    /// <param name="sqrMag1">Length of segment before joint</param>
    /// <param name="sqrMag2">Length of segment after joint</param>
    /// <returns>The bend direction for the joint</returns>
    /// <remarks>
    ///   Magnitude of the returned vector does not equal to the length of the first bone!
    /// </remarks>
    private static Vector3 getDirectionToBendPoint(
      Vector3 direction, float directionMag, Vector3 bendDirection, float sqrMag1, float sqrMag2
    ) {
      float x = ((directionMag * directionMag) + (sqrMag1 - sqrMag2)) / 2.0f / directionMag;
      float y = (float)Math.Sqrt(Mathf.Clamp(sqrMag1 - x * x, 0.0f, Mathf.Infinity));

      if(direction == Vector3.zero) {
        return Vector3.zero;
      }

      return Quaternion.LookRotation(direction, bendDirection) * new Vector3(0.0f, y, x);
    }
    
  }

} // namespace Framework.Support