using System;

using UnityEngine;

namespace Framework.Actors {

  /// <summary>Checks whether an actor is grounded</summary>
  public class GroundChecker {

    /// <summary>Maximum number of colliders the actor can touch at the same time</summary>
    /// <remarks>
    ///   If this number is exceeded, undefined behavior results.
    /// </remarks>
    private const int MaximumIntersectionCount = 16;

    /// <summary>
    ///   Normalized angle around the bottom of the capsule/sphere colliders that is considered
    ///   a ground collision. A value of 1.0 would count collisions with the sides as grounded,
    ///   a value approaching 0.0 would only allow for a very tight spot on the bottom to count
    ///   as grounded (with the potential to cause issues if the actor is standing on a slope).
    /// </summary>
    private const float CapsuleAndSphereGroundDot = 0.5f;

    /// <summary>Padding used around any collision checks</summary>
    /// <remarks>
    ///   Due to the limits of floating point accuracy, the physics engine or Unity's
    ///   character controller may leave the actor slightly above the ground, thus resulting
    ///   in an unreliable ground contact state. The <see cref="SkinWidth" /> is added to
    ///   the size of the actor's collider when checking its grounded state.
    /// </remarks>
    public float SkinWidth = 0.03f;

    /// <summary>Vector that is downwards relative to the actor</summary>
    /// <remarks>
    ///   This only needs to be touched if your game does crazy things with gravity,
    ///   such as having characters walk on walls :)
    /// </remarks>
    public Vector3 Down = Vector3.down;

    /// <summary>Initialized a new collision tracker</summary>
    public GroundChecker() {
      this.intersectingColliders = new Collider[MaximumIntersectionCount];
    }

    /// <summary>Checks if a sphere collider is grounded</summary>
    /// <param name="collider">Collider that will be checked</param>
    /// <param name="layerMask">Layers against which the sphere collider will be checked</param>
    /// <returns>True if the sphere collider is grounded</returns>
    public bool CheckIfGrounded(Collider collider, int layerMask) {
      var characterController = collider as CharacterController;
      if(characterController != null) {
        return CheckIfGrounded(characterController, layerMask);
      }

      var capsuleCollider = collider as CapsuleCollider;
      if(capsuleCollider != null) {
        return CheckIfGrounded(capsuleCollider, layerMask);
      }

      var sphereCollider = collider as SphereCollider;
      if(sphereCollider != null) {
        return CheckIfGrounded(sphereCollider, layerMask);
      }

      var boxCollider = collider as BoxCollider;
      if(boxCollider != null) {
        return CheckIfGrounded(boxCollider, layerMask);
      }

      throw new ArgumentException("Unsupport collider type in grounded check", "collider");
    }

    /// <summary>Checks if a sphere collider is grounded</summary>
    /// <param name="sphereCollider">Sphere collider that will be checked</param>
    /// <param name="layerMask">Layers against which the sphere collider will be checked</param>
    /// <returns>True if the sphere collider is grounded</returns>
    public bool CheckIfGrounded(SphereCollider sphereCollider, int layerMask) {
      return CheckIfSphereGrounded(
        sphereCollider.transform.position + sphereCollider.center,
        sphereCollider.radius,
        layerMask
      );
    }

    /// <summary>Checks if a capsule collider is grounded</summary>
    /// <param name="capsuleCollider">Capsule collider that will be checked</param>
    /// <param name="layerMask">Layers against which the capsule collider will be checked</param>
    /// <returns>True if the capsule collider is grounded</returns>
    public bool CheckIfGrounded(CapsuleCollider capsuleCollider, int layerMask) {
      Vector3 sphereCenter;
      float sphereRadius;
      {
        Transform capsuleColliderTransform = capsuleCollider.transform;

        Vector3 downVector = capsuleColliderTransform.rotation * Vector3.down;
        sphereRadius = capsuleCollider.radius;

        float bottomSphereOffset = (capsuleCollider.height / 2.0f) - sphereRadius;
        sphereCenter = (
          capsuleColliderTransform.position +
          capsuleCollider.center +
          (bottomSphereOffset * downVector)
        );
      }

      return CheckIfSphereGrounded(sphereCenter, sphereRadius, layerMask);
    }

    /// <summary>Checks if a box collider is grounded</summary>
    /// <param name="boxCollider">Box collider that will be checked</param>
    /// <param name="layerMask">Layers against which the box collider will be checked</param>
    /// <returns>True if the box collider is grounded</returns>
    public bool CheckIfGrounded(BoxCollider boxCollider, int layerMask) {
      return CheckIfBoxGrounded(
        boxCollider.transform.rotation,
        boxCollider.transform.position + boxCollider.center,
        boxCollider.size / 2.0f,
        layerMask
      );
    }

    /// <summary>Checks if the actor is grounded</summary>
    /// <param name="actorPhysics">Actor that will be checked</param>
    /// <returns>True if the actor is grounded, false otherwise</returns>
    public bool CheckIfGrounded(ActorPhysics actorPhysics) {
      return CheckIfGrounded(actorPhysics.CharacterController, actorPhysics.SolidLayerMask);
    }

    /// <summary>Checks if a character is grounded</summary>
    /// <param name="characterController">Character controller that will be checked</param>
    /// <param name="layerMask">Layers against which the character will be checked</param>
    /// <returns>True if the character is grounded, false otherwise</returns>
    /// <remarks>
    ///   This figures out where the bottom sphere of the character controller's
    ///   capsule collider is, enlarges it minimally and then looks for any colliders
    ///   that are touching its bottom side (in a 60 degree cone).
    /// </remarks>
    public bool CheckIfGrounded(CharacterController characterController, int layerMask) {
      Vector3 sphereCenter;
      float sphereRadius;
      {
        Transform characterControllerTransform = characterController.transform;

        Vector3 downVector = characterControllerTransform.rotation * Vector3.down;
        sphereRadius = characterController.radius;

        float bottomSphereOffset = (characterController.height / 2.0f) - sphereRadius;
        sphereCenter = (
          characterControllerTransform.position +
          characterController.center +
          (bottomSphereOffset * downVector)
        );
      }

      return CheckIfSphereGrounded(sphereCenter, sphereRadius, layerMask);
    }

    /// <summary>Checks if a box is grounded</summary>
    /// <param name="orientation">Orientation of the box</param>
    /// <param name="center">Center of the box</param>
    /// <param name="extents">Extents of the box (half of its side lengths / size)</param>
    /// <param name="layerMask">Lyers on which to check for collisions</param>
    /// <returns>True if the box is touching the ground, false otherwise</returns>
    public bool CheckIfBoxGrounded(
      Quaternion orientation, Vector3 center, Vector3 extents, int layerMask
    ) {
      this.intersectingColliderCount = Physics.OverlapBoxNonAlloc(
        center, extents + (Vector3.one * this.SkinWidth),
        this.intersectingColliders,
        orientation,
        layerMask, QueryTriggerInteraction.Ignore
      );
      for(int intersectionIndex = 0; intersectionIndex < this.intersectingColliderCount;) {
        Vector3 closest = (
          this.intersectingColliders[intersectionIndex].ClosestPoint(center)
        );
        closest -= center;

        // Get the contact point as an interval along a line going through the center
        // of the box towards its bottom
        float y = Vector2.Dot(closest, this.Down);

        // If the collider is being touched by the box' lower half, we're grounded.
        if(y > this.SkinWidth) {
          ++intersectionIndex;
        } else { // Contact is not on the bottom half of the box, discard this intersection
          this.intersectingColliders[intersectionIndex] = (
            this.intersectingColliders[this.intersectingColliderCount - 1]
          );
          --this.intersectingColliderCount;
        }
      }

      // We're grounded if one or more colliders are touching the box' bottom
      return (this.intersectingColliderCount > 0);
    }

    /// <summary>Checks if a sphere is grounded</summary>
    /// <param name="center">Center of the sphere</param>
    /// <param name="radius">Radius of the sphere</param>
    /// <param name="layerMask">Lyers on which to check for collisions</param>
    /// <returns>True if the box is touching the ground, false otherwise</returns>
    public bool CheckIfSphereGrounded(Vector3 center, float radius, int layerMask) {
      this.intersectingColliderCount = Physics.OverlapSphereNonAlloc(
        center, radius + this.SkinWidth,
        this.intersectingColliders,
        layerMask, QueryTriggerInteraction.Ignore
      );
      for(int intersectionIndex = 0; intersectionIndex < this.intersectingColliderCount;) {
        Vector3 closest;
        {
          // Wasting a lot of CPU power here doing dynamic casts :-(
          // Terrain and mesh collisions aren't trivial and can have multiple answers,
          // but forcing the user to write special case code still sucks.
          var terrainCollider = this.intersectingColliders[intersectionIndex] as TerrainCollider;
          if(terrainCollider != null) {
            closest = center;
          } else {
            var meshCollider = this.intersectingColliders[intersectionIndex] as MeshCollider;
            if(meshCollider != null) {
              closest = center;
            } else {
              closest = (
                this.intersectingColliders[intersectionIndex].ClosestPoint(center)
              );
            }
          }
          closest -= center;
        }

        // If we're inside the collider, we're grounded. Oh well.
        if(closest.sqrMagnitude < this.SkinWidth * this.SkinWidth) {
          ++intersectionIndex;
          continue;
        }

        // If the sphere's bottom is in contact, consider this collider
        float dot = Vector3.Dot(this.Down, Vector3.Normalize(closest));
        if(dot > CapsuleAndSphereGroundDot) {
          ++intersectionIndex;
        } else { // Contact is not on the bottom of the sphere, discard this intersection
          this.intersectingColliders[intersectionIndex] = (
            this.intersectingColliders[this.intersectingColliderCount - 1]
          );
          --this.intersectingColliderCount;
        }
      }

      // We're grounded if one or more colliders are touching the sphere's bottom
      return (this.intersectingColliderCount > 0);
    }

    /// <summary>
    ///   Colliders the actor was intersecting with at the most recent call to one of
    ///   the Check...() methods
    /// </summary>
    public ArraySegment<Collider> IntersectingCollidersFromMostRecentCheck {
      get {
        return new ArraySegment<Collider>(
          this.intersectingColliders, 0, this.intersectingColliderCount
        );
      }
    }

    /// <summary>Whether the actor was grounded during the most recent check</summary>
    public bool WasGroundedInMostRecentCheck {
      get { return (this.intersectingColliderCount > 0); }
    }

    /// <summary>Checks the velocity of the ground the actor is standing upon</summary>
    /// <returns>The velocity of the ground the actor is standing upon</returns>
    public Vector3 GetMostRecentGroundVelocity() {
      Vector3 result = Vector3.zero;

      // Find the intersecting collider which has the highest upwards velocity
      float? highestUpwardVelocity = null;
      for(int index = 0; index < this.intersectingColliderCount; ++index) {
        Vector3 velocity = getColliderVelocity(this.intersectingColliders[index]);

        float y = Vector2.Dot(velocity, this.Down);
        if(highestUpwardVelocity.HasValue) {
          if(y < highestUpwardVelocity.Value) { // Check: Shouldn't this be greater-than?
            highestUpwardVelocity = y;
            result = velocity;
          }
        } else {
          highestUpwardVelocity = y;
          result = velocity;
        }
      }

      return result;
    }

    /// <summary>Determines the velocity of a collider</summary>
    /// <param name="collider">Collider whose velocity will be determined</param>
    /// <returns>The velocity of the collider</returns>
    private static Vector3 getColliderVelocity(Collider collider) {

      // If it has a Rigidbody component or is the child of a Rigidbody, return that velocity
      Rigidbody rigidbody = collider.GetComponentInParent<Rigidbody>();
      if(rigidbody != null) {
        return rigidbody.velocity;
      }

      // If it has a component implementing IVelocityProvider or is the child of such
      // a game object (complex moving geometry), return that velocity
      IVelocityProvider velocityProvider = collider.GetComponentInParent<IVelocityProvider>();
      if(velocityProvider != null) {
        return velocityProvider.Velocity;
      }

      // No physics component means its velocity is zero (and not undefined!)
      return Vector3.zero;

    }

    /// <summary>Reused array to store colliders the actor is intersecting with</summary>
    private Collider[/*MaximumIntersectionCount*/] intersectingColliders;
    /// <summary>Number of intersecting colliders</summary>
    private int intersectingColliderCount;

  }

} // namespace Framework.Actors