using System.Collections.Generic;
using UnityEngine;
using Thousandto.Core.Base;
using SimpleAI.Planning;
using SceneEditor.Proxy.Plugin;
using Thousandto.Plugins.Common.UniScene;

namespace Thousandto.Plugins.PathGrid
{
    //陆地寻路系统
    public class PathLandSystem : PathGridSystem
    {
        #region 私有变量
        private List<Vector2> _pathPoints2d = new List<Vector2>();
        //A星寻路
        private AStarPlanner wayPointPlanner = null;
        //wayPoint数据
        private WayPointPlanData wayPointWorld = null;
        #endregion

        #region 私有函数
        //在没有wayPoint情况下的纯A星寻路
        private List<Vector2> GetPathNoWayPoint(Vector2 startPosition, Vector2 endPosition)
        {
            _pathPoints2d.Clear();
            var grid = _pathWorld;
            int startNodeIndex = grid.GetPathNodeIndex(startPosition);
            if (grid.IsBlocked(startNodeIndex))
            {
                return null;
            }
            int endNodeIndex = grid.GetPathNodeIndex(endPosition);
            if (grid.IsBlocked(endNodeIndex))
            {
                return null;
            }
            float dx = MathLib.FastAbs(startPosition.x - endPosition.x);
            float dz = MathLib.FastAbs(startPosition.y - endPosition.y);
            float cdx = dx * grid.InvCellSize + 1;
            float cdz = dz * grid.InvCellSize + 1;
            float fdd = cdx * cdz;
            bool result = false;
            if (fdd <= PreSearchSize)
            {
                _pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex, true);
                while (!_pathPlannder.HasPlanCompleted())
                {
                    _pathPlannder.Update(50);
                }
                result = _pathPlannder.HasPlanSucceeded();
                if (!result)
                {
                    if (_pathPlannder.GetAvailableSearchNodeCount() != 0)
                    {
                        // search node pool size is ok, add node in openTable has been fully tested.
                        return null;
                    }
                }
            }
            if (!result)
            {
                _pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex);
                while (!_pathPlannder.HasPlanCompleted())
                {
                    _pathPlannder.Update(50);
                }
            }
            result = _pathPlannder.HasPlanSucceeded();
            if (_pathPlannder.Solution.Count > 0)
            {
                Vector2 last = startPosition;
                _pathPoints2d.Add(startPosition);
                var solution = _pathPlannder.Solution;
                for (int i = solution.Count - 1; i >= 0; --i)
                {
                    var pt = MathLib.ToVector2_XOZ(grid.GetPathNodePos(solution[i].Index));
                    if ((pt - last).sqrMagnitude > Vector2.kEpsilon)
                    {
                        _pathPoints2d.Add(pt);
                        last = pt;
                    }
                }
                if ((endPosition - last).sqrMagnitude > Vector2.kEpsilon)
                {
                    _pathPoints2d.Add(endPosition);
                }
                var finalPath = SmoothPath(_pathPoints2d, endPosition);
                if (finalPath.Count >= 2)
                {
                    return finalPath;
                }
            }
            return null;
        }
        //获取点到waypoint的寻路路径
        private bool GetGridPath(Vector2 startPosition, Vector2 endPosition, List<Vector2> pathPosition)
        {
            var grid = _pathWorld;
            int startNodeIndex = grid.GetPathNodeIndex(startPosition);
            int endNodeIndex = grid.GetPathNodeIndex(endPosition);
            float dx = MathLib.FastAbs(startPosition.x - endPosition.x);
            float dz = MathLib.FastAbs(startPosition.y - endPosition.y);
            float cdx = dx * grid.InvCellSize + 1;
            float cdz = dz * grid.InvCellSize + 1;
            float fdd = cdx * cdz;
            bool result = false;
            if (fdd <= PreSearchSize)
            {
                _pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex, true);
                while (!_pathPlannder.HasPlanCompleted())
                {
                    _pathPlannder.Update(50);
                }
                result = _pathPlannder.HasPlanSucceeded();
                if (!result)
                {
                    if (_pathPlannder.GetAvailableSearchNodeCount() != 0)
                    {
                        return false;
                    }
                }
            }
            if (!result)
            {
                _pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex);
                while (!_pathPlannder.HasPlanCompleted())
                {
                    _pathPlannder.Update(50);
                }
            }
            result = _pathPlannder.HasPlanSucceeded();
            if (_pathPlannder.Solution.Count > 0)
            {
                Vector2 last = startPosition;
                pathPosition.Add(startPosition);
                var solution = _pathPlannder.Solution;
                for (int i = solution.Count - 1; i >= 0; --i)
                {
                    var pt = MathLib.ToVector2_XOZ(grid.GetPathNodePos(solution[i].Index));
                    if ((pt - last).sqrMagnitude > Vector2.kEpsilon)
                    {
                        pathPosition.Add(pt);
                        last = pt;
                    }
                }
                if ((endPosition - last).sqrMagnitude > Vector2.kEpsilon)
                {
                    pathPosition.Add(endPosition);
                }
            }
            return result;
        }
        //寻出waypoint的路径
        public bool searchWayPointPath(int startIndex, int endIndex, List<Vector2> pathPosition)
        {
            if (wayPointWorld.IsNodeBlocked(startIndex))
            {
                WayPointNode node = wayPointWorld.GetNode(startIndex);
                //Debug.LogError(string.Format("{0}这个点放到了阻挡里面",node.name));
                return false;
            }
            if (wayPointWorld.IsNodeBlocked(endIndex))
            {
                WayPointNode node = wayPointWorld.GetNode(endIndex);
                //Debug.LogError(string.Format("{0}这个点放到了阻挡里面", node.name));
                return false;
            }
            FProfiler.Begin("PathLandSystem.searchWayPointPath");
            wayPointPlanner.StartANewPlan(startIndex, endIndex);
            while (!wayPointPlanner.HasPlanCompleted())
                wayPointPlanner.Update(100);
            FProfiler.End();
            if (!wayPointPlanner.HasPlanSucceeded())
            {
                //Debug.LogError(string.Format("MapId:{0} 路点编辑有问题,起始路点和终点路点不连通！！！", Owner.Cfg.MapId));
                return false;
            }
            Vector2 tempPos = Vector2.zero;
            for (int i = wayPointPlanner.Solution.Count - 1; i >= 0; --i)
            {
                Vector3 pos = wayPointWorld.m_pointInfo[wayPointPlanner.Solution[i].Index].pos;
                tempPos.x = pos.x;
                tempPos.y = pos.z;
                pathPosition.Add(tempPos);
            }
            return true;
        }
        private List<Vector2> GetPath(Vector2 startPosition, Vector2 endPosition)
        {
            if (wayPointWorld == null)
            {
                //Debug.LogError(string.Format("MapId:{0} 场景中找不到[WayPoint] WayPointWorld为空",Owner.Cfg.MapId));
                return GetPathNoWayPoint(startPosition, endPosition);
            }
            int startWayPoint = wayPointWorld.GetNearestIndexBypos(ref startPosition);
            int endWaypoint = wayPointWorld.GetNearestIndexBypos(ref endPosition);
            if (startWayPoint == -1 || endWaypoint == -1)
            {
                //Debug.LogError(string.Format("MapId:{0} 场景中找不到[WayPoint]",Owner.Cfg.MapId));
                return GetPathNoWayPoint(startPosition, endPosition);
            }

            _pathPoints2d.Clear();
            int startNodeIndex = _pathWorld.GetPathNodeIndex(startPosition);
            int endNodeIndex = _pathWorld.GetPathNodeIndex(endPosition);
            Vector3 startWayPointPos = wayPointWorld.m_pointInfo[startWayPoint].pos;
            Vector3 endWayPointPos = wayPointWorld.m_pointInfo[endWaypoint].pos;
            Vector2 startWayPoint2d = new Vector2(startWayPointPos.x, startWayPointPos.z);
            Vector2 endWayPoint2d = new Vector2(endWayPointPos.x, endWayPointPos.z);

            _pathPoints2d.Add(startPosition);
            if(!GetGridPath(startPosition, startWayPoint2d, _pathPoints2d))
                return null;
            if(!searchWayPointPath(startWayPoint, endWaypoint, _pathPoints2d))
                return null;
            if(!GetGridPath(endWayPoint2d, endPosition, _pathPoints2d))
                return null;
            var finalPath = SmoothPath(_pathPoints2d, endPosition);
            if (finalPath.Count >= 2)
                return finalPath;
            return null;
        }
        //优化寻路出来的寻路点,把可以去掉的寻路点去掉
        private List<Vector2> SmoothPath(List<Vector2> input, Vector2 endPosition)
        {
            var ret = Path2dPool.sharedInstance.Allocate(input.Count >> 1);
            int count = input.Count;
            if (count > 1)
            {
                ret.Add(input[0]); // add start point
                Vector2 s, e, hit;
                int curEndIndex = -1;
                for (int j = 0; j < count - 1;)
                {
                    s = input[j];
                    int i = j + 1;
                    int si = j;
                    int c = 0;
                    for (; i < count && c < int.MaxValue; ++i)
                    {
                        e = input[i];
                        if ((i - si) > 1 && Raycast2dSafe(s, e, out hit))
                        {
                            break;
                        }
                        else
                        {
                            curEndIndex = i;
                            j = i;
                        }
                        ++c;
                    }
                    ret.Add(input[curEndIndex]);
                }
            }
            return ret;
        }
        #endregion

        #region 公共方法
        public override void Initialize(BaseScene scene)
        {
            base.Initialize(scene);
        }
        public override void Uninitialize()
        {
            base.Uninitialize();
        }
        public bool LoadWayPointData(BaseScene owner)
        {
            Transform trans = owner.SceneObjectsRoot.Find(BaseScene.WayPointRootName);
            if (trans != null)
                wayPointWorld = trans.GetComponent<WayPointPlanData>();
            if (wayPointWorld != null && wayPointWorld.m_pointInfo != null && wayPointWorld.m_maxNeighborNumber > 0)
            {
                wayPointPlanner = new AStarPlanner(wayPointWorld.m_pointInfo.Length + 1, 0);
                wayPointPlanner.Awake();
                wayPointPlanner.Start(wayPointWorld);
            }
            return true;
        }
        //根据目标的半径范围,从起点到终点寻路获取寻路点
        public override List<Vector2> SearchTargetPath(Vector3 startPos, Vector3 endPos, float targetRadius)
        {
            var startIsBlock = IsBlocked(startPos);
            if(startIsBlock)
            {
                //Debug.Log("起点在阻挡里");
                return null;
            }

            var endIsSafe = IsSafe(endPos);
            if (endIsSafe)
            {
                //Debug.Log("终点在传送点里");
            }

            List<Vector2> outPath = Path2dPool.sharedInstance.Allocate(1);

            //如果玩家处于跟目标位置一样的点
            if (startPos == endPos)
            {
                return outPath;
            }

            //如果寻路到的目标点有半径
            if (targetRadius > 0.0f)
            {
                if (Vector3.Distance(startPos, endPos) <= targetRadius)
                {
                    return outPath;
                }
            }

            Vector2 start2d = MathLib.ToVector2_XOZ(startPos);
            Vector2 end2d = MathLib.ToVector2_XOZ(endPos);

            var blocked = IsBlocked(endPos);
            if (blocked)
            {
                //Debug.Log("目标点在阻挡里");
                Vector2 dir = (end2d - start2d).normalized;
                Vector2 newPos = end2d - dir * Mathf.Min(targetRadius, CellSize * 0.5f);
                if(IsBlocked(newPos))
                {
                    //Debug.Log("在半径边缘的目标点还是在阻挡里");
                    return null;
                }
                if (GetCellIndex(start2d) != GetCellIndex(newPos))
                {
                    end2d = newPos;
                }
                else
                {
                    return outPath;
                }
            }

            Math2d.Circle circle;
            circle.Center = end2d;
            circle.Radius = targetRadius;
            bool goDirect = false;
            if(endIsSafe)
            {
                goDirect = CanGoDirect(start2d, end2d);
            }
            else
            {
                goDirect = CanGoDirectSafe(start2d, end2d);
            }
            if (goDirect)
            {
                if (targetRadius > 0)
                {
                    Math2d.LineSegment line;
                    line.Start = start2d;
                    line.End = end2d;
                    var solves = Math2d.Intersect_LineSegment_Circle(circle, line);
                    if (solves != null && solves.Length > 0 && solves[0] != end2d)
                    {
                        end2d = solves[0];
                    }
                    else
                    {
                        return outPath;
                    }
                }
                if (start2d != end2d)
                {
                    outPath = Path2dPool.sharedInstance.Allocate(2);
                    outPath.Add(start2d);
                    outPath.Add(end2d);
                }
                else
                {
                    return outPath;
                }
            }
            else
            {
                var ptlist = GetPath(start2d, end2d);
                if (ptlist == null || ptlist.Count < 2)
                {
                    //Debug.Log("通过waypoint寻路没有寻到");
                    return null;
                }
                Vector2 currPoint = ptlist.Count > 0 ? ptlist[0] : Vector2.zero;
                var _ptlist = Path2dPool.sharedInstance.Allocate(ptlist.Count);
                bool earlyBreak = false;
                _ptlist.Add(currPoint);
                for (int i = 1; i < ptlist.Count; ++i)
                {
                    Vector2 nextPoint = ptlist[i];
                    if (targetRadius > 0)
                    {
                        Math2d.LineSegment line;
                        line.Start = currPoint;
                        line.End = nextPoint;
                        var solves = Math2d.Intersect_LineSegment_Circle(circle, line);
                        if (solves != null && solves.Length > 0)
                        {
                            nextPoint = solves[0];
                            earlyBreak = true;
                        }
                    }
                    _ptlist.Add(nextPoint);
                    if (earlyBreak)
                    {
                        break;
                    }
                    currPoint = nextPoint;
                }
                outPath = _ptlist;
            }
            return outPath;
        }

        public override List<Vector2> SearchPlayerPath(Vector3 startPosition, Vector3 endPosition)
        {
            if (startPosition == endPosition)
            {
                return null;
            }
            else
            {
                Vector2 hit;
                if (!Raycast2d(startPosition, endPosition, out hit))
                {
                    return GetPath(startPosition, endPosition);
                }
            }
            return null;
        }

        public override List<Vector2> SearchOtherPath(Vector3 startPosition, Vector3 endPosition)
        {
            if (startPosition == endPosition)
            {
                return null;
            }
            else
            {
                List<Vector2> ret = Path2dPool.sharedInstance.Allocate(2);
                ret.Add(startPosition);
                ret.Add(endPosition);
                return ret;
            }
        }


        //评价路径，返回-1表示无法寻路到达，返回距离表示到目标点的实际距离
        public override float EvaluatePath(Vector3 startPosition, Vector3 endPosition)
        {
            List<Vector2> outPath = null;
            outPath = SearchTargetPath(startPosition, endPosition, 0f);
            if (outPath == null || outPath.Count < 2)
            {
                //寻路失败，无法到达
                return -1f;
            }

            var result = 0f;
            for(int i = 0; i < outPath.Count - 1; ++i)
            {
                if (IsNotBlockRayCast2d(outPath[i], outPath[i + 1]))
                {
                    result += Vector2.Distance(outPath[i], outPath[i + 1]);
                }
                else
                {
                    //有阻断，无法到达
                    return -1f;
                }
            }
            //寻路失败无法到达
            return result;
        }
        #endregion
    }
}