using UnityEngine;
using System.Collections.Generic;

public class CollisionUtility
{

    /// <summary>
    /// 扫描碰撞检测
    /// </summary>
    /// <param name="_start_pos">扫描的起点</param>
    /// <param name="_end_pos">扫描的终点</param>
    /// <param name="_sweep_radius">扫描的半径</param>
    /// <param name="_position">被扫描对象的坐标</param>
    /// <param name="_height">被扫描对象的高度，碰撞体的高度已经包括了半径</param>
    /// <param name="_radius">被扫描对象的半径</param>
    /// <returns>返回是否扫描到</returns>
    public static bool ScanCollide(Vector3 _start_pos, Vector3 _end_pos, float _sweep_radius,
                                    Vector3 _position, float _height, float _radius)
    {
        // 转为判断2个向量的距离判断
        // 因为要扫的对象都是不会左右旋转的，是一个直立的胶囊提对象，所以取得最高点和最低点
        // 与扫的起点和终点为2个向量
        float low_x_1 = 0, height_x_1 = 0, low_y_1 = 0, height_y_1 = 0, low_z_1 = 0, height_z_1 = 0;
        // 计算2个向量的高点和低点
        if (_start_pos.y > _end_pos.y)
        {
            low_y_1 = _end_pos.y;
            height_y_1 = _start_pos.y;
        }
        else
        {
            low_y_1 = _start_pos.y;
            height_y_1 = _end_pos.y;
        }

        if (_start_pos.x > _end_pos.x)
        {
            low_x_1 = _end_pos.x;
            height_x_1 = _start_pos.x;
        }
        else
        {
            low_x_1 = _start_pos.x;
            height_x_1 = _end_pos.x;
        }

        if (_start_pos.z > _end_pos.z)
        {
            low_z_1 = _end_pos.z;
            height_z_1 = _start_pos.z;
        }
        else
        {
            low_z_1 = _start_pos.z;
            height_z_1 = _end_pos.z;
        }

        float low_y_2 = _position.y;
        float height_y_2 = _position.y + _height;

        // 如果其中一个的高点比另一个的低点-半径还要低，或者低点比另一个的高点+半径还要高，则肯定不会碰撞
        if (low_y_1 - _sweep_radius > height_y_2 || height_y_1 + _sweep_radius < low_y_2 ||
            low_x_1 - _sweep_radius > _position.x + _radius || height_x_1 + _sweep_radius < _position.x - _radius ||
            low_z_1 - _sweep_radius > _position.z + _radius || height_z_1 + _sweep_radius < _position.z - _radius)
            return false;

        // 再来就是比较2个向量的距离，如果距离小于2个半径，则会碰撞
        Vector3 vec1 = _end_pos - _start_pos;
        Vector3 vec2 = new Vector3(0, _height, 0);

        float d = float.MaxValue;

        // 这里要判断是否平行的情况
        // 只需要判断vec1的x和z是否为0，为0则与要判断向量平行
        if (vec1.x == 0 && vec1.z == 0)
        {
            d = Mathf.Sqrt((_start_pos.x - _position.x) * (_start_pos.x - _position.x) + (_start_pos.z - _position.z) * (_start_pos.z - _position.z));
        }
        else
        {
            // 扫描向量上任取一点A，被扫对象向量上任选一点B组成新的向量AB，
            // 求出扫描向量和被扫向量的法向量N
            // 则这2个向量的距离为 d = AB * N / N的模
            Vector3 N = Vector3.Cross(vec1, vec2);
            Vector3 AB = _start_pos - _position;

            d = Mathf.Abs(N.x * AB.x + N.y * AB.y + N.z * AB.z) / Mathf.Sqrt(N.x * N.x + N.y * N.y + N.z * N.z);
        }

        // CatDebugger.Log("distance : " + d + " , range : " + (_sweep_radius + _radius));

        return d < _sweep_radius + _radius;
    }


    public static bool Sweep(List<Vector3> points,
                             float radius,
                             Vector3 targetPosition,
                             float targetRadius)
    {

        bool _result = false;

        if (points.Count == 1)
        {
            return Vector3.SqrMagnitude(targetPosition - points[0]) < Mathf.Pow(radius + targetRadius, 2);
        }

        Vector3 _from;
        Vector3 _to;

        for (int i = 0; i < points.Count - 1; ++i)
        {
            _from = points[i];
            _to = points[i + 1];
            if (CircleCollied(_from, _to, radius, targetPosition, targetRadius))
            {
                _result = true;
                break;
            }
        }

        return _result;
    }

    /// <summary>
    /// 一段距离的线段的平面圆扫描检测
    /// </summary>
    /// <param name="f">线段起点</param>
    /// <param name="t">线段终点</param>
    /// <param name="radius">圆的半径</param>
    /// <param name="p">检测点</param>
    /// <param name="targetRadius">检测半径</param>
    /// <returns>返回距离是否小于2个半径的长度</returns>
    public static bool CircleCollied(Vector3 f,
                                     Vector3 t,
                                     float radius,
                                     Vector3 p,
                                     float targetRadius)
    {

        // 对y进行归零
        f.y = 0;
        t.y = 0;
        p.y = 0;

        // 得到真实的距离差
        float _radiusSum = radius + targetRadius;

        // 如果传入的起点,终点相同, 则只计算2点之间的距离是否小于半径
        if (f == t)
        {
            return Vector3.SqrMagnitude(p - f) < _radiusSum * _radiusSum;
        }

        // 如果点在两端之外,则一定不会碰撞
        // 起点x更大
        if (f.x > t.x)
        {
            if (p.x > f.x)
            {
                return Vector3.SqrMagnitude(p - f) < _radiusSum * _radiusSum;
            }
        }
        else
        {// 终点更大
            if (p.x > t.x)
            {
                return Vector3.SqrMagnitude(p - t) < _radiusSum * _radiusSum;
            }
        }

        // 起点z更大
        if (f.z > t.z)
        {
            if (p.z > f.z)
            {
                return Vector3.SqrMagnitude(p - f) < _radiusSum * _radiusSum;
            }
        }
        else
        {
            if (p.z > t.z)
            {
                return Vector3.SqrMagnitude(p - t) < _radiusSum * _radiusSum;
            }
        }

        float _distance = 0;

        if (f.x == t.x)
        {
            Debug.LogFormat("_distance: {0}", Mathf.Abs(p.x - f.x));
            // 只要保证
        }

        float cross = (t.x - f.x) * (p.x - f.x) + (t.z - f.z) * (p.z - f.z);
        if (cross <= 0)
        {
            _distance = Mathf.Sqrt((p.x - f.x) * (p.x - f.x) + (p.z - f.z) * (p.z - f.z));
            return _distance < _radiusSum;
        }

        float d2 = (t.x - f.x) * (t.x - f.x) + (t.z - f.z) * (t.z - f.z);
        if (cross >= d2)
        {
            _distance = Mathf.Sqrt((p.x - t.x) * (p.x - t.x) + (p.z - t.z) * (p.z - t.z));
            return _distance < _radiusSum;
        }

        float r = cross / d2;
        float px = f.x + (t.x - f.x) * r;
        float py = f.z + (t.z - f.z) * r;

        _distance = Mathf.Sqrt((p.x - px) * (p.x - px) + (py - p.z) * (py - p.z));
        return _distance < _radiusSum;
    }

    /// <summary>
    /// 检测点是否在给定的矩形中
    /// </summary>
    /// <param name="position">矩形位置</param>
    /// <param name="direction">矩形的朝向</param>
    /// <param name="width">矩形的宽度</param>
    /// <param name="distance">矩形的距离</param>
    /// <param name="checkPoint">检测点的坐标</param>
    /// <returns>是否在检测范围内</returns>
    public static bool IsPointInRectangle(Vector3 position,
                                          Vector3 direction,
                                          float width,
                                          float distance,
                                          Vector3 checkPoint)
    {
        Vector3 p1 = position + Quaternion.Euler(0, -90, 0) * direction * width * .5f;
        Vector3 p2 = p1 + direction * distance;
        Vector3 p3 = p2 + Quaternion.Euler(0, 90, 0) * direction * width;
        Vector3 p4 = position + Quaternion.Euler(0, 90, 0) * direction * width * .5f;

        Line2D _line1 = new Line2D
        {
            start = new Vector2(p1.x, p1.z),
            end = new Vector2(p2.x, p2.z)
        };

        Line2D _line2 = new Line2D
        {
            start = new Vector2(p2.x, p2.z),
            end = new Vector2(p3.x, p3.z)
        };

        Line2D _line3 = new Line2D
        {
            start = new Vector2(p3.x, p3.z),
            end = new Vector2(p4.x, p4.z)
        };

        Line2D _line4 = new Line2D
        {
            start = new Vector2(p4.x, p4.z),
            end = new Vector2(p1.x, p1.z)
        };

        bool result = Line2D.PointAtLineLeft(new Vector2(checkPoint.x, checkPoint.z), _line1)
                   || Line2D.PointAtLineLeft(new Vector2(checkPoint.x, checkPoint.z), _line2)
                   || Line2D.PointAtLineLeft(new Vector2(checkPoint.x, checkPoint.z), _line3)
                   || Line2D.PointAtLineLeft(new Vector2(checkPoint.x, checkPoint.z), _line4);

        return !result;
    }

    /// <summary>
    /// 检测给定的点是否在给定的扇形区域中
    /// </summary>
    /// <param name="position">扇形的圆心点</param>
    /// <param name="direction">扇形的朝向</param>
    /// <param name="radius">扇形的半径</param>
    /// <param name="angle">扇形的张角</param>
    /// <param name="checkPoint">检测点坐标</param>
    /// <returns></returns>
    public static bool IsPointInSector(Vector3 position,
                                       Vector3 direction,
                                       float radius,
                                       float angle,
                                       Vector3 checkPoint)
    {
        // 半径检测
        if (!IsPointsInDistance(position, checkPoint, radius))
        {
            return false;
        }

        // 扇形范围检测
        Vector3 _chkToSelf = (checkPoint - position).normalized;
        float _chkAngle = Mathf.Acos(Vector3.Dot(direction, _chkToSelf)) * Mathf.Rad2Deg;
        return _chkAngle <= angle * .5f;
    }

    public static bool IsPointInSector2(Vector3 position,
                                       Vector3 direction,
                                       float radius,
                                       float angle,
                                       Vector3 checkPoint)
    {
        return IsPointInSector2Sqrt(position, direction, radius * radius, angle, checkPoint);
    }

    public static bool IsPointInSector2Sqrt(Vector3 position,
                                   Vector3 direction,
                                   float radiusSqrt,
                                   float angle,
                                   Vector3 checkPoint)
    {
        float _chkDistSqrt = MathUtility.DistanceSqrtXZ(position, checkPoint);

        // 半径检测
        if (_chkDistSqrt > radiusSqrt)
        {
            return false;
        }

        Vector3 _targetDir = (checkPoint - position).normalized;
        float _chkAngle = Vector3.Angle(_targetDir, direction);
        bool _result = _chkAngle <= angle * .5f;
        return _result;
    }

    /// <summary>
    /// 判断2个点的距离是否小于给定距离
    /// </summary>
    /// <param name="position">检测点</param>
    /// <param name="checkPosition">检测点</param>
    /// <param name="distance">距离</param>
    /// <returns></returns>
    public static bool IsPointsInDistance(Vector3 position, Vector3 checkPosition, float distance)
    {
        return Vector3.SqrMagnitude(checkPosition - position) <= distance * distance;
    }


    public static bool TryGetGroundHeight(Vector3 _position, out float _height)
    {
        RaycastHit hit;
        var ray = new Ray(_position.SetY(150f), Vector3.down);

        if (Physics.Raycast(ray, out hit, 200f, LayerUtility.WalkbleMask))
        {
            _height = hit.point.y;
            return true;
        }
        else
        {
            _height = 0f;
            return false;
        }
    }

    public static bool NM_RayCast(Vector3 sourcePosition, Vector3 targetPosition, ref Vector3 result)
    {
        sourcePosition.y = 0;
        targetPosition.y = 0;

        UnityEngine.AI.NavMeshHit _hit;

        if (UnityEngine.AI.NavMesh.Raycast(sourcePosition, targetPosition, out _hit, UnityEngine.AI.NavMesh.AllAreas))
        {
            result = _hit.position;
            return true;
        }

        return false;
    }
}