// MIT License
// 
// Copyright (c) 2017 Justin Larrabee <justonia@gmail.com>
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;

public static class PhysicsExtensions
{
    //
    // Box
    //

    public static bool BoxCast(BoxCollider box, Vector3 direction, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.BoxCast( center, halfExtents, direction, orientation, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static bool BoxCast(BoxCollider box, Vector3 direction, out RaycastHit hitInfo, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.BoxCast( center, halfExtents, direction, out hitInfo, orientation, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static RaycastHit[] BoxCastAll(BoxCollider box, Vector3 direction, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.BoxCastAll( center, halfExtents, direction, orientation, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static int BoxCastNonAlloc(BoxCollider box, Vector3 direction, RaycastHit[] results, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.BoxCastNonAlloc( center, halfExtents, direction, results, orientation, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static bool CheckBox(BoxCollider box, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.CheckBox( center, halfExtents, orientation, layerMask, queryTriggerInteraction );
    }

    public static Collider[] OverlapBox(BoxCollider box, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.OverlapBox( center, halfExtents, orientation, layerMask, queryTriggerInteraction );
    }

    public static int OverlapBoxNonAlloc(BoxCollider box, Collider[] results, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center, halfExtents;
        Quaternion orientation;
        box.ToWorldSpaceBox( out center, out halfExtents, out orientation );
        return Physics.OverlapBoxNonAlloc( center, halfExtents, results, orientation, layerMask, queryTriggerInteraction );
    }

    public static void ToWorldSpaceBox(this BoxCollider box, out Vector3 center, out Vector3 halfExtents, out Quaternion orientation)
    {
        orientation = box.transform.rotation;
        center = box.transform.TransformPoint( box.center );
        var lossyScale = box.transform.lossyScale;
        var scale = AbsVec3( lossyScale );
        halfExtents = Vector3.Scale( scale, box.size ) * 0.5f;
    }

    //
    // Sphere
    //

    public static bool SphereCast(SphereCollider sphere, Vector3 direction, out RaycastHit hitInfo, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.SphereCast( center, radius, direction, out hitInfo, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static RaycastHit[] SphereCastAll(SphereCollider sphere, Vector3 direction, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.SphereCastAll( center, radius, direction, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static int SphereCastNonAlloc(SphereCollider sphere, Vector3 direction, RaycastHit[] results, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.SphereCastNonAlloc( center, radius, direction, results, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static bool CheckSphere(SphereCollider sphere, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.CheckSphere( center, radius, layerMask, queryTriggerInteraction );
    }

    public static Collider[] OverlapSphere(SphereCollider sphere, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.OverlapSphere( center, radius, layerMask, queryTriggerInteraction );
    }

    public static int OverlapSphereNonAlloc(SphereCollider sphere, Collider[] results, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 center;
        float radius;
        sphere.ToWorldSpaceSphere( out center, out radius );
        return Physics.OverlapSphereNonAlloc( center, radius, results, layerMask, queryTriggerInteraction );
    }

    public static void ToWorldSpaceSphere(this SphereCollider sphere, out Vector3 center, out float radius)
    {
        center = sphere.transform.TransformPoint( sphere.center );
        radius = sphere.radius * MaxVec3( AbsVec3( sphere.transform.lossyScale ) );
    }

    //
    // Capsule
    //

    public static bool CapsuleCast(CapsuleCollider capsule, Vector3 direction, out RaycastHit hitInfo, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.CapsuleCast( point0, point1, radius, direction, out hitInfo, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static RaycastHit[] CapsuleCastAll(CapsuleCollider capsule, Vector3 direction, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.CapsuleCastAll( point0, point1, radius, direction, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static int CapsuleCastNonAlloc(CapsuleCollider capsule, Vector3 direction, RaycastHit[] results, float maxDistance = Mathf.Infinity, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.CapsuleCastNonAlloc( point0, point1, radius, direction, results, maxDistance, layerMask, queryTriggerInteraction );
    }

    public static bool CheckCapsule(CapsuleCollider capsule, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.CheckCapsule( point0, point1, radius, layerMask, queryTriggerInteraction );
    }

    public static Collider[] OverlapCapsule(CapsuleCollider capsule, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.OverlapCapsule( point0, point1, radius, layerMask, queryTriggerInteraction );
    }

    public static int OverlapCapsuleNonAlloc(CapsuleCollider capsule, Collider[] results, int layerMask = Physics.DefaultRaycastLayers, QueryTriggerInteraction queryTriggerInteraction = QueryTriggerInteraction.UseGlobal)
    {
        Vector3 point0, point1;
        float radius;
        capsule.ToWorldSpaceCapsule( out point0, out point1, out radius );
        return Physics.OverlapCapsuleNonAlloc( point0, point1, radius, results, layerMask, queryTriggerInteraction );
    }

    public static void ToWorldSpaceCapsule(this CapsuleCollider capsule, out Vector3 point0, out Vector3 point1, out float radius)
    {
        var center = capsule.transform.TransformPoint( capsule.center );
        radius = 0f;
        float height = 0f;
        Vector3 lossyScale = AbsVec3( capsule.transform.lossyScale );
        Vector3 dir = Vector3.zero;

        switch (capsule.direction)
        {
            case 0: // x
            radius = Mathf.Max( lossyScale.y, lossyScale.z ) * capsule.radius;
            height = lossyScale.x * capsule.height;
            dir = capsule.transform.TransformDirection( Vector3.right );
            break;
            case 1: // y
            radius = Mathf.Max( lossyScale.x, lossyScale.z ) * capsule.radius;
            height = lossyScale.y * capsule.height;
            dir = capsule.transform.TransformDirection( Vector3.up );
            break;
            case 2: // z
            radius = Mathf.Max( lossyScale.x, lossyScale.y ) * capsule.radius;
            height = lossyScale.z * capsule.height;
            dir = capsule.transform.TransformDirection( Vector3.forward );
            break;
        }

        if (height < radius * 2f)
        {
            dir = Vector3.zero;
        }

        point0 = center + dir * ( height * 0.5f - radius );
        point1 = center - dir * ( height * 0.5f - radius );
    }

    //  
    // Util
    //

    public static void SortClosestToFurthest(RaycastHit[] hits, int hitCount = -1)
    {
        if (hitCount == 0)
        {
            return;
        }

        if (hitCount < 0)
        {
            hitCount = hits.Length;
        }

        Array.Sort<RaycastHit>( hits, 0, hitCount, ascendDistance );
    }

    //
    // Private 
    //

    private class AscendingDistanceComparer : IComparer<RaycastHit>
    {
        public int Compare(RaycastHit h1, RaycastHit h2)
        {
            return h1.distance < h2.distance ? -1 : ( h1.distance > h2.distance ? 1 : 0 );
        }
    }

    private static AscendingDistanceComparer ascendDistance = new AscendingDistanceComparer();

    private static Vector3 AbsVec3(Vector3 v)
    {
        return new Vector3( Mathf.Abs( v.x ), Mathf.Abs( v.y ), Mathf.Abs( v.z ) );
    }

    private static float MaxVec3(Vector3 v)
    {
        return Mathf.Max( v.x, Mathf.Max( v.y, v.z ) );
    }
}