using AGVMapDemo;
using CtuDeviceLib;
using log4net;
using Mushiny;
using OnlineStore.Common;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Drawing.Imaging;
using System.IO;
using System.Linq;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Timers;
using System.Windows.Forms;
using zhouchen.chart;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.ProgressBar;

namespace TheMachine.Model
{
    public partial class Form6 : Form
    {
        private CTUPointCode _agvPosition;
        private List<CTUPointCode> _pathPoints = new List<CTUPointCode>();
        float ratio = 0.019f;//比例
        float addreX = 50;//整体偏移X
        float addreY = 50;//整体偏移Y
        //新的缩放方法参数
        float maxPointX = float.MinValue;
        float maxPointY = float.MinValue;
        float minPointX = float.MaxValue;
        float minPointY = float.MaxValue;
        float RangX = float.MaxValue;
        float RangY = float.MaxValue;
        int screenWidth = 1920;
        int screenHeight = 1080;
        int screenRatio = 2;
        List<CTURobot> cTUs = new List<CTURobot>();
        //object Lock_Update_CTU = new object();
        object Lock_Update_CTUUI = new object();
        float CTU_Width = 50f;
        System.Timers.Timer CTU_timer = new System.Timers.Timer();
        System.Timers.Timer CTU_test_timer = new System.Timers.Timer();
        List<PointCode> PointCodes = new List<PointCode>();
        Bitmap MapBmp;
        Bitmap LimitAreaMapBmp;

        float distanceRatio = 0.04f;
        float markRatio = 5.0f;
        int markFontRatio = 6;
        float LineRatio = 2.0f;
        float CTULineRatio = 2.0f;
        int CTUFontRatio = 12;
        event EventHandler<Bitmap> setimage;
        Graphics mapDraw = null;
        Graphics CTUDraw = null;
        Graphics LimitAreaDraw = null;
        //箭头
        // 角度A（以度为单位）  
        float angleA = 135;
        // 长度B  
        float lengthB = 5;
        Dictionary<uint, CTUPointCode> CTUMap = new Dictionary<uint, CTUPointCode>();
        //新的线程同步测试
        private ConcurrentQueue<AGVMapDemo.UpdateCTUData> updateCTUData = new ConcurrentQueue<AGVMapDemo.UpdateCTUData>();

        #region 测试
        List<uint> planway = new List<uint>() { 427, 525, 3001, 3002, 3003, 3004,
                3005, 3006, 3007, 3008, 3009, 3010, 3011, 3012,10011,
                3013, 3014, 3015, 3016, 3017, 3018, 3019, 3020, 539, 487 };
        List<uint> planwaytemp = new List<uint>() { 427, 525, 3001, 3002, 3003, 3004,
                3005, 3006, 3007, 3008, 3009, 3010, 3011, 3012,10011,
                3013, 3014, 3015, 3016, 3017, 3018, 3019, 3020, 539, 487 };
        List<uint> currentway = new List<uint>() { 3019, 3020, 539, 487, 486, 488, 863 };
        List<uint> occupyway = new List<uint>() { 427 };
        Color ctuclor = Color.Red;
        Color planclor = Color.Orange;
        Color currentclor = Color.Blue;
        Color occupyclor = Color.Green;
        int inedx = 0;

        List<uint> planwayB = new List<uint>() { 524,2901,2902,2903,2904,2905,2906,2907,
            2908,2909,2910,2911,2912,2913,2914,2915,2916,2917,2918,2919,2920,538 };
        List<uint> currentwayB = new List<uint>() { 524, 2901, 2902, 2903 };
        List<uint> occupywayB = new List<uint>() { 524 };
        int inedxB = 0;
        private void CTU_test_timer_Tick(object sender, EventArgs e)
        {
            if (inedx > planway.Count - 2)
            {
                inedx = 0;
                planwaytemp.AddRange(planway);
            }
            else
            {
                inedx++;
                occupyway.RemoveAt(0);
                occupyway.Add(planway[inedx]);
                planwaytemp.RemoveAt(0);
                Update_CTU(3037, planwaytemp, currentway, occupyway, ctuclor, planclor, currentclor, occupyclor, "正常");
            }

            //if (inedxB > planwayB.Count - 2)
            //{
            //    inedxB = 0;
            //}
            //else
            //{
            //    inedxB++;
            //    occupywayB.RemoveAt(0);
            //    occupywayB.Add(planwayB[inedxB]);
            //    Update_CTU(3036, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3035, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3034, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3033, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3032, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3031, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3030, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3038, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3039, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3020, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3021, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3022, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //    Update_CTU(3023, planwayB, currentwayB, occupywayB, ctuclor, planclor, currentclor, occupyclor, "正常");
            //}
        }
        //private void CTU_test_timer_Tick(object sender, EventArgs e)
        //{
        //    List<uint> planway = new List<uint>() { 427, 525, 3001, 3002, 3003, 3004,
        //        3005, 3006, 3007, 3008, 3009, 3010, 3011, 3012,
        //        3013, 3014, 3015, 3016, 3017, 3018, 3019, 3020, 539, 487 };
        //    List<uint> currentway = new List<uint>() { 427, 525, 3001, 3002, 3003, 3004,

        //         };
        //    List<uint> occupyway = new List<uint>() { 427, 525, 3001,

        //      };
        //    Color ctuclor = Color.Red;
        //    Color planclor = Color.Orange;
        //    Color currentclor = Color.Yellow;
        //    Color occupyclor = Color.Green;

        //    Update_CTU(3037, planway, currentway, occupyway, ctuclor, planclor, currentclor, occupyclor, "正常");

        //}
        #endregion

        private List<(Point, Point)> Get_oneWayEdges(List<CTUPointCode> pointCodes)
        {
            List<Point> points = new List<Point>();
            foreach (CTUPointCode ctuPoint in pointCodes)
            {
                Point point = new Point(ctuPoint.X, ctuPoint.Y, ctuPoint.Id);

                //左
                {
                    if (ctuPoint.Left != null)
                    {
                        uint id = ctuPoint.Left.Id;
                        point.LeftID = id;
                    }
                }
                //右
                {
                    if (ctuPoint.Right != null)
                    {
                        uint id = ctuPoint.Right.Id;
                        point.RightID = id;
                    }
                }
                //上
                {
                    if (ctuPoint.Above != null)
                    {
                        uint id = ctuPoint.Above.Id;
                        point.UpID = id;
                    }
                }
                //下
                {
                    if (ctuPoint.Below != null)
                    {
                        uint id = ctuPoint.Below.Id;
                        point.DownID = id;
                    }
                }

                points.Add(point);
            }
            //为图的坐标点确定上下左右的点
            foreach (var point in points)
            {
                point.Up = points.Find(m => m.Id == point.UpID);
                point.Down = points.Find(m => m.Id == point.DownID);
                point.Left = points.Find(m => m.Id == point.LeftID);
                point.Right = points.Find(m => m.Id == point.RightID);
            }
            //输出所有的边
            // 用于存储单向联通的边  
            List<(Point, Point)> oneWayEdges = new List<(Point, Point)>();

            foreach (var point in points)
            {
                if (point.Up != null && !oneWayEdges.Contains((point.Up, point)))
                {
                    oneWayEdges.Add((point, point.Up));
                }
                if (point.Down != null && !oneWayEdges.Contains((point.Down, point)))
                {
                    oneWayEdges.Add((point, point.Down));
                }
                if (point.Left != null && !oneWayEdges.Contains((point.Left, point)))
                {
                    oneWayEdges.Add((point, point.Left));
                }
                if (point.Right != null && !oneWayEdges.Contains((point.Right, point)))
                {
                    oneWayEdges.Add((point, point.Right));
                }
            }
            return oneWayEdges;
        }
       
        private void Draw_LimitArea(List<CTUPointCode> LimitAreaCode, Color LimitAreasColor, Graphics g)
        {
            //lock (Lock_Update_CTUUI)
            {
                #region 测试
                {
                    List<(Point, Point)> oneWayEdges = new List<(Point, Point)>();
                    oneWayEdges = Get_oneWayEdges(LimitAreaCode);

                    //绘制边
                    //Pen CurrentWay = new Pen(ctu.CTUCurrentWayColor, CTULineRatio);
                    Pen CurrentWay = new Pen(LimitAreasColor, 15);
                    foreach (var point in oneWayEdges)
                    {
                        g.DrawLine(CurrentWay, point.Item1.X, point.Item1.Y, point.Item2.X, point.Item2.Y);

                    }
                }
                g.Save();
            }
            #endregion
        }
        /// <summary>
        /// 更新地图限制区域
        /// </summary>
        /// <param name="LimitAreas">所有限制区域的地码</param>
        /// <param name="LimitAreasColor">限制区域颜色</param>
        /// <param name="Transparen">限制区域颜色透明度默认128（半透明）</param>
        public bool Update_LimitArea(List<uint> LimitAreas, Color LimitAreasColor, int Transparen = 128)
        {
            //lock (Lock_Update_CTUUI)
            if(Monitor.TryEnter(Lock_Update_CTUUI,TimeSpan.FromSeconds(2)))
            {
                try
                {
                    List<CTUPointCode> LimitAreaCode = new List<CTUPointCode>();
                    LimitAreaCode = ConvertToCTUPointCode(LimitAreas);
                    Color semiTransparentColor = Color.FromArgb(Transparen, LimitAreasColor); // 128是透明度（0-255），后面是RGB颜色值  

                    if (MapBmp != null)
                    {
                        LimitAreaMapBmp = MapBmp.Clone() as Bitmap;
                        LimitAreaDraw = Graphics.FromImage(LimitAreaMapBmp);
                        // 设置绘图品质  
                        LimitAreaDraw.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
                        LimitAreaDraw.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.HighQualityBicubic;
                        // 绘制背景，填充为白色  
                        Draw_LimitArea(LimitAreaCode, semiTransparentColor, LimitAreaDraw);
                    }
                }
                finally
                {
                    Monitor.Exit(Lock_Update_CTUUI);
                }
                return true;
            }
            else
            {
                return false;
            }

        }
        /// <summary>
        /// 更新单个小车位置信息
        /// </summary>
        /// <param name="Id">小车ID</param>
        /// <param name="PlanWay">小车完整路径</param>
        /// <param name="CurrentWay">小车当前阶段运行路径</param>
        /// <param name="OccupyPoint">小车占用地标</param>
        /// <param name="CTUColor">小车颜色</param>
        /// <param name="PlanWayColor">小车计划路径颜色</param>
        /// <param name="CurrentWayColor">小车当前路径颜色</param>
        /// <param name="OccupyPointColor">小车占用地标颜色</param>
        /// <param name="state">小车状态</param>
        public void Update_CTU(uint Id, List<uint> PlanWay, List<uint> CurrentWay, List<uint> OccupyPoint,
            Color CTUColor, Color PlanWayColor, Color CurrentWayColor, Color OccupyPointColor, string state)
        {

            //lock (Lock_Update_CTU)
            {
                if (cTUs.Count > 0)
                {
                    var ctu = cTUs.Find(m => m.Id == Id);
                    if (ctu != null)
                    {
                        ctu.PlanWay.Clear();
                        ctu.CurrentWay.Clear();
                        ctu.OccupyPoint.Clear();

                        ctu.PlanWay = ConvertToCTUPointCode(PlanWay);
                        ctu.CurrentWay = ConvertToCTUPointCode(CurrentWay);
                        ctu.OccupyPoint = ConvertToCTUPointCode(OccupyPoint);
                        ctu.CTUColor = CTUColor;
                        ctu.CTUPlanWayColor = PlanWayColor;
                        ctu.CTUCurrentWayColor = CurrentWayColor;
                        ctu.CTUOccupyPointColor = OccupyPointColor;
                    }
                    else
                    {
                        CTURobot cTU = new CTURobot();
                        cTU.Id = Id;
                        cTU.PlanWay = ConvertToCTUPointCode(PlanWay);
                        cTU.CurrentWay = ConvertToCTUPointCode(CurrentWay);
                        cTU.OccupyPoint = ConvertToCTUPointCode(OccupyPoint);
                        cTU.CTUColor = CTUColor;
                        cTU.CTUPlanWayColor = PlanWayColor;
                        cTU.CTUCurrentWayColor = CurrentWayColor;
                        cTU.CTUOccupyPointColor = OccupyPointColor;
                        cTUs.Add(cTU);
                    }
                }
                else
                {
                    CTURobot cTU = new CTURobot();
                    cTU.Id = Id;
                    cTU.PlanWay = ConvertToCTUPointCode(PlanWay);
                    cTU.CurrentWay = ConvertToCTUPointCode(CurrentWay);
                    cTU.OccupyPoint = ConvertToCTUPointCode(OccupyPoint);
                    cTU.CTUColor = CTUColor;
                    cTU.CTUPlanWayColor = PlanWayColor;
                    cTU.CTUCurrentWayColor = CurrentWayColor;
                    cTU.CTUOccupyPointColor = OccupyPointColor;
                    cTUs.Add(cTU);
                }
            }
           AGVMapDemo. UpdateCTUData update=new AGVMapDemo.UpdateCTUData();
            update.CTUData = new List<CTURobot>();
            update.CTUData.AddRange(cTUs);
            updateCTUData.Enqueue(update);
            if (updateCTUData.Count > 50)
            {
                updateCTUData.TryDequeue(out AGVMapDemo.UpdateCTUData result);
            }
        }
        List<CTUPointCode> ConvertToCTUPointCode(List<uint> pointCodes)
        {
            CTUPointCode cTU;
            List<CTUPointCode> cTUs = new List<CTUPointCode>();
            foreach (var pathPoint in pointCodes)
            {
                if (CTUMap.TryGetValue(pathPoint, out cTU))
                {
                    cTUs.Add(cTU);
                }
            }
            return cTUs;
        }

        private PointF RotateVector(PointF vector, float angleDegrees)
        {
            float angleRadians = (float)(angleDegrees * Math.PI / 180.0);
            float cosTheta = (float)Math.Cos(angleRadians);
            float sinTheta = (float)Math.Sin(angleRadians);

            float newX = vector.X * cosTheta - vector.Y * sinTheta;
            float newY = vector.X * sinTheta + vector.Y * cosTheta;

            return new PointF(newX, newY);
        }

        private void DrwaMap(Graphics g)
        {
            #region 绘制地图
            //this.Invoke(new Action(UpdateUI));

            // 重新绘制路径和AGV位置  
            //Graphics g = panel1.CreateGraphics();
            Pen pen = new Pen(Color.Blue, LineRatio);
            Brush brushpoint = new SolidBrush(Color.Blue);
            Brush brushTurningpoint = new SolidBrush(Color.Green);
            Brush brush = new SolidBrush(Color.Red);

            // 绘制地标点  
            for (int i = 1; i < _pathPoints.Count; i++)
            {
                if (_pathPoints[i].Type == LandmarkType.Turning)
                {
                    g.FillRectangle(brushTurningpoint, _pathPoints[i].X, _pathPoints[i].Y, markRatio, markRatio);
                    //文字
                    // 设置字体和笔刷  
                    Font font = new Font("Arial", markFontRatio);
                    Brush CTUNameBrush = Brushes.Black;
                    // 要显示的文字
                    string CTUNameText = _pathPoints[i].Id.ToString();
                    PointF location = new PointF(_pathPoints[i].X + 10, _pathPoints[i].Y);
                    // 绘制文字  
                    g.DrawString(CTUNameText, font, CTUNameBrush, location);
                    // 释放字体资源（可选，如果使用IDisposable的对象，最好进行释放）  
                    font.Dispose();
                }
                else
                {
                    g.FillEllipse(brushpoint, _pathPoints[i].X, _pathPoints[i].Y, markRatio, markRatio);

                    //文字
                    // 设置字体和笔刷  
                    Font font = new Font("Arial", markFontRatio);
                    Brush CTUNameBrush = Brushes.Black;
                    // 要显示的文字
                    string CTUNameText = _pathPoints[i].Id.ToString();
                    PointF location = new PointF(_pathPoints[i].X + 10, _pathPoints[i].Y);
                    // 绘制文字  
                    g.DrawString(CTUNameText, font, CTUNameBrush, location);
                    // 释放字体资源（可选，如果使用IDisposable的对象，最好进行释放）  
                    font.Dispose();
                }

            }
            //绘制路径
            //将原有坐标点转换为图论的坐标点
            List<Point> points = new List<Point>();
            foreach (CTUPointCode ctuPoint in _pathPoints)
            {
                Point point = new Point(ctuPoint.X, ctuPoint.Y, ctuPoint.Id);

                //左
                {
                    if (ctuPoint.Left != null)
                    {
                        uint id = ctuPoint.Left.Id;
                        point.LeftID = id;
                    }
                }
                //右
                {
                    if (ctuPoint.Right != null)
                    {
                        uint id = ctuPoint.Right.Id;
                        point.RightID = id;
                    }
                }
                //上
                {
                    if (ctuPoint.Above != null)
                    {
                        uint id = ctuPoint.Above.Id;
                        point.UpID = id;
                    }
                }
                //下
                {
                    if (ctuPoint.Below != null)
                    {
                        uint id = ctuPoint.Below.Id;
                        point.DownID = id;
                    }
                }

                points.Add(point);
            }
            //为图的坐标点确定上下左右的点
            foreach (var point in points)
            {
                point.Up = points.Find(m => m.Id == point.UpID);
                point.Down = points.Find(m => m.Id == point.DownID);
                point.Left = points.Find(m => m.Id == point.LeftID);
                point.Right = points.Find(m => m.Id == point.RightID);
            }
            // 创建GraphTraversal实例并执行遍历  
            GraphTraversal traversal = new GraphTraversal();

            traversal.Traverse(points[0]); // 从点(0, 0)开始遍历  

            // 输出所有的边  
            // 用于存储单向联通的边  
            List<(Point, Point)> oneWayEdges = new List<(Point, Point)>();
            // 用于存储双向联通的边  
            List<(Point, Point)> twoWayEdges = new List<(Point, Point)>();
            oneWayEdges = traversal.oneWayEdges;
            twoWayEdges = traversal.twoWayEdges;
            //绘制单向边
            Pen penOneWay = new Pen(Color.Red, LineRatio);
            foreach (var point in oneWayEdges)
            {
                g.DrawLine(penOneWay, point.Item1.X, point.Item1.Y, point.Item2.X, point.Item2.Y);

                // 计算P1P2的方向向量  
                PointF direction = new PointF(point.Item2.X - point.Item1.X, point.Item2.Y - point.Item1.Y);
                // 计算旋转后的向量（角度A需要转换为弧度）  
                PointF rotatedDirectionA = RotateVector(direction, angleA);
                PointF rotatedDirectionB = RotateVector(direction, -angleA);
                // 规范化旋转后的向量，并乘以长度B得到终点坐标  
                float magnitudeA = (float)Math.Sqrt(rotatedDirectionA.X * rotatedDirectionA.X + rotatedDirectionA.Y * rotatedDirectionA.Y);
                float magnitudeB = (float)Math.Sqrt(rotatedDirectionB.X * rotatedDirectionB.X + rotatedDirectionB.Y * rotatedDirectionB.Y);
                PointF normalizedRotatedDirectionA = new PointF(rotatedDirectionA.X / magnitudeA, rotatedDirectionA.Y / magnitudeA);
                PointF normalizedRotatedDirectionB = new PointF(rotatedDirectionB.X / magnitudeB, rotatedDirectionB.Y / magnitudeB);
                PointF endPointA = new PointF(point.Item2.X + normalizedRotatedDirectionA.X * lengthB, point.Item2.Y + normalizedRotatedDirectionA.Y * lengthB);
                PointF endPointB = new PointF(point.Item2.X + normalizedRotatedDirectionB.X * lengthB, point.Item2.Y + normalizedRotatedDirectionB.Y * lengthB);
                // 绘制线段  
                g.DrawLine(penOneWay, point.Item2.X, point.Item2.Y, endPointA.X, endPointA.Y);
                g.DrawLine(penOneWay, point.Item2.X, point.Item2.Y, endPointB.X, endPointB.Y);


                //g.DrawLine(penOneWay, point.Item2.X, point.Item2.Y, (point.Item2.X + point.Item1.X) / 2, point.Item2.Y + Math.Abs(point.Item2.X - point.Item1.X) / 2);
                //g.DrawLine(penOneWay, point.Item2.X, point.Item2.Y, (point.Item2.X + point.Item1.X) / 2, point.Item2.Y - Math.Abs(point.Item2.X - point.Item1.X) / 2);
            }
            //绘制双向边
            Pen penTwoWay = new Pen(Color.Green, LineRatio);
            foreach (var point in twoWayEdges)
            {
                g.DrawLine(penTwoWay, point.Item1.X, point.Item1.Y, point.Item2.X, point.Item2.Y);

            }
            #endregion
            g.Save();
        }
        private void UpdateCTU(Graphics g)
        {
          
            {
                updateCTUData.TryDequeue(out var data);
                //lock (Lock_Update_CTU)
                {
                    #region 绘制AGV小车，路线
                    //foreach (var ctu in cTUs)
                    foreach (var ctu in data.CTUData)
                    {
                        if (ctu.PlanWay.Count == 0 || ctu.CurrentWay.Count == 0 || ctu.OccupyPoint.Count == 0 || ctu.CTUPlanWayColor == null || ctu.CTUCurrentWayColor == null || ctu.CTUOccupyPointColor == null)
                        {
                            return;
                        }
                        //小车(以固定半透明框代表）
                        Color semiTransparentColor = Color.FromArgb(128, ctu.CTUColor); // 128是透明度（0-255），后面是RGB颜色值  
                        Brush semiTransparentBrush = new SolidBrush(semiTransparentColor);
                        int currpoint = (ctu.OccupyPoint.Count / 2 - 1) > 0 ? (ctu.OccupyPoint.Count / 2 - 1) : 0;
                        LogUtil.debug($"{currpoint},{ctu.OccupyPoint.Count}");
                        var x = ctu.OccupyPoint.ToArray();
                        var CTUPoint = x[currpoint];

                        g.FillRectangle(semiTransparentBrush, CTUPoint.X - CTU_Width / 2, CTUPoint.Y - CTU_Width / 2, CTU_Width, CTU_Width);
                        //文字
                        // 设置字体和笔刷  
                        Font font = new Font("Arial", CTUFontRatio);
                        Brush CTUNameBrush = Brushes.Black;
                        // 要显示的文字
                        string CTUNameText = ctu.Id.ToString();
                        // 测量文字的大小以获取其位置  
                        //SizeF textSize = g.MeasureString(CTUNameText, font);
                        //PointF location = new PointF((panel1.Width - textSize.Width) / 2, (panel1.Height - textSize.Height) / 2);
                        PointF location = new PointF(CTUPoint.X, CTUPoint.Y);
                        // 绘制文字  
                        g.DrawString(CTUNameText, font, CTUNameBrush, location);
                        // 释放字体资源（可选，如果使用IDisposable的对象，最好进行释放）  
                        font.Dispose();


                        //计划路径
                        Pen PlanWay = new Pen(ctu.CTUPlanWayColor, CTULineRatio);
                        for (int i = 1; i < ctu.PlanWay.Count; i++)
                        {
                            g.DrawLine(PlanWay, ctu.PlanWay[i - 1].X, ctu.PlanWay[i - 1].Y, ctu.PlanWay[i].X, ctu.PlanWay[i].Y);
                        }
                        //当前路径
                        //Pen CurrentWay = new Pen(ctu.CTUCurrentWayColor, CTULineRatio);
                        //for (int i = 1; i < ctu.CurrentWay.Count; i++)
                        //{
                        //    g.DrawLine(CurrentWay, ctu.CurrentWay[i - 1].X, ctu.CurrentWay[i - 1].Y, ctu.CurrentWay[i].X, ctu.CurrentWay[i].Y);
                        //}
                        #region 测试
                        {

                            // 用于存储单向联通的边  
                            List<(Point, Point)> oneWayEdges = new List<(Point, Point)>();
                            oneWayEdges = Get_oneWayEdges(ctu.CurrentWay);
                            //绘制边
                            Pen CurrentWay = new Pen(semiTransparentColor, 15);
                            foreach (var point in oneWayEdges)
                            {
                                g.DrawLine(CurrentWay, point.Item1.X, point.Item1.Y, point.Item2.X, point.Item2.Y);
                            }
                        }
                        #endregion


                        //占用路径
                        #region 新的路径显示方法
                        {
                            ////绘制路径
                            ////将原有坐标点转换为图论的坐标点
                            //List<Point> points = new List<Point>();
                            //foreach (CTUPointCode ctuPoint in ctu.OccupyPoint)
                            //{
                            //    Point point = new Point(ctuPoint.X, ctuPoint.Y, ctuPoint.Id);

                            //    //左
                            //    {
                            //        if (ctuPoint.Left != null)
                            //        {
                            //            uint id = ctuPoint.Left.Id;
                            //            point.LeftID = id;
                            //        }
                            //    }
                            //    //右
                            //    {
                            //        if (ctuPoint.Right != null)
                            //        {
                            //            uint id = ctuPoint.Right.Id;
                            //            point.RightID = id;
                            //        }
                            //    }
                            //    //上
                            //    {
                            //        if (ctuPoint.Above != null)
                            //        {
                            //            uint id = ctuPoint.Above.Id;
                            //            point.UpID = id;
                            //        }
                            //    }
                            //    //下
                            //    {
                            //        if (ctuPoint.Below != null)
                            //        {
                            //            uint id = ctuPoint.Below.Id;
                            //            point.DownID = id;
                            //        }
                            //    }

                            //    points.Add(point);
                            //}
                            ////为图的坐标点确定上下左右的点
                            //foreach (var point in points)
                            //{
                            //    point.Up = points.Find(m => m.Id == point.UpID);
                            //    point.Down = points.Find(m => m.Id == point.DownID);
                            //    point.Left = points.Find(m => m.Id == point.LeftID);
                            //    point.Right = points.Find(m => m.Id == point.RightID);
                            //}
                            ////输出所有的边
                            //// 用于存储单向联通的边  
                            //List<(Point, Point)> oneWayEdges = new List<(Point, Point)>();

                            //foreach( var point in points)
                            //{
                            //    if(point.Up != null&& !oneWayEdges.Contains((point.Up,point)))
                            //    {
                            //        oneWayEdges.Add((point, point.Up));
                            //    }
                            //    if (point.Down != null && !oneWayEdges.Contains((point.Down, point)))
                            //    {
                            //        oneWayEdges.Add((point, point.Down));
                            //    }
                            //    if (point.Left != null && !oneWayEdges.Contains((point.Left, point)))
                            //    {
                            //        oneWayEdges.Add((point, point.Left));
                            //    }
                            //    if (point.Right != null && !oneWayEdges.Contains((point.Right, point)))
                            //    {
                            //        oneWayEdges.Add((point, point.Right));
                            //    }
                            //}
                            ////绘制边
                            //Pen OccupyWay = new Pen(ctu.CTUOccupyPointColor, CTULineRatio);
                            //foreach (var point in oneWayEdges)
                            //{
                            //    g.DrawLine(OccupyWay, point.Item1.X, point.Item1.Y, point.Item2.X, point.Item2.Y);

                            //}
                        }
                        #endregion
                        Pen OccupyWay = new Pen(ctu.CTUOccupyPointColor, CTULineRatio);
                        for (int i = 1; i < ctu.OccupyPoint.Count; i++)
                        {
                            g.DrawLine(OccupyWay, ctu.OccupyPoint[i - 1].X, ctu.OccupyPoint[i - 1].Y, ctu.OccupyPoint[i].X, ctu.OccupyPoint[i].Y);
                        }


                    }
                    #endregion
                }
            }
        }
        private int updateUIState = 0;
        private void timer1_Tick(object sender, EventArgs e)
        {
            if (Interlocked.CompareExchange(ref updateUIState, 1, 0) == 0)
            {
                lock (Lock_Update_CTUUI)
                {
                    //if (cTUs != null && mapDraw != null && LimitAreaMapBmp != null)
                    if (updateCTUData.Count>0 && mapDraw != null && LimitAreaMapBmp != null)
                    {
                        Bitmap DrawMapBmp = LimitAreaMapBmp.Clone() as Bitmap;
                        CTUDraw = Graphics.FromImage(DrawMapBmp);
                        // 设置绘图品质  
                        CTUDraw.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
                        CTUDraw.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.HighQualityBicubic;
                        // 绘制背景，填充为白色  
                        UpdateCTU(CTUDraw);
                        setimage?.Invoke(this, DrawMapBmp);
                        DrawMapBmp.Dispose();
                        
                    }
                    Interlocked.Exchange(ref updateUIState, 0);
                }
            }

        }

        public static void Init()
        {
            MushinyManager.Init();
        }
        /// <summary>
        /// 加工坐标点
        /// </summary>
        /// <param name="point">输入点</param>
        /// <param name="ratio">缩放比例</param>
        /// <param name="overturn">是否翻转</param>
        /// <param name="addreX">X轴偏移量</param>
        /// <param name="addreY">Y轴偏移量</param>
        /// <returns></returns>
        CTUPointCode PointProcess(CTUPointCode point, float ratio = 1, bool overturn = true, float addreX = 0, float addreY = 0)
        {
            float x = point.X;
            float y = point.Y;
            if (overturn)
            {
                point.X = y * ratio + addreY;
                point.Y = x * ratio + addreX;
            }
            else
            {
                point.X = x * ratio + addreX;
                point.Y = y * ratio + addreY;
            }

            return point;
        }


        private void AlgoMatch_setimage(object sender, Bitmap e)
        {
            Bitmap temp = e.Clone() as Bitmap;
            this.pictureBox1.Invoke(new Action(() =>
            {
                pictureBox1.Image = temp;
            }));
        }

        public Form6()
        {
            InitializeComponent();
            Init();
            CTU_test_timer.Enabled = true;
            CTU_test_timer.Elapsed += CTU_test_timer_Tick;
            CTU_test_timer.Interval = 2;
            CTU_test_timer.Start();
        }

        private void Form3_Load(object sender, EventArgs e)
        {
            #region 对于加载的地码重新计算坐标
            PointCode tempPoint;
            foreach (var pathPoint in LandMarks.GetPointCodes())
            {
                tempPoint = new PointCode()
                {
                    Above = pathPoint.Above,
                    Below = pathPoint.Below,
                    Id = pathPoint.Id,
                    Name = pathPoint.Name,
                    Type = pathPoint.Type,
                    CanTurning = pathPoint.CanTurning,
                    Left = pathPoint.Left,
                    Right = pathPoint.Right,
                    X = pathPoint.X,
                    Y = pathPoint.Y,
                    Reserved = pathPoint.Reserved
                };
                // 更新路径点  
                PointCodes.Add(tempPoint);
            }
            List<Point_CTU> points = new List<Point_CTU>();
            foreach (PointCode ctuPoint in PointCodes)
            {
                Point_CTU point = new Point_CTU(ctuPoint.X, ctuPoint.Y, ctuPoint.Id);
                #region
                //左
                {
                    if (ctuPoint.Left != null)
                    {
                        uint id = ctuPoint.Left.Id;
                        point.LeftID = id;
                        point.LeftDistance = ctuPoint.Left.Distance;
                    }
                }
                //右
                {
                    if (ctuPoint.Right != null)
                    {
                        uint id = ctuPoint.Right.Id;
                        point.RightID = id;
                        point.RightDistance = ctuPoint.Right.Distance;
                    }
                }
                //上
                {
                    if (ctuPoint.Above != null)
                    {
                        uint id = ctuPoint.Above.Id;
                        point.UpID = id;
                        point.UpDistance = ctuPoint.Above.Distance;
                    }
                }
                //下
                {
                    if (ctuPoint.Below != null)
                    {
                        uint id = ctuPoint.Below.Id;
                        point.DownID = id;
                        point.DownDistance = ctuPoint.Below.Distance;
                    }
                }
                #endregion
                points.Add(point);
            }
            //为图的坐标点确定上下左右的点
            foreach (var point in points)
            {
                point.Up = points.Find(m => m.Id == point.UpID);
                point.Down = points.Find(m => m.Id == point.DownID);
                point.Left = points.Find(m => m.Id == point.LeftID);
                point.Right = points.Find(m => m.Id == point.RightID);
            }
            // 创建GraphTraversal实例并执行遍历  
            GraphTraversal_CTU traversal = new GraphTraversal_CTU();

            traversal.Traverse(points.First(m => m.Id == 617)); // 从点(0, 0)开始遍历  
            //更新原始地码的坐标
            foreach (var point in traversal.PointCodes)
            {
                PointCodes.Find(m => m.Id == point.Id).X = point.X;
                PointCodes.Find(m => m.Id == point.Id).Y = point.Y;
            }
            #endregion
            foreach (var pathPoint in PointCodes)
            {
                _agvPosition = new CTUPointCode()
                {
                    Above = pathPoint.Above,
                    Below = pathPoint.Below,
                    Id = pathPoint.Id,
                    Name = pathPoint.Name,
                    Type = pathPoint.Type,
                    CanTurning = pathPoint.CanTurning,
                    Left = pathPoint.Left,
                    Right = pathPoint.Right,
                    X = pathPoint.X,
                    Y = pathPoint.Y,
                    Reserved = pathPoint.Reserved
                };
                // 更新路径点  
                _pathPoints.Add(PointProcess(_agvPosition, distanceRatio, false, addreX, addreY));
            }
            foreach (var point in _pathPoints)
            {
                if (maxPointX < point.X)
                {
                    maxPointX = point.X;
                }
                if (maxPointY < point.Y)
                {
                    maxPointY = point.Y;
                }
                if (minPointX > point.X)
                {
                    minPointX = point.X;
                }
                if (minPointY > point.Y)
                {
                    minPointY = point.Y;
                }
            }
            RangX = maxPointX - minPointX;
            RangY = maxPointY - minPointY;
            //修正坐标为正数
            #region 修正坐标为正数
            List<CTUPointCode> tempPathPoints = new List<CTUPointCode>();
            foreach (var point in _pathPoints)
            {
                tempPathPoints.Add(PointProcess(point, 1, false, -minPointX + addreX, -minPointY + addreY));
            }
            _pathPoints.Clear();
            _pathPoints = tempPathPoints;

            #endregion
            maxPointX = float.MinValue;
            maxPointY = float.MinValue;
            minPointX = float.MaxValue;
            minPointY = float.MaxValue;
            RangX = float.MaxValue;
            RangY = float.MaxValue;
            foreach (var point in _pathPoints)
            {
                if (maxPointX < point.X)
                {
                    maxPointX = point.X;
                }
                if (maxPointY < point.Y)
                {
                    maxPointY = point.Y;
                }
                if (minPointX > point.X)
                {
                    minPointX = point.X;
                }
                if (minPointY > point.Y)
                {
                    minPointY = point.Y;
                }
            }
            #region 按Y轴镜像   
            List<CTUPointCode> tempPathPointsY = new List<CTUPointCode>();
            foreach (var point in _pathPoints)
            {
                point.Y = (maxPointY) - point.Y + addreY;
                tempPathPointsY.Add(point);
            }
            _pathPoints.Clear();
            _pathPoints = tempPathPointsY;

            #endregion
            #region 处理好的点加载进地图字典
            foreach (var point in _pathPoints)
            {
                CTUMap.Add(point.Id, point);
            }
            #endregion

            MapBmp = new Bitmap(Convert.ToInt32(maxPointX + addreX), Convert.ToInt32(maxPointY + addreY), PixelFormat.Format32bppRgb);
            mapDraw = Graphics.FromImage(MapBmp);
            // 设置绘图品质  
            mapDraw.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
            mapDraw.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.HighQualityBicubic;
            // 绘制背景，填充为白色  
            mapDraw.Clear(Color.White);
            DrwaMap(mapDraw);
            setimage += AlgoMatch_setimage;
            // 保存图像到文件  
            MapBmp.Save("output.png", System.Drawing.Imaging.ImageFormat.Png);
            pictureBox1.Size = new Size(MapBmp.Width, MapBmp.Height);

            setimage?.Invoke(this, MapBmp);
            LimitAreaMapBmp = MapBmp.Clone() as Bitmap;

            CTU_timer.Enabled = true;
            CTU_timer.Elapsed += timer1_Tick;
            CTU_timer.Interval = 100;
            CTU_timer.Start();
        }
        private void Form3_FormClosed(object sender, FormClosedEventArgs e)
        {
            Application.Exit();
            System.Environment.Exit(0);
        }

        private void button1_Click(object sender, EventArgs e)
        {
            List<uint> currentway = new List<uint>() { 3101, 3102, 3103, 3104, 3105, 3106, 3107, 3108 };
            Update_LimitArea(currentway, Color.Red);
            //问题：会遗留之前的限制区域在图上
        }
    }
}