using CtuDeviceLib;
using log4net;
using Mushiny;
using OnlineStore.Common;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading.Tasks;
using System.Web.UI.WebControls;

namespace DeviceLibrary
{
    public class PathManger
    {
        static readonly ILog log = Common.LOGGER;
        /// <summary>
        /// 计算移动路径
        /// </summary>
        /// <param name="ctu">需要规划路径的CTU</param>
        /// <param name="end">终点</param>
        /// <param name="pointCodes">返回可路过的点</param>
        /// <param name="errmsg">路径计算失败的原因</param>
        /// <returns></returns>
        public static bool PathCalc(CTU ctu, uint end, out List<PointCode> pointCodes, out string errmsg, List<uint> barrierPoints = null)
        {
            var start = ctu.CurLandMark;
            pointCodes = new List<PointCode>();
            errmsg = "";
            var startPoint = LandMarks.GetPointCode(start);
            var endPoint = LandMarks.GetPointCode(end);
            if (start == end)
            {
                pointCodes.Add(startPoint);
                return true;
            }
            if (startPoint == null)
            {
                errmsg = $"起始点【{start}】未查询到，无法规划";
                return false;
            }
            if (endPoint == null)
            {
                errmsg = $"终点【{end}】未查询到，无法规划";
                return false;
            }
            var startAp = new AStartPoint(startPoint);
            var endAp = new AStartPoint(endPoint);

            AStart aStartAlgo = new AStart();
            var parent = aStartAlgo.FindPath(startAp, endAp, barrierPoints);
            if (parent == null)
            {
                errmsg = $"规划失败";
                return false;
            }
            while (parent != null)
            {
                pointCodes.Add(parent.Landmark);
                parent = parent.ParentPoint;
            }
            pointCodes.Reverse();
            return true; ;
        }
        /// <summary>
        /// 获取两点间的最短距离
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        public static int GetPToPDis(uint p1, uint p2)
        {
            if (p1 == p2) return 0;
            var startPoint = LandMarks.GetPointCode(p1);
            var endPoint = LandMarks.GetPointCode(p2);
            if (startPoint == null || endPoint == null) return int.MaxValue;

            var start = new AStartPoint(startPoint);
            var end = new AStartPoint(endPoint);
            if (start == null || end == null) return int.MaxValue;
            AStart aStartAlgo = new AStart();
            var asp = aStartAlgo.FindPath(start, end);
            if (asp != null)
            {
                return asp.F;
            }
            return int.MaxValue;
        }
        /// <summary>
        /// 获取两点间的运行地码数量
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        public static int GetPToPCount(uint p1, uint p2)
        {
            if (p1 == p2) return 0;
            var startPoint = LandMarks.GetPointCode(p1);
            var endPoint = LandMarks.GetPointCode(p2);
            if (startPoint == null || endPoint == null) return int.MaxValue;

            var start = new AStartPoint(startPoint);
            var end = new AStartPoint(endPoint);
            if (start == null || end == null) return int.MaxValue;
            AStart aStartAlgo = new AStart();
            var asp = aStartAlgo.FindPath(start, end);
            if (asp != null)
            {
                return asp.F;
            }
            return int.MaxValue;
        }
        /// <summary>
        /// 获取当前ctu最近的出料分拣机入口
        /// </summary>
        /// <param name="ctuPos"></param>
        /// <returns></returns>
        public static bool GetNearOutLineIn(uint ctuPos, out string linename)
        {
            linename = "";
            var outLineIns = RobotManager.GetOutLineIns();
            if (outLineIns == null || outLineIns.Count == 0) return false;
            int minDis = int.MaxValue;
            foreach (var item in outLineIns)
            {
                var posInfo = PosInfos.GetPosInfoByName(item);
                if (posInfo == null) continue;
                var dis = GetPToPDis(ctuPos, posInfo.PointCode);
                if (dis < minDis)
                {
                    minDis = dis;
                    linename = item;
                }
            }
            return !string.IsNullOrEmpty(linename);
        }

        /// <summary>
        /// 查找距离给定路径3个点的损失最小(距离目的地最近)的避让点
        /// </summary>
        /// <param name="currentPoint">当前CTU的路径点</param>
        /// <param name="destination">当前CTU的目标点</param>
        /// <param name="usedPathPointCodes">对向CTU的路径点</param>
        /// <returns></returns>
        public static uint FindNoConflitPoint(uint CurLandMark, uint destination, List<PointCode> usedPathPointCodes)
        {
            //所有不在路径上的第一个点
            var firstNoConflitPoints = new List<PointCode>();
            var allreadyCalculatePoints = new List<uint>();
            var waitCalculatePoints = new List<PointCode>();
            waitCalculatePoints.Add(LandMarks.GetPointCode(CurLandMark));
            while (waitCalculatePoints.Count != 0)
            {
                var nextWaitCalculatePoints = new List<PointCode>();
                foreach (var waitPoint in waitCalculatePoints)
                {
                    allreadyCalculatePoints.Add(waitPoint.Id);
                    if (usedPathPointCodes.Contains(waitPoint))
                    {
                        //还在路径上，继续找四周的点
                        if (waitPoint.Above != null && !allreadyCalculatePoints.Contains(waitPoint.Above.Id))
                        {
                            PointCode Above = LandMarks.GetPointCode(waitPoint.Above.Id);
                            nextWaitCalculatePoints.Add(Above);
                        }
                        if (waitPoint.Below != null && !allreadyCalculatePoints.Contains(waitPoint.Below.Id))
                        {
                            PointCode Below = LandMarks.GetPointCode(waitPoint.Below.Id);
                            nextWaitCalculatePoints.Add(Below);
                        }
                        if (waitPoint.Left != null && !allreadyCalculatePoints.Contains(waitPoint.Left.Id))
                        {
                            PointCode Left = LandMarks.GetPointCode(waitPoint.Left.Id);
                            nextWaitCalculatePoints.Add(Left);
                        }
                        if (waitPoint.Right != null && !allreadyCalculatePoints.Contains(waitPoint.Right.Id))
                        {
                            PointCode Right = LandMarks.GetPointCode(waitPoint.Right.Id);
                            nextWaitCalculatePoints.Add(Right);
                        }
                    }
                    else
                    {
                        //不在路径上，加入到不冲突的点
                        firstNoConflitPoints.Add(waitPoint);
                    }

                }
                waitCalculatePoints = nextWaitCalculatePoints;
            }

            var noConflitPoints = firstNoConflitPoints;
            //扩展到距离路径3个点的所有点
            for (int i = 0; i < 3; i++)
            {
                var expandNoConflitPoints = new List<PointCode>();
                foreach (var noConflitPoint in noConflitPoints)
                {
                    if (noConflitPoint.Above != null && !allreadyCalculatePoints.Contains(noConflitPoint.Above.Id))
                    {
                        PointCode Above = LandMarks.GetPointCode(noConflitPoint.Above.Id);
                        expandNoConflitPoints.Add(Above);
                    }
                    if (noConflitPoint.Below != null && !allreadyCalculatePoints.Contains(noConflitPoint.Below.Id))
                    {
                        PointCode Below = LandMarks.GetPointCode(noConflitPoint.Below.Id);
                        expandNoConflitPoints.Add(Below);
                    }
                    if (noConflitPoint.Left != null && !allreadyCalculatePoints.Contains(noConflitPoint.Left.Id))
                    {
                        PointCode Left = LandMarks.GetPointCode(noConflitPoint.Left.Id);
                        expandNoConflitPoints.Add(Left);
                    }
                    if (noConflitPoint.Right != null && !allreadyCalculatePoints.Contains(noConflitPoint.Right.Id))
                    {
                        PointCode Right = LandMarks.GetPointCode(noConflitPoint.Right.Id);
                        expandNoConflitPoints.Add(Right);
                    }
                }
                noConflitPoints = expandNoConflitPoints;
            }


            //查找到目的地路径最近的点
            List<PointCode> nearstNoConflitPathCodes = new List<PointCode>();
            var endAp = new AStartPoint(LandMarks.GetPointCode(destination));
            uint noConflitPointId = 0;
            foreach (var secondNoConflitPoint in noConflitPoints)
            {
                var startAp = new AStartPoint(secondNoConflitPoint);
                AStart aStartAlgo = new AStart();
                var parent = aStartAlgo.FindPath(startAp, endAp);
                var pathCodes = new List<PointCode>();
                while (parent != null)
                {
                    pathCodes.Add(parent.Landmark);
                    parent = parent.ParentPoint;
                }
                if (pathCodes.Count > 0)
                {
                    if (nearstNoConflitPathCodes.Count == 0 || pathCodes.Count <= nearstNoConflitPathCodes.Count)
                    {
                        nearstNoConflitPathCodes = pathCodes;
                        noConflitPointId = secondNoConflitPoint.Id;
                    }
                }
            }
            return noConflitPointId;

        }

        /// <summary>
        /// 查找该ctu最近的待机点
        /// </summary>
        /// <param name="ctu"></param>
        /// <param name="ctuStandbies"></param>
        /// <param name="ctuStandby"></param>
        /// <returns></returns>
        public static bool GetNearStand(CTU ctu, out CtuStandby ctuStandby)
        {
            ctuStandby = null;
            List<CtuStandby> ctuStandbies = CtuStandbys.GetIdleStandbies(ctu);
            if (ctuStandbies == null || ctuStandbies.Count == 0) return false;
            Dictionary<int, CtuStandby> dis = new Dictionary<int, CtuStandby>();
            ctuStandbies.ForEach(s =>
            {
                var dstInfo = PosInfos.GetPosInfoByName(s.Name);
                if (dstInfo != null)
                {
                    int d = GetPToPDis(ctu.CurLandMark, dstInfo.PointCode);
                    dis[d] = s;
                }

            });
            if (dis.Count > 0)
            {
                var minKey = dis.Keys.Min();
                ctuStandby = dis[minKey];
                return ctuStandby != null;
            }
            return ctuStandby != null;
        }
        /// <summary>
        /// 获取指定类型附近的限制区域
        /// </summary>
        /// <param name="ctu"></param>
        /// <param name="usedPoints"></param>
        /// <returns></returns>
        public static List<PointCode> GetNearArea(List<PointCode> usedPoints, LandmarkType specifiedType)
        {
            int len = ConfigAppSettings.GetIntValue("CtuSelfSemiLength", 990, "CTU旋转半径");
            var limitArea = new List<PointCode>();
            if (usedPoints.Count == 0) return limitArea;
            foreach (var ppc in usedPoints)
            {
                if (ppc.Type != specifiedType) continue;
                int sumLen = 0;
                var neighboor = ppc.Above;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Above;
                    }
                }
                sumLen = 0;
                neighboor = ppc.Right;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Right;
                    }
                }
                sumLen = 0;
                neighboor = ppc.Below;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Below;
                    }
                }

                sumLen = 0;
                neighboor = ppc.Left;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Left;
                    }
                }
            }
            return limitArea;
        }

        public static List<PointCode> GetNearArea(uint pointCode, LandmarkType specifiedType = LandmarkType.None)
        {
            int len = ConfigAppSettings.GetIntValue("CtuSelfSemiLength", 990, "CTU旋转半径");
            var limitArea = new List<PointCode>();
            var ppc = LandMarks.GetPointCode(pointCode);
            if (ppc == null) return limitArea;
            //foreach (var ppc in usedPoints)
            {
                if (specifiedType != LandmarkType.None)
                {
                    if (ppc.Type != specifiedType) return limitArea;
                }
                int sumLen = 0;
                var neighboor = ppc.Above;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Above;
                    }
                }
                sumLen = 0;
                neighboor = ppc.Right;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Right;
                    }
                }
                sumLen = 0;
                neighboor = ppc.Below;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Below;
                    }
                }

                sumLen = 0;
                neighboor = ppc.Left;
                while (neighboor != null)
                {
                    var dis = neighboor.Distance;
                    var id = neighboor.Id;
                    sumLen += dis;
                    var code = LandMarks.GetPointCode(id);
                    if (code != null)
                        limitArea.Add(code);
                    if (sumLen > len)
                    {
                        break;
                    }
                    else
                    {
                        var tmpPP = LandMarks.GetPointCode(id);
                        neighboor = tmpPP.Left;
                    }
                }
            }
            return limitArea;
        }
    }
    public class AStart
    {
        static ILog log = Mushiny.Common.LOGGER;
        List<AStartPoint> CloseList;
        List<AStartPoint> OpenList;
        public const int OBLIQUE = 14;
        public const int STEP = 10;

        public AStart()
        {
            OpenList = new List<AStartPoint>();
            CloseList = new List<AStartPoint>();
        }

        public AStartPoint FindPath(AStartPoint start, AStartPoint end, List<uint> barrierPoints = null)
        {
            OpenList = new List<AStartPoint>();
            CloseList = new List<AStartPoint>();
            OpenList.Add(start);
            while (OpenList.Count != 0)
            {
                //找出F值最小的点
                var tempStart = OpenList.MinPoint();
                OpenList.Remove(tempStart);
                CloseList.Add(tempStart);
                //找出它相邻的点
                var surroundPoints = SurrroundPoints(tempStart, barrierPoints);
                foreach (AStartPoint point in surroundPoints)
                {
                    if (OpenList.Exists(point))
                        //计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
                        FoundPoint(tempStart, point);
                    else if (!CloseList.Exists(point))
                        //如果它们不在开始列表里, 就加入, 并设置父节点,并计算GHF
                        NotFoundPoint(tempStart, end, point);
                }
                if (OpenList.Get(end) != null)
                    return OpenList.Get(end);
            }

            return OpenList.Get(end);
        }

        private void FoundPoint(AStartPoint tempStart, AStartPoint point)
        {
            var G = CalcG(tempStart, point);
            if (G < point.G)
            {
                point.ParentPoint = tempStart;
                point.G = G;
                point.CalcF();
            }
            log.Debug($"寻路算法-在开始列表：{point.Id}");
        }

        private void NotFoundPoint(AStartPoint tempStart, AStartPoint end, AStartPoint point)
        {
            point.ParentPoint = tempStart;
            point.G = CalcG(tempStart, point);
            point.H = CalcH(end, point);
            point.CalcF();
            OpenList.Add(point);
            log.Debug($"寻路算法-{tempStart.Id}->{point.Id}不在开始列表，{point.Id} G=【{point.G}】");
        }

        private int CalcG(AStartPoint start, AStartPoint point)
        {
            var dis = start.Landmark.GetDis(point.Id);
            // int G = (Math.Abs(point.X - start.X) + Math.Abs(point.Y - start.Y)) == 2 ? STEP : OBLIQUE;
            int parentG = point.ParentPoint != null ? point.ParentPoint.G : 0;
            return dis + parentG;
        }

        private int CalcH(AStartPoint end, AStartPoint point)
        {
            int step = Math.Abs(point.X - end.X) + Math.Abs(point.Y - end.Y);
            return step * STEP;
        }

        /// <summary>
        /// 获取某个点周围可以到达的点
        /// </summary>
        /// <param name="point"></param>
        /// <param name="barrierPoints">不可走的点</param>
        /// <returns></returns>
        public List<AStartPoint> SurrroundPoints(AStartPoint point, List<uint> barrierPoints)
        {
            var surroundPoints = new List<AStartPoint>();

            if (point.Landmark.Above != null)
            {
                var pid = point.Landmark.Above.Id;
                if (barrierPoints == null || !barrierPoints.Contains(pid))
                {
                    var p = new AStartPoint(LandMarks.GetPointCode(pid));
                    //  if (!barrierPoints.HasBarrier(p) && PathWays.IsTwoWay(point.Id, point.Landmark.Above.Id))
                    surroundPoints.Add(p);
                }
            }
            if (point.Landmark.Right != null)
            {
                var pid = point.Landmark.Right.Id;
                if (barrierPoints == null || !barrierPoints.Contains(pid))
                {
                    var p = new AStartPoint(LandMarks.GetPointCode(pid));
                    //   if (!barrierPoints.HasBarrier(p) && PathWays.IsTwoWay(point.Id, point.Landmark.Right.Id))
                    surroundPoints.Add(p);
                }
            }
            if (point.Landmark.Below != null)
            {
                var pid = point.Landmark.Below.Id;
                if (barrierPoints == null || !barrierPoints.Contains(pid))
                {
                    var p = new AStartPoint(LandMarks.GetPointCode(pid));
                    //   if (!barrierPoints.HasBarrier(p) && PathWays.IsTwoWay(point.Id, point.Landmark.Below.Id))
                    surroundPoints.Add(p);
                }
            }
            if (point.Landmark.Left != null)
            {
                var pid = point.Landmark.Left.Id;
                if (barrierPoints == null || !barrierPoints.Contains(pid))
                {
                    var p = new AStartPoint(LandMarks.GetPointCode(pid));
                    //if (!barrierPoints.HasBarrier(p) && PathWays.IsTwoWay(point.Id, point.Landmark.Left.Id))
                    surroundPoints.Add(p);
                }
            }
            return surroundPoints;
        }
    }




    public class AStartPoint
    {
        public uint Id { get { return Landmark.Id; } }
        public AStartPoint ParentPoint { get; set; }
        public int F { get; set; }  //F=G+H
        public int G { get; set; }
        public int H { get; set; }
        public int X { get { return Landmark.X; } }
        public int Y { get { return Landmark.Y; } }
        public PointCode Landmark { get; set; }
        public AStartPoint(PointCode landmark)
        {
            Landmark = landmark;
        }
        public void CalcF()
        {
            this.F = this.G + this.H;
        }
    }

    //对 List<Point> 的一些扩展方法
    public static class ListHelper
    {
        public static bool Exists(this List<AStartPoint> points, AStartPoint point)
        {
            if (points == null || points.Count == 0) return false;
            foreach (AStartPoint p in points)
                if ((p.X == point.X) && (p.Y == point.Y))
                    return true;
            return false;
        }
        /// <summary>
        /// 是否有障碍
        /// </summary>
        /// <param name="points"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public static bool HasBarrier(this List<AStartPoint> points, AStartPoint point)
        {
            if (point == null || point.Landmark == null)
            {
                return true;
            }

            if (points == null || points.Count == 0) return false;
            foreach (AStartPoint p in points)
                if (p.Landmark != null && p.Id == point.Landmark.Id)
                    return true;

            return false;
        }

        public static AStartPoint MinPoint(this List<AStartPoint> points)
        {
            points = points.OrderBy(p => p.F).ToList();
            return points[0];
        }
        public static AStartPoint Get(this List<AStartPoint> points, AStartPoint point)
        {
            foreach (AStartPoint p in points)
                if ((p.X == point.X) && (p.Y == point.Y))
                    return p;
            return null;
        }

        public static void Remove(this List<AStartPoint> points, int x, int y)
        {
            foreach (AStartPoint point in points)
            {
                if (point.X == x && point.Y == y)
                    points.Remove(point);
            }
        }
    }
}