using UnityEngine;

namespace pdinklag
{
    /// <summary>
    /// Debug Extension
    /// 	- Static class that extends Unity's debugging functionallity.
    /// 	- Attempts to mimic Unity's existing debugging behaviour for ease-of-use.
    /// 	- Includes gizmo drawing methods for less memory-intensive debug visualization.
    /// </summary>

    // nb: this was a free asset from the asset store, but has been modified to a point where I just integrated it into my Common module

    public static class DebugExtension
    {
        #region DebugDrawFunctions

        /// <summary>
        /// 	- Debugs a point.
        /// </summary>
        /// <param name='position'>
        /// 	- The point to debug.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the point.
        /// </param>
        /// <param name='scale'>
        /// 	- The size of the point.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the point.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not this point should be faded when behind other objects.
        /// </param>
        public static void DebugPoint(Vector3 position, Color color, float scale = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            color = (color == default) ? Color.white : color;

            Debug.DrawRay(position + (Vector3.up * (scale * 0.5f)), -Vector3.up * scale, color, duration, depthTest);
            Debug.DrawRay(position + (Vector3.right * (scale * 0.5f)), -Vector3.right * scale, color, duration, depthTest);
            Debug.DrawRay(position + (Vector3.forward * (scale * 0.5f)), -Vector3.forward * scale, color, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a point.
        /// </summary>
        /// <param name='position'>
        /// 	- The point to debug.
        /// </param>
        /// <param name='scale'>
        /// 	- The size of the point.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the point.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not this point should be faded when behind other objects.
        /// </param>
        public static void DebugPoint(Vector3 position, float scale = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugPoint(position, Color.white, scale, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs an axis-aligned bounding box.
        /// </summary>
        /// <param name='bounds'>
        /// 	- The bounds to debug.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the bounds.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the bounds.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the bounds should be faded when behind other objects.
        /// </param>
        public static void DebugBounds(Bounds bounds, Color color, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Vector3 center = bounds.center;

            float x = bounds.extents.x;
            float y = bounds.extents.y;
            float z = bounds.extents.z;

            Vector3 ruf = center + new Vector3(x, y, z);
            Vector3 rub = center + new Vector3(x, y, -z);
            Vector3 luf = center + new Vector3(-x, y, z);
            Vector3 lub = center + new Vector3(-x, y, -z);

            Vector3 rdf = center + new Vector3(x, -y, z);
            Vector3 rdb = center + new Vector3(x, -y, -z);
            Vector3 lfd = center + new Vector3(-x, -y, z);
            Vector3 lbd = center + new Vector3(-x, -y, -z);

            Debug.DrawLine(ruf, luf, color, duration, depthTest);
            Debug.DrawLine(ruf, rub, color, duration, depthTest);
            Debug.DrawLine(luf, lub, color, duration, depthTest);
            Debug.DrawLine(rub, lub, color, duration, depthTest);

            Debug.DrawLine(ruf, rdf, color, duration, depthTest);
            Debug.DrawLine(rub, rdb, color, duration, depthTest);
            Debug.DrawLine(luf, lfd, color, duration, depthTest);
            Debug.DrawLine(lub, lbd, color, duration, depthTest);

            Debug.DrawLine(rdf, lfd, color, duration, depthTest);
            Debug.DrawLine(rdf, rdb, color, duration, depthTest);
            Debug.DrawLine(lfd, lbd, color, duration, depthTest);
            Debug.DrawLine(lbd, rdb, color, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs an axis-aligned bounding box.
        /// </summary>
        /// <param name='bounds'>
        /// 	- The bounds to debug.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the bounds.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the bounds should be faded when behind other objects.
        /// </param>
        public static void DebugBounds(Bounds bounds, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugBounds(bounds, Color.white, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a local cube.
        /// </summary>
        /// <param name='transform'>
        /// 	- The transform that the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The size of the cube.
        /// </param>
        /// <param name='color'>
        /// 	- Color of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The position (relative to transform) where the cube will be debugged.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cube.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cube should be faded when behind other objects.
        /// </param>
        public static void DebugLocalCube(Transform transform, Vector3 size, Color color, Vector3 center = default, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Vector3 lbb = transform.TransformPoint(center + ((-size) * 0.5f));
            Vector3 rbb = transform.TransformPoint(center + (new Vector3(size.x, -size.y, -size.z) * 0.5f));

            Vector3 lbf = transform.TransformPoint(center + (new Vector3(size.x, -size.y, size.z) * 0.5f));
            Vector3 rbf = transform.TransformPoint(center + (new Vector3(-size.x, -size.y, size.z) * 0.5f));

            Vector3 lub = transform.TransformPoint(center + (new Vector3(-size.x, size.y, -size.z) * 0.5f));
            Vector3 rub = transform.TransformPoint(center + (new Vector3(size.x, size.y, -size.z) * 0.5f));

            Vector3 luf = transform.TransformPoint(center + ((size) * 0.5f));
            Vector3 ruf = transform.TransformPoint(center + (new Vector3(-size.x, size.y, size.z) * 0.5f));

            Debug.DrawLine(lbb, rbb, color, duration, depthTest);
            Debug.DrawLine(rbb, lbf, color, duration, depthTest);
            Debug.DrawLine(lbf, rbf, color, duration, depthTest);
            Debug.DrawLine(rbf, lbb, color, duration, depthTest);

            Debug.DrawLine(lub, rub, color, duration, depthTest);
            Debug.DrawLine(rub, luf, color, duration, depthTest);
            Debug.DrawLine(luf, ruf, color, duration, depthTest);
            Debug.DrawLine(ruf, lub, color, duration, depthTest);

            Debug.DrawLine(lbb, lub, color, duration, depthTest);
            Debug.DrawLine(rbb, rub, color, duration, depthTest);
            Debug.DrawLine(lbf, luf, color, duration, depthTest);
            Debug.DrawLine(rbf, ruf, color, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a local cube.
        /// </summary>
        /// <param name='transform'>
        /// 	- The transform that the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The size of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The position (relative to transform) where the cube will be debugged.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cube.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cube should be faded when behind other objects.
        /// </param>
        public static void DebugLocalCube(Transform transform, Vector3 size, Vector3 center = default, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugLocalCube(transform, size, Color.white, center, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a local cube.
        /// </summary>
        /// <param name='space'>
        /// 	- The space the cube will be local to.
        /// </param>
        /// <param name='size'>
        ///		- The size of the cube.
        /// </param>
        /// <param name='color'>
        /// 	- Color of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The position (relative to transform) where the cube will be debugged.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cube.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cube should be faded when behind other objects.
        /// </param>
        public static void DebugLocalCube(Matrix4x4 space, Vector3 size, Color color, Vector3 center = default, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            color = (color == default) ? Color.white : color;

            Vector3 lbb = space.MultiplyPoint3x4(center + ((-size) * 0.5f));
            Vector3 rbb = space.MultiplyPoint3x4(center + (new Vector3(size.x, -size.y, -size.z) * 0.5f));

            Vector3 lbf = space.MultiplyPoint3x4(center + (new Vector3(size.x, -size.y, size.z) * 0.5f));
            Vector3 rbf = space.MultiplyPoint3x4(center + (new Vector3(-size.x, -size.y, size.z) * 0.5f));

            Vector3 lub = space.MultiplyPoint3x4(center + (new Vector3(-size.x, size.y, -size.z) * 0.5f));
            Vector3 rub = space.MultiplyPoint3x4(center + (new Vector3(size.x, size.y, -size.z) * 0.5f));

            Vector3 luf = space.MultiplyPoint3x4(center + ((size) * 0.5f));
            Vector3 ruf = space.MultiplyPoint3x4(center + (new Vector3(-size.x, size.y, size.z) * 0.5f));

            Debug.DrawLine(lbb, rbb, color, duration, depthTest);
            Debug.DrawLine(rbb, lbf, color, duration, depthTest);
            Debug.DrawLine(lbf, rbf, color, duration, depthTest);
            Debug.DrawLine(rbf, lbb, color, duration, depthTest);

            Debug.DrawLine(lub, rub, color, duration, depthTest);
            Debug.DrawLine(rub, luf, color, duration, depthTest);
            Debug.DrawLine(luf, ruf, color, duration, depthTest);
            Debug.DrawLine(ruf, lub, color, duration, depthTest);

            Debug.DrawLine(lbb, lub, color, duration, depthTest);
            Debug.DrawLine(rbb, rub, color, duration, depthTest);
            Debug.DrawLine(lbf, luf, color, duration, depthTest);
            Debug.DrawLine(rbf, ruf, color, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a local cube.
        /// </summary>
        /// <param name='space'>
        /// 	- The space the cube will be local to.
        /// </param>
        /// <param name='size'>
        ///		- The size of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The position (relative to transform) where the cube will be debugged.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cube.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cube should be faded when behind other objects.
        /// </param>
        public static void DebugLocalCube(Matrix4x4 space, Vector3 size, Vector3 center = default, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugLocalCube(space, size, Color.white, center, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='up'>
        /// 	- The direction perpendicular to the surface of the circle.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the circle.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the circle should be faded when behind other objects.
        /// </param>
        public static void DebugCircle(Vector3 position, Vector3 up, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Vector3 _up = up.normalized * radius;
            Vector3 _forward = Vector3.Slerp(_up, -_up, 0.5f);
            Vector3 _right = Vector3.Cross(_up, _forward).normalized * radius;

            Matrix4x4 matrix = new Matrix4x4();

            matrix[0] = _right.x;
            matrix[1] = _right.y;
            matrix[2] = _right.z;

            matrix[4] = _up.x;
            matrix[5] = _up.y;
            matrix[6] = _up.z;

            matrix[8] = _forward.x;
            matrix[9] = _forward.y;
            matrix[10] = _forward.z;

            Vector3 _lastPoint = position + matrix.MultiplyPoint3x4(new Vector3(Mathf.Cos(0), 0, Mathf.Sin(0)));
            Vector3 _nextPoint = Vector3.zero;

            color = (color == default) ? Color.white : color;

            for (var i = 0; i < 91; i++)
            {
                _nextPoint.x = Mathf.Cos((i * 4) * Mathf.Deg2Rad);
                _nextPoint.z = Mathf.Sin((i * 4) * Mathf.Deg2Rad);
                _nextPoint.y = 0;

                _nextPoint = position + matrix.MultiplyPoint3x4(_nextPoint);

                Debug.DrawLine(_lastPoint, _nextPoint, color, duration, depthTest);
                _lastPoint = _nextPoint;
            }
#endif
        }

        /// <summary>
        /// 	- Debugs a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the circle.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the circle should be faded when behind other objects.
        /// </param>
        public static void DebugCircle(Vector3 position, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCircle(position, Vector3.up, color, radius, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='up'>
        /// 	- The direction perpendicular to the surface of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the circle.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the circle should be faded when behind other objects.
        /// </param>
        public static void DebugCircle(Vector3 position, Vector3 up, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCircle(position, up, Color.white, radius, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the circle.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the circle should be faded when behind other objects.
        /// </param>
        public static void DebugCircle(Vector3 position, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCircle(position, Vector3.up, Color.white, radius, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a wire sphere.
        /// </summary>
        /// <param name='position'>
        /// 	- The position of the center of the sphere.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the sphere.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the sphere.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the sphere.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the sphere should be faded when behind other objects.
        /// </param>
        public static void DebugWireSphere(Vector3 position, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            float angle = 10.0f;

            Vector3 x = new Vector3(position.x, position.y + radius * Mathf.Sin(0), position.z + radius * Mathf.Cos(0));
            Vector3 y = new Vector3(position.x + radius * Mathf.Cos(0), position.y, position.z + radius * Mathf.Sin(0));
            Vector3 z = new Vector3(position.x + radius * Mathf.Cos(0), position.y + radius * Mathf.Sin(0), position.z);

            Vector3 new_x;
            Vector3 new_y;
            Vector3 new_z;

            for (int i = 1; i < 37; i++)
            {

                new_x = new Vector3(position.x, position.y + radius * Mathf.Sin(angle * i * Mathf.Deg2Rad), position.z + radius * Mathf.Cos(angle * i * Mathf.Deg2Rad));
                new_y = new Vector3(position.x + radius * Mathf.Cos(angle * i * Mathf.Deg2Rad), position.y, position.z + radius * Mathf.Sin(angle * i * Mathf.Deg2Rad));
                new_z = new Vector3(position.x + radius * Mathf.Cos(angle * i * Mathf.Deg2Rad), position.y + radius * Mathf.Sin(angle * i * Mathf.Deg2Rad), position.z);

                Debug.DrawLine(x, new_x, color, duration, depthTest);
                Debug.DrawLine(y, new_y, color, duration, depthTest);
                Debug.DrawLine(z, new_z, color, duration, depthTest);

                x = new_x;
                y = new_y;
                z = new_z;
            }
#endif
        }

        /// <summary>
        /// 	- Debugs a wire sphere.
        /// </summary>
        /// <param name='position'>
        /// 	- The position of the center of the sphere.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the sphere.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the sphere.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the sphere should be faded when behind other objects.
        /// </param>
        public static void DebugWireSphere(Vector3 position, float radius = 1.0f, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugWireSphere(position, Color.white, radius, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cylinder.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the cylinder.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the cylinder.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cylinder.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the cylinder.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cylinder.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cylinder should be faded when behind other objects.
        /// </param>
        public static void DebugCylinder(Vector3 start, Vector3 end, Color color, float radius = 1, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Vector3 up = (end - start).normalized * radius;
            Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
            Vector3 right = Vector3.Cross(up, forward).normalized * radius;

            //Radial circles
            DebugExtension.DebugCircle(start, up, color, radius, duration, depthTest);
            DebugExtension.DebugCircle(end, -up, color, radius, duration, depthTest);
            DebugExtension.DebugCircle((start + end) * 0.5f, up, color, radius, duration, depthTest);

            //Side lines
            Debug.DrawLine(start + right, end + right, color, duration, depthTest);
            Debug.DrawLine(start - right, end - right, color, duration, depthTest);

            Debug.DrawLine(start + forward, end + forward, color, duration, depthTest);
            Debug.DrawLine(start - forward, end - forward, color, duration, depthTest);

            //Start endcap
            Debug.DrawLine(start - right, start + right, color, duration, depthTest);
            Debug.DrawLine(start - forward, start + forward, color, duration, depthTest);

            //End endcap
            Debug.DrawLine(end - right, end + right, color, duration, depthTest);
            Debug.DrawLine(end - forward, end + forward, color, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cylinder.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the cylinder.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the cylinder.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the cylinder.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cylinder.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cylinder should be faded when behind other objects.
        /// </param>
        public static void DebugCylinder(Vector3 start, Vector3 end, float radius = 1, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCylinder(start, end, Color.white, radius, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction for the cone gets wider in.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cone.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cone.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cone should be faded when behind other objects.
        /// </param>
        public static void DebugCone(Vector3 position, Vector3 direction, Color color, float angle = 45, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            float length = direction.magnitude;

            Vector3 _forward = direction;
            Vector3 _up = Vector3.Slerp(_forward, -_forward, 0.5f);
            Vector3 _right = Vector3.Cross(_forward, _up).normalized * length;

            direction = direction.normalized;

            Vector3 slerpedVector = Vector3.Slerp(_forward, _up, angle / 90.0f);

            var farPlane = new Plane(-direction, position + _forward);
            var distRay = new Ray(position, slerpedVector);

            farPlane.Raycast(distRay, out float dist);

            Debug.DrawRay(position, slerpedVector.normalized * dist, color);
            Debug.DrawRay(position, Vector3.Slerp(_forward, -_up, angle / 90.0f).normalized * dist, color, duration, depthTest);
            Debug.DrawRay(position, Vector3.Slerp(_forward, _right, angle / 90.0f).normalized * dist, color, duration, depthTest);
            Debug.DrawRay(position, Vector3.Slerp(_forward, -_right, angle / 90.0f).normalized * dist, color, duration, depthTest);

            DebugExtension.DebugCircle(position + _forward, direction, color, (_forward - (slerpedVector.normalized * dist)).magnitude, duration, depthTest);
            DebugExtension.DebugCircle(position + (_forward * 0.5f), direction, color, ((_forward * 0.5f) - (slerpedVector.normalized * (dist * 0.5f))).magnitude, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction for the cone gets wider in.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cone.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cone should be faded when behind other objects.
        /// </param>
        public static void DebugCone(Vector3 position, Vector3 direction, float angle = 45, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCone(position, direction, Color.white, angle, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cone.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cone.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cone should be faded when behind other objects.
        /// </param>
        public static void DebugCone(Vector3 position, Color color, float angle = 45, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCone(position, Vector3.up, color, angle, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the cone.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the cone should be faded when behind other objects.
        /// </param>
        public static void DebugCone(Vector3 position, float angle = 45, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCone(position, Vector3.up, Color.white, angle, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs an arrow.
        /// </summary>
        /// <param name='position'>
        /// 	- The start position of the arrow.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction the arrow will point in.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the arrow.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the arrow.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the arrow should be faded when behind other objects.
        /// </param>
        public static void DebugArrow(Vector3 position, Vector3 direction, Color color, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Debug.DrawRay(position, direction, color, duration, depthTest);
            DebugExtension.DebugCone(position + direction, -direction * 0.333f, color, 15, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs an arrow.
        /// </summary>
        /// <param name='position'>
        /// 	- The start position of the arrow.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction the arrow will point in.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the arrow.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the arrow should be faded when behind other objects.
        /// </param>
        public static void DebugArrow(Vector3 position, Vector3 direction, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugArrow(position, direction, Color.white, duration, depthTest);
#endif
        }

        /// <summary>
        /// 	- Debugs a capsule.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the capsule.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the capsule.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the capsule.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the capsule.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the capsule.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the capsule should be faded when behind other objects.
        /// </param>
        public static void DebugCapsule(Vector3 start, Vector3 end, Color color, float radius = 1, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            Vector3 up = (end - start).normalized * radius;
            Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
            Vector3 right = Vector3.Cross(up, forward).normalized * radius;

            float height = (start - end).magnitude;
            float sideLength = Mathf.Max(0, (height * 0.5f));
            Vector3 middle = (end + start) * 0.5f;

            start = middle + ((start - middle).normalized * sideLength);
            end = middle + ((end - middle).normalized * sideLength);

            //Radial circles
            DebugExtension.DebugCircle(start, up, color, radius, duration, depthTest);
            DebugExtension.DebugCircle(end, -up, color, radius, duration, depthTest);

            //Side lines
            Debug.DrawLine(start + right, end + right, color, duration, depthTest);
            Debug.DrawLine(start - right, end - right, color, duration, depthTest);

            Debug.DrawLine(start + forward, end + forward, color, duration, depthTest);
            Debug.DrawLine(start - forward, end - forward, color, duration, depthTest);

            for (int i = 1; i < 26; i++)
            {

                //Start endcap
                Debug.DrawLine(Vector3.Slerp(right, -up, i / 25.0f) + start, Vector3.Slerp(right, -up, (i - 1) / 25.0f) + start, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(-right, -up, i / 25.0f) + start, Vector3.Slerp(-right, -up, (i - 1) / 25.0f) + start, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(forward, -up, i / 25.0f) + start, Vector3.Slerp(forward, -up, (i - 1) / 25.0f) + start, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(-forward, -up, i / 25.0f) + start, Vector3.Slerp(-forward, -up, (i - 1) / 25.0f) + start, color, duration, depthTest);

                //End endcap
                Debug.DrawLine(Vector3.Slerp(right, up, i / 25.0f) + end, Vector3.Slerp(right, up, (i - 1) / 25.0f) + end, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(-right, up, i / 25.0f) + end, Vector3.Slerp(-right, up, (i - 1) / 25.0f) + end, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(forward, up, i / 25.0f) + end, Vector3.Slerp(forward, up, (i - 1) / 25.0f) + end, color, duration, depthTest);
                Debug.DrawLine(Vector3.Slerp(-forward, up, i / 25.0f) + end, Vector3.Slerp(-forward, up, (i - 1) / 25.0f) + end, color, duration, depthTest);
            }
#endif
        }

        /// <summary>
        /// 	- Debugs a capsule.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the capsule.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the capsule.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the capsule.
        /// </param>
        /// <param name='duration'>
        /// 	- How long to draw the capsule.
        /// </param>
        /// <param name='depthTest'>
        /// 	- Whether or not the capsule should be faded when behind other objects.
        /// </param>
        public static void DebugCapsule(Vector3 start, Vector3 end, float radius = 1, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugCapsule(start, end, Color.white, radius, duration, depthTest);
#endif
        }

        private static void DebugBone(Animator a, HumanBodyBones start, HumanBodyBones end, Color color, float duration, bool depthTest)
        {
#if UNITY_EDITOR
            var startBone = a.GetBoneTransform(start);
            var endBone = a.GetBoneTransform(end);

            if (startBone && endBone)
            {
                Debug.DrawLine(startBone.position, endBone.position, color, duration, depthTest);
            }
#endif
        }


        private static void DebugBone(Animator a, HumanBodyBones start, HumanBodyBones mid, HumanBodyBones end, Color color, float duration, bool depthTest)
        {
#if UNITY_EDITOR
            var startBone = a.GetBoneTransform(start);
            var midBone = a.GetBoneTransform(mid);
            var endBone = a.GetBoneTransform(end);

            if (midBone)
            {
                Debug.DrawLine(startBone.position, midBone.position, color, duration, depthTest);
                Debug.DrawLine(midBone.position, endBone.position, color, duration, depthTest);
            }
            else
            {
                Debug.DrawLine(startBone.position, endBone.position, color, duration, depthTest);
            }
#endif
        }

        /// <summary>
        /// Debugs the bones of an animator.
        /// </summary>
        /// <param name="a">the animator</param>
        /// <param name="color">the bone line colors</param>
        /// <param name="duration">the debug duration</param>
        /// <param name="depthTest">whether or not to fade when behind other objects</param>
        public static void DebugAnimator(Animator a, Color color, float duration = 0, bool depthTest = true)
        {
#if UNITY_EDITOR
            DebugBone(a, HumanBodyBones.Hips, HumanBodyBones.Spine, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.Spine, HumanBodyBones.Neck, HumanBodyBones.Head, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.Spine, HumanBodyBones.LeftShoulder, HumanBodyBones.LeftUpperArm, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.LeftUpperArm, HumanBodyBones.LeftLowerArm, HumanBodyBones.LeftHand, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.Spine, HumanBodyBones.RightShoulder, HumanBodyBones.RightUpperArm, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.RightUpperArm, HumanBodyBones.RightLowerArm, HumanBodyBones.RightHand, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.Hips, HumanBodyBones.LeftUpperLeg, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.LeftUpperLeg, HumanBodyBones.LeftLowerLeg, HumanBodyBones.LeftFoot, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.LeftFoot, HumanBodyBones.LeftToes, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.Hips, HumanBodyBones.RightUpperLeg, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.RightUpperLeg, HumanBodyBones.RightLowerLeg, HumanBodyBones.RightFoot, color, duration, depthTest);
            DebugBone(a, HumanBodyBones.RightFoot, HumanBodyBones.RightToes, color, duration, depthTest);
#endif
        }

        #endregion

        #region GizmoDrawFunctions

        /// <summary>
        /// 	- Draws a point.
        /// </summary>
        /// <param name='position'>
        /// 	- The point to draw.
        /// </param>
        ///  <param name='color'>
        /// 	- The color of the drawn point.
        /// </param>
        /// <param name='scale'>
        /// 	- The size of the drawn point.
        /// </param>
        public static void DrawPoint(Vector3 position, Color color, float scale = 1.0f)
        {
#if UNITY_EDITOR
            Color oldColor = Gizmos.color;

            Gizmos.color = color;
            Gizmos.DrawRay(position + (Vector3.up * (scale * 0.5f)), -Vector3.up * scale);
            Gizmos.DrawRay(position + (Vector3.right * (scale * 0.5f)), -Vector3.right * scale);
            Gizmos.DrawRay(position + (Vector3.forward * (scale * 0.5f)), -Vector3.forward * scale);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a point.
        /// </summary>
        /// <param name='position'>
        /// 	- The point to draw.
        /// </param>
        /// <param name='scale'>
        /// 	- The size of the drawn point.
        /// </param>
        public static void DrawPoint(Vector3 position, float scale = 1.0f)
        {
#if UNITY_EDITOR
            DrawPoint(position, Color.white, scale);
#endif
        }

        /// <summary>
        /// 	- Draws an axis-aligned bounding box.
        /// </summary>
        /// <param name='bounds'>
        /// 	- The bounds to draw.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the bounds.
        /// </param>
        public static void DrawBounds(Bounds bounds, Color color)
        {
#if UNITY_EDITOR
            Vector3 center = bounds.center;

            float x = bounds.extents.x;
            float y = bounds.extents.y;
            float z = bounds.extents.z;

            Vector3 ruf = center + new Vector3(x, y, z);
            Vector3 rub = center + new Vector3(x, y, -z);
            Vector3 luf = center + new Vector3(-x, y, z);
            Vector3 lub = center + new Vector3(-x, y, -z);

            Vector3 rdf = center + new Vector3(x, -y, z);
            Vector3 rdb = center + new Vector3(x, -y, -z);
            Vector3 lfd = center + new Vector3(-x, -y, z);
            Vector3 lbd = center + new Vector3(-x, -y, -z);

            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            Gizmos.DrawLine(ruf, luf);
            Gizmos.DrawLine(ruf, rub);
            Gizmos.DrawLine(luf, lub);
            Gizmos.DrawLine(rub, lub);

            Gizmos.DrawLine(ruf, rdf);
            Gizmos.DrawLine(rub, rdb);
            Gizmos.DrawLine(luf, lfd);
            Gizmos.DrawLine(lub, lbd);

            Gizmos.DrawLine(rdf, lfd);
            Gizmos.DrawLine(rdf, rdb);
            Gizmos.DrawLine(lfd, lbd);
            Gizmos.DrawLine(lbd, rdb);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws an axis-aligned bounding box.
        /// </summary>
        /// <param name='bounds'>
        /// 	- The bounds to draw.
        /// </param>
        public static void DrawBounds(Bounds bounds)
        {
#if UNITY_EDITOR
            DrawBounds(bounds, Color.white);
#endif
        }

        /// <summary>
        /// 	- Draws a local cube.
        /// </summary>
        /// <param name='transform'>
        /// 	- The transform the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The local size of the cube.
        /// </param>
        /// <param name='center'>
        ///		- The local position of the cube.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cube.
        /// </param>
        public static void DrawLocalCube(Transform transform, Vector3 size, Color color, Vector3 center = default)
        {
#if UNITY_EDITOR
            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            Vector3 lbb = transform.TransformPoint(center + ((-size) * 0.5f));
            Vector3 rbb = transform.TransformPoint(center + (new Vector3(size.x, -size.y, -size.z) * 0.5f));

            Vector3 lbf = transform.TransformPoint(center + (new Vector3(size.x, -size.y, size.z) * 0.5f));
            Vector3 rbf = transform.TransformPoint(center + (new Vector3(-size.x, -size.y, size.z) * 0.5f));

            Vector3 lub = transform.TransformPoint(center + (new Vector3(-size.x, size.y, -size.z) * 0.5f));
            Vector3 rub = transform.TransformPoint(center + (new Vector3(size.x, size.y, -size.z) * 0.5f));

            Vector3 luf = transform.TransformPoint(center + ((size) * 0.5f));
            Vector3 ruf = transform.TransformPoint(center + (new Vector3(-size.x, size.y, size.z) * 0.5f));

            Gizmos.DrawLine(lbb, rbb);
            Gizmos.DrawLine(rbb, lbf);
            Gizmos.DrawLine(lbf, rbf);
            Gizmos.DrawLine(rbf, lbb);

            Gizmos.DrawLine(lub, rub);
            Gizmos.DrawLine(rub, luf);
            Gizmos.DrawLine(luf, ruf);
            Gizmos.DrawLine(ruf, lub);

            Gizmos.DrawLine(lbb, lub);
            Gizmos.DrawLine(rbb, rub);
            Gizmos.DrawLine(lbf, luf);
            Gizmos.DrawLine(rbf, ruf);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a local cube.
        /// </summary>
        /// <param name='transform'>
        /// 	- The transform the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The local size of the cube.
        /// </param>
        /// <param name='center'>
        ///		- The local position of the cube.
        /// </param>
        public static void DrawLocalCube(Transform transform, Vector3 size, Vector3 center = default)
        {
#if UNITY_EDITOR
            DrawLocalCube(transform, size, Color.white, center);
#endif
        }

        /// <summary>
        /// 	- Draws a local cube.
        /// </summary>
        /// <param name='space'>
        /// 	- The space the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The local size of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The local position of the cube.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cube.
        /// </param>
        public static void DrawLocalCube(Matrix4x4 space, Vector3 size, Color color, Vector3 center = default)
        {
#if UNITY_EDITOR
            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            Vector3 lbb = space.MultiplyPoint3x4(center + ((-size) * 0.5f));
            Vector3 rbb = space.MultiplyPoint3x4(center + (new Vector3(size.x, -size.y, -size.z) * 0.5f));

            Vector3 lbf = space.MultiplyPoint3x4(center + (new Vector3(size.x, -size.y, size.z) * 0.5f));
            Vector3 rbf = space.MultiplyPoint3x4(center + (new Vector3(-size.x, -size.y, size.z) * 0.5f));

            Vector3 lub = space.MultiplyPoint3x4(center + (new Vector3(-size.x, size.y, -size.z) * 0.5f));
            Vector3 rub = space.MultiplyPoint3x4(center + (new Vector3(size.x, size.y, -size.z) * 0.5f));

            Vector3 luf = space.MultiplyPoint3x4(center + ((size) * 0.5f));
            Vector3 ruf = space.MultiplyPoint3x4(center + (new Vector3(-size.x, size.y, size.z) * 0.5f));

            Gizmos.DrawLine(lbb, rbb);
            Gizmos.DrawLine(rbb, lbf);
            Gizmos.DrawLine(lbf, rbf);
            Gizmos.DrawLine(rbf, lbb);

            Gizmos.DrawLine(lub, rub);
            Gizmos.DrawLine(rub, luf);
            Gizmos.DrawLine(luf, ruf);
            Gizmos.DrawLine(ruf, lub);

            Gizmos.DrawLine(lbb, lub);
            Gizmos.DrawLine(rbb, rub);
            Gizmos.DrawLine(lbf, luf);
            Gizmos.DrawLine(rbf, ruf);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a local cube.
        /// </summary>
        /// <param name='space'>
        /// 	- The space the cube will be local to.
        /// </param>
        /// <param name='size'>
        /// 	- The local size of the cube.
        /// </param>
        /// <param name='center'>
        /// 	- The local position of the cube.
        /// </param>
        public static void DrawLocalCube(Matrix4x4 space, Vector3 size, Vector3 center = default)
        {
#if UNITY_EDITOR
            DrawLocalCube(space, size, Color.white, center);
#endif
        }

        /// <summary>
        /// 	- Draws a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='up'>
        /// 	- The direction perpendicular to the surface of the circle.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        public static void DrawCircle(Vector3 position, Vector3 up, Color color, float radius = 1.0f)
        {
#if UNITY_EDITOR
            up = ((up == Vector3.zero) ? Vector3.up : up).normalized * radius;
            Vector3 _forward = Vector3.Slerp(up, -up, 0.5f);
            Vector3 _right = Vector3.Cross(up, _forward).normalized * radius;

            Matrix4x4 matrix = new Matrix4x4();

            matrix[0] = _right.x;
            matrix[1] = _right.y;
            matrix[2] = _right.z;

            matrix[4] = up.x;
            matrix[5] = up.y;
            matrix[6] = up.z;

            matrix[8] = _forward.x;
            matrix[9] = _forward.y;
            matrix[10] = _forward.z;

            Vector3 _lastPoint = position + matrix.MultiplyPoint3x4(new Vector3(Mathf.Cos(0), 0, Mathf.Sin(0)));
            Vector3 _nextPoint = Vector3.zero;

            Color oldColor = Gizmos.color;
            Gizmos.color = (color == default) ? Color.white : color;

            for (var i = 0; i < 91; i++)
            {
                _nextPoint.x = Mathf.Cos((i * 4) * Mathf.Deg2Rad);
                _nextPoint.z = Mathf.Sin((i * 4) * Mathf.Deg2Rad);
                _nextPoint.y = 0;

                _nextPoint = position + matrix.MultiplyPoint3x4(_nextPoint);

                Gizmos.DrawLine(_lastPoint, _nextPoint);
                _lastPoint = _nextPoint;
            }

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        public static void DrawCircle(Vector3 position, Color color, float radius = 1.0f)
        {
#if UNITY_EDITOR
            DrawCircle(position, Vector3.up, color, radius);
#endif
        }

        /// <summary>
        /// 	- Draws a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='up'>
        /// 	- The direction perpendicular to the surface of the circle.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        public static void DrawCircle(Vector3 position, Vector3 up, float radius = 1.0f)
        {
#if UNITY_EDITOR
            DrawCircle(position, position, Color.white, radius);
#endif
        }

        /// <summary>
        /// 	- Draws a circle.
        /// </summary>
        /// <param name='position'>
        /// 	- Where the center of the circle will be positioned.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the circle.
        /// </param>
        public static void DrawCircle(Vector3 position, float radius = 1.0f)
        {
#if UNITY_EDITOR
            DrawCircle(position, Vector3.up, Color.white, radius);
#endif
        }

        //Wiresphere already exists

        /// <summary>
        /// 	- Draws a cylinder.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the cylinder.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the cylinder.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cylinder.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the cylinder.
        /// </param>
        public static void DrawCylinder(Vector3 start, Vector3 end, Color color, float radius = 1.0f)
        {
#if UNITY_EDITOR
            Vector3 up = (end - start).normalized * radius;
            Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
            Vector3 right = Vector3.Cross(up, forward).normalized * radius;

            //Radial circles
            DebugExtension.DrawCircle(start, up, color, radius);
            DebugExtension.DrawCircle(end, -up, color, radius);
            DebugExtension.DrawCircle((start + end) * 0.5f, up, color, radius);

            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            //Side lines
            Gizmos.DrawLine(start + right, end + right);
            Gizmos.DrawLine(start - right, end - right);

            Gizmos.DrawLine(start + forward, end + forward);
            Gizmos.DrawLine(start - forward, end - forward);

            //Start endcap
            Gizmos.DrawLine(start - right, start + right);
            Gizmos.DrawLine(start - forward, start + forward);

            //End endcap
            Gizmos.DrawLine(end - right, end + right);
            Gizmos.DrawLine(end - forward, end + forward);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a cylinder.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the cylinder.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the cylinder.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the cylinder.
        /// </param>
        public static void DrawCylinder(Vector3 start, Vector3 end, float radius = 1.0f)
        {
#if UNITY_EDITOR
            DrawCylinder(start, end, Color.white, radius);
#endif
        }

        /// <summary>
        /// 	- Draws a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction for the cone to get wider in.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cone.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        public static void DrawCone(Vector3 position, Vector3 direction, Color color, float angle = 45)
        {
#if UNITY_EDITOR
            float length = direction.magnitude;

            Vector3 _forward = direction;
            Vector3 _up = Vector3.Slerp(_forward, -_forward, 0.5f);
            Vector3 _right = Vector3.Cross(_forward, _up).normalized * length;

            direction = direction.normalized;

            Vector3 slerpedVector = Vector3.Slerp(_forward, _up, angle / 90.0f);

            var farPlane = new Plane(-direction, position + _forward);
            var distRay = new Ray(position, slerpedVector);

            farPlane.Raycast(distRay, out float dist);

            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            Gizmos.DrawRay(position, slerpedVector.normalized * dist);
            Gizmos.DrawRay(position, Vector3.Slerp(_forward, -_up, angle / 90.0f).normalized * dist);
            Gizmos.DrawRay(position, Vector3.Slerp(_forward, _right, angle / 90.0f).normalized * dist);
            Gizmos.DrawRay(position, Vector3.Slerp(_forward, -_right, angle / 90.0f).normalized * dist);

            DebugExtension.DrawCircle(position + _forward, direction, color, (_forward - (slerpedVector.normalized * dist)).magnitude);
            DebugExtension.DrawCircle(position + (_forward * 0.5f), direction, color, ((_forward * 0.5f) - (slerpedVector.normalized * (dist * 0.5f))).magnitude);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction for the cone to get wider in.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        public static void DrawCone(Vector3 position, Vector3 direction, float angle = 45)
        {
#if UNITY_EDITOR
            DrawCone(position, direction, Color.white, angle);
#endif
        }

        /// <summary>
        /// 	- Draws a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the cone.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        public static void DrawCone(Vector3 position, Color color, float angle = 45)
        {
#if UNITY_EDITOR
            DrawCone(position, Vector3.up, color, angle);
#endif
        }

        /// <summary>
        /// 	- Draws a cone.
        /// </summary>
        /// <param name='position'>
        /// 	- The position for the tip of the cone.
        /// </param>
        /// <param name='angle'>
        /// 	- The angle of the cone.
        /// </param>
        public static void DrawCone(Vector3 position, float angle = 45)
        {
#if UNITY_EDITOR
            DrawCone(position, Vector3.up, Color.white, angle);
#endif
        }

        /// <summary>
        /// 	- Draws an arrow.
        /// </summary>
        /// <param name='position'>
        /// 	- The start position of the arrow.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction the arrow will point in.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the arrow.
        /// </param>
        public static void DrawArrow(Vector3 position, Vector3 direction, Color color)
        {
#if UNITY_EDITOR
            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            Gizmos.DrawRay(position, direction);
            DebugExtension.DrawCone(position + direction, -direction * 0.333f, color, 15);

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws an arrow.
        /// </summary>
        /// <param name='position'>
        /// 	- The start position of the arrow.
        /// </param>
        /// <param name='direction'>
        /// 	- The direction the arrow will point in.
        /// </param>
        public static void DrawArrow(Vector3 position, Vector3 direction)
        {
#if UNITY_EDITOR
            DrawArrow(position, direction, Color.white);
#endif
        }

        /// <summary>
        /// 	- Draws a capsule.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the capsule.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the capsule.
        /// </param>
        /// <param name='color'>
        /// 	- The color of the capsule.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the capsule.
        /// </param>
        public static void DrawCapsule(Vector3 start, Vector3 end, Color color, float radius = 1)
        {
#if UNITY_EDITOR
            Vector3 up = (end - start).normalized * radius;
            Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
            Vector3 right = Vector3.Cross(up, forward).normalized * radius;

            Color oldColor = Gizmos.color;
            Gizmos.color = color;

            float height = (start - end).magnitude;
            float sideLength = Mathf.Max(0, (height * 0.5f));
            Vector3 middle = (end + start) * 0.5f;

            start = middle + ((start - middle).normalized * sideLength);
            end = middle + ((end - middle).normalized * sideLength);

            //Radial circles
            DebugExtension.DrawCircle(start, up, color, radius);
            DebugExtension.DrawCircle(end, -up, color, radius);

            //Side lines
            Gizmos.DrawLine(start + right, end + right);
            Gizmos.DrawLine(start - right, end - right);

            Gizmos.DrawLine(start + forward, end + forward);
            Gizmos.DrawLine(start - forward, end - forward);

            for (int i = 1; i < 26; i++)
            {

                //Start endcap
                Gizmos.DrawLine(Vector3.Slerp(right, -up, i / 25.0f) + start, Vector3.Slerp(right, -up, (i - 1) / 25.0f) + start);
                Gizmos.DrawLine(Vector3.Slerp(-right, -up, i / 25.0f) + start, Vector3.Slerp(-right, -up, (i - 1) / 25.0f) + start);
                Gizmos.DrawLine(Vector3.Slerp(forward, -up, i / 25.0f) + start, Vector3.Slerp(forward, -up, (i - 1) / 25.0f) + start);
                Gizmos.DrawLine(Vector3.Slerp(-forward, -up, i / 25.0f) + start, Vector3.Slerp(-forward, -up, (i - 1) / 25.0f) + start);

                //End endcap
                Gizmos.DrawLine(Vector3.Slerp(right, up, i / 25.0f) + end, Vector3.Slerp(right, up, (i - 1) / 25.0f) + end);
                Gizmos.DrawLine(Vector3.Slerp(-right, up, i / 25.0f) + end, Vector3.Slerp(-right, up, (i - 1) / 25.0f) + end);
                Gizmos.DrawLine(Vector3.Slerp(forward, up, i / 25.0f) + end, Vector3.Slerp(forward, up, (i - 1) / 25.0f) + end);
                Gizmos.DrawLine(Vector3.Slerp(-forward, up, i / 25.0f) + end, Vector3.Slerp(-forward, up, (i - 1) / 25.0f) + end);
            }

            Gizmos.color = oldColor;
#endif
        }

        /// <summary>
        /// 	- Draws a capsule.
        /// </summary>
        /// <param name='start'>
        /// 	- The position of one end of the capsule.
        /// </param>
        /// <param name='end'>
        /// 	- The position of the other end of the capsule.
        /// </param>
        /// <param name='radius'>
        /// 	- The radius of the capsule.
        /// </param>
        public static void DrawCapsule(Vector3 start, Vector3 end, float radius = 1)
        {
#if UNITY_EDITOR
            DrawCapsule(start, end, Color.white, radius);
#endif
        }

        private static void DrawBone(Animator a, HumanBodyBones start, HumanBodyBones end)
        {
#if UNITY_EDITOR
            var startBone = a.GetBoneTransform(start);
            var endBone = a.GetBoneTransform(end);

            if (startBone && endBone)
            {
                Gizmos.DrawLine(startBone.position, endBone.position);
            }
#endif
        }

        private static void DrawBone(Animator a, HumanBodyBones start, HumanBodyBones mid, HumanBodyBones end)
        {
#if UNITY_EDITOR
            var startBone = a.GetBoneTransform(start);
            var midBone = a.GetBoneTransform(mid);
            var endBone = a.GetBoneTransform(end);

            if (midBone)
            {
                Gizmos.DrawLine(startBone.position, midBone.position);
                Gizmos.DrawLine(midBone.position, endBone.position);
            }
            else
            {
                Gizmos.DrawLine(startBone.position, endBone.position);
            }
#endif
        }

        /// <summary>
        /// Debugs the bones of an animator.
        /// </summary>
        /// <param name="a">the animator</param>
        /// <param name="color">the bone line colors</param>
        public static void DrawAnimator(Animator a, Color color)
        {
#if UNITY_EDITOR
            var oldColor = Gizmos.color;
            Gizmos.color = color;
            DrawBone(a, HumanBodyBones.Hips, HumanBodyBones.Spine);
            DrawBone(a, HumanBodyBones.Spine, HumanBodyBones.Neck, HumanBodyBones.Head);
            DrawBone(a, HumanBodyBones.Spine, HumanBodyBones.LeftShoulder, HumanBodyBones.LeftUpperArm);
            DrawBone(a, HumanBodyBones.LeftUpperArm, HumanBodyBones.LeftLowerArm, HumanBodyBones.LeftHand);
            DrawBone(a, HumanBodyBones.Spine, HumanBodyBones.RightShoulder, HumanBodyBones.RightUpperArm);
            DrawBone(a, HumanBodyBones.RightUpperArm, HumanBodyBones.RightLowerArm, HumanBodyBones.RightHand);
            DrawBone(a, HumanBodyBones.Hips, HumanBodyBones.LeftUpperLeg);
            DrawBone(a, HumanBodyBones.LeftUpperLeg, HumanBodyBones.LeftLowerLeg, HumanBodyBones.LeftFoot);
            DrawBone(a, HumanBodyBones.LeftFoot, HumanBodyBones.LeftToes);
            DrawBone(a, HumanBodyBones.Hips, HumanBodyBones.RightUpperLeg);
            DrawBone(a, HumanBodyBones.RightUpperLeg, HumanBodyBones.RightLowerLeg, HumanBodyBones.RightFoot);
            DrawBone(a, HumanBodyBones.RightFoot, HumanBodyBones.RightToes);
            Gizmos.color = oldColor;
#endif
        }

        #endregion
    }
}
