STATION_COLLECT/JGR/JGRManager.cs

using AmrControl.ADS;
using AmrControl.Common;
using AmrControl.Common.HttpClients;
using AmrControl.DB;
using AmrControl.DB.Models;
using AmrControl.Dto;
using AmrControl.exceptions;
using AmrControl.mq;
using AmrControl.services;
using AmrControl.Vo;
using Microsoft.AspNetCore.SignalR;
using Microsoft.EntityFrameworkCore.Metadata.Internal;
using Newtonsoft.Json;
using NPOI.HSSF.Record;
using NPOI.SS.Formula.Functions;
using NPOI.SS.UserModel;
using NPOI.Util;
using NPOI.XSSF.UserModel;
using NPOI.XWPF.UserModel;
using Org.BouncyCastle.Crypto.Paddings;
using Org.BouncyCastle.Ocsp;
using Org.BouncyCastle.Utilities;
using System.Collections.Concurrent;
using System.Diagnostics.Metrics;
using System.Drawing;
using System.IO;
using System.Net.NetworkInformation;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TwinCAT.Ads.TypeSystem;
using TwinCAT.PlcOpen;
using static NPOI.HSSF.Util.HSSFColor;

namespace AmrControl.JGR
{
    /// <summary>
    /// 车辆控制器，用来控制车辆，通过ADS-EtherCAT系统
    /// </summary>
    public class JGRManager : IJGR, IDisposable
    {
        protected static JGRManager Instance = null;
        private readonly static object balanceLock = new object();
        private IHubContext<ChatHub> hubContext;
        private MyDbHandler dbHandler;
        private int endpointX = 0, endpointY = 0;
        /// <summary>
        /// 仓储入库位
        /// </summary>
        public readonly int[] inPos = new int[2] { 1, 4 };
        /// <summary>
        /// 仓储出库位
        /// </summary>
        public readonly int[] outPos = new int[2] { 1, 4 };

        /// <summary>
        /// 轨道车入库位
        /// </summary>
        public readonly int[] inAmrPos = new int[2] { 30, 3 };
        /// <summary>
        /// 轨道车出库位
        /// </summary>
        public readonly int[] outAmrPos = new int[2] { 30, 3 };
        /// <summary>
        /// 拣选工位坐标
        /// </summary>
        public readonly int[] selectStationPos = new int[2] {29, 7 };
        /// <summary>
        /// 服务地址
        /// </summary>
        private string serviceAddr;
        private readonly IConfiguration _configuration;

        private readonly ITaskExecManager _taskExecManager;

        private readonly IRegistryService _registryService;
        /// <summary>
        /// 车辆任务占用处理
        /// </summary>
        private volatile ConcurrentDictionary<string, string> jgrTaskMap = new();
        /// <summary>
        /// 取消任务
        /// </summary>
        private volatile List<string> cancelTaskIds = new();
        /// <summary>
        /// 启动
        /// </summary>
        private volatile bool started;
        ///// <summary>
        ///// 车的状态更新时间
        ///// </summary>
        //private readonly double querySeconds = 5;

        //包含的车和车的信息,会实时更新
        ConcurrentDictionary<string, JGR_Tc_Model> jgrs = new ConcurrentDictionary<string, JGR_Tc_Model>();
        Dictionary<string, ChargingStationDataModel> chargers = new Dictionary<string, ChargingStationDataModel>();
        Dictionary<string, StationDataModel> stations = new Dictionary<string, StationDataModel>();

        int maxDisplayX = 0, maxDisplayY = 0;
        /// <summary>
        /// 轨道的格子定义
        /// </summary>
        Dictionary<long, GridCell> railGridCells = new Dictionary<long, GridCell>();
        /// <summary>
        /// 仓储的格子定义
        /// </summary>
        Dictionary<long, GridCell> storageGridCells = new Dictionary<long, GridCell>();

        private string[] disableStorageCells = new string[]
        {
            "1,3","1,4","1,5","1,6","2,3","2,4","2,5"
        };
        /// <summary>
        /// 锁对象
        /// </summary>
        Mutex mutex = new Mutex();
        /// <summary>
        /// 这种signalr注入的方式和实际使用chathub并不是同一个，实际上并不齐作用
        /// 这种方式是新建了一个ChatHub
        /// 此处留在这里做个记忆，调了半天。
        /// </summary>
        /// <param name="context"></param>
        public JGRManager(IHubContext<ChatHub> context, 
            MyDbHandler dbHandler, 
            IConfiguration configuration,
            ITaskExecManager taskExecManager,
            IRegistryService registryService)
        {
            Instance = this;
            hubContext = context;
            this.dbHandler = dbHandler;
            this._configuration = configuration;
            this._taskExecManager = taskExecManager;
            this._registryService = registryService;
        }

        public static JGRManager GetInstance()
        {
            //lock (balanceLock)
            //{
            //    if (Instance == null)
            //        Instance = new JGRManager();
            //}

            return Instance;
        }

        public void Dispose()
        {
            //关闭前记录最终位置信息
            dbHandler.saveJGRs(GetJgrs());
            //关闭定时器
            cts.Cancel();
            ctsTask.Cancel();
            started = false;
            _registryService.Stop();
            //关闭任务
            _taskExecManager.stop();
        }

        public bool Init(IServiceProvider services)
        {
            started = true;
            //从数据库加载信息做初始化动作
            InitDB();
            ////初始化车辆坐标
            //foreach(var car in jgrs.Values)
            //{
            //    SetNewXY(car.jgrSN, car.currLocation_X, car.currLocation_Y);
            //}
            //加载地图数据
            LoadGridConfig();
            
            //异步，执行定期查询车辆状态的事情
            queryJgrSatus();
            //异步，执行车的调度
            movingJGRS();
            //监控工站状态
            watchStationState();
            //注册心跳服务启动，此服务包含心跳，
            //同步储位信息，
            //后期同步库存也是从该方法
            _registryService.Start();
            //启动任务
            _taskExecManager.start();
            return started;
        }

        async Task watchStationState()
        {
            while (true)
            {
                try
                {
                    byte[] datas = TcClient.GetInstance().ReadBytes("GVL_STATION.station", 110);
                    List<StationStateVO> stationList = new();
                    int stationPos = 1;
                    for (var i=0; i<datas.Length; i+=10)
                    {
                        stationList.Add(new()
                        {
                            stationPosNo = (stationPos++),
                            //此处按规则新增工站编号信息（位置从仓储开始计算工位）
                            power = Convert.ToBoolean(datas[i] | (datas[i + 1] << 8) | (datas[i + 2] << 16) | (datas[i + 3] << 32)),
                            triColorLight = Convert.ToInt16(datas[i + 4] | (datas[i + 5] << 8) | (datas[i + 6] << 16) | (datas[i + 7] << 32)),
                            wsristStrapMonitorCh1 = Convert.ToInt16(datas[i + 8] | (datas[i + 9] << 8) | (datas[i + 10] << 16) | (datas[i + 11] << 32)),
                            wsristStrapMonitorCh2 = Convert.ToInt16(datas[i + 12] | (datas[i + 13] << 8) | (datas[i + 14] << 16) | (datas[i + 15] << 32)),
                            smokePurifier = Convert.ToBoolean(datas[i + 16] | (datas[i + 17] << 8) | (datas[i + 18] << 16) | (datas[i + 19] << 32)),
                            bigArm_encoding = Convert.ToInt16(datas[i + 20] | (datas[i + 21] << 8) | (datas[i + 22] << 16) | (datas[i + 23] << 32)),
                            bigArm_angle = Convert.ToSingle(datas[i + 24] | (datas[i + 25] << 8) | (datas[i + 26] << 16) | (datas[i + 27] << 32)),
                            smallArm_encoding = Convert.ToInt16(datas[i + 28] | (datas[i + 29] << 8) | (datas[i + 30] << 16) | (datas[i + 31] << 32)),
                            smallArm_angle = Convert.ToSingle(datas[i + 32] | (datas[i + 29] << 33) | (datas[i + 34] << 16) | (datas[i + 35] << 32)),
                        }) ;
                    }
                    hubContext.Clients.All.SendAsync("stationStatus", stationList);
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ex.ToString());
                }
                await Task.Delay(TimeSpan.FromMilliseconds(1000)); // 间隔1000毫秒
            }
        }

        /// <summary>
        /// 设置sianalr集线器
        /// </summary>
        /// <param name="hub"></param>
        public void SetHub(IHubContext<ChatHub> hub)
        {
            bool ok = hubContext == hub;
            hubContext = hub;
            // Console.WriteLine(ok.ToString());
        }

        public bool ResetADS()
        {
            return TcClient.GetInstance().InitAds();
        }

        /// <summary>
        /// 给界面上返回单元格的定义
        /// </summary>
        /// <returns></returns>
        public GridCellsDto getCells()
        {
            GridCellsDto dto = new GridCellsDto();
            dto.maxRow = maxDisplayY;
            dto.maxCol = maxDisplayX;
            dto.railCells = new List<GridCell>(this.railGridCells.Values.ToList());
            dto.storageCells = new List<GridCell>(this.storageGridCells.Values.ToList());
            return dto;
        }

        /// <summary>
        /// 获得车的清单，目前充电桩和车是一个结构
        /// </summary>
        /// <returns></returns>
        public List<JGR_Tc_Model> GetJgrs()
        {
            return jgrs.Values.ToList();
        }

        public List<StationDataModel> getStations()
        {
            return this.stations.Values.ToList();
        }

        public StationDataModel? getStation(string stanCode)
        {
            return this.stations.Values.ToList().Where(item=>item.stationID == stanCode).FirstOrDefault();
        }

        public List<ChargingStationDataModel> getCharges()
        {
            return this.chargers.Values.ToList();
        }

        /// <summary>
        /// 定时查询jgr的状态，并推送到前端
        /// 
        /// </summary>
        /// 
        CancellationTokenSource cts = new CancellationTokenSource();
        async Task queryJgrSatus()
        {
            //uint jgrCycleTime = 2;
            //uint chargerCycleTime = 3;
            //uint stationCycleTime = 4;
            //uint tick = 0;
            while (started)
            {
                List<JGR_Tc_Model> lsJgrs = Get_Tc_Models();
                string strJgr = JsonConvert.SerializeObject(lsJgrs);  
                hubContext.Clients.All.SendAsync("JgrStatus", strJgr);
                await Task.Delay(TimeSpan.FromMilliseconds(500));
            }
            
            ////间隔时间1秒
            //using (var timer = new PeriodicTimer(TimeSpan.FromSeconds(1)))
            //{
            //    int x = 1, y = 1;
            //    //在到达指定周期后执行方法
            //    while (await timer.WaitForNextTickAsync(cts.Token))
            //    {
            //        //遍历每辆车和充电桩,在线的就发，状态变更的也发
            //        tick = (tick + 1) % uint.MaxValue;
            //        if (tick % jgrCycleTime == 0)
            //        {
            //            List<JGR_Tc_Model> lsJgrs = Get_Tc_Models();
            //            string strJgr = JsonConvert.SerializeObject(lsJgrs);
            //            hubContext.Clients.All.SendAsync("JgrStatus", strJgr);
            //            //dbHandler.saveJGRs(lsJgrs);
            //        }

            ////发送工位信息
            //if (tick % stationCycleTime == 0)
            //{

            //}

            ////发送充电桩信息
            //if (tick % chargerCycleTime == 0)
            //{
            //    List<ChargingStationDataModel> lsJgrs = getChargers();
            //    string strCharger = JsonConvert.SerializeObject(lsJgrs);
            //    hubContext.Clients.All.SendAsync("ChargingStatus", strCharger);
            //}


            //发送车的信息
            //x = (x + 1) % 6;
            //if(x ==0)
            //{
            //    x = 1;
            //}
            //List<JGR_Tc_Model> jgrs = this.GetJgrs();
            //JGR_Tc_Model xx = jgrs[0];
            //xx.currLocation_X = (short)x;
            ////转成displayX
            //long key = 0;
            //key = (jgrs[0].currLocation_X & 0xffff);
            //key = key + ((jgrs[0].currLocation_Y & 0xffff) << 16);
            //if(this.storageGridCells.ContainsKey(key))
            //{
            //    xx.displayX = this.storageGridCells[key].displayX;
            //}

            //xx = jgrs[1];
            //xx.currLocation_X = (short)x;
            ////转成displayX
            //key = 0;
            //key = (jgrs[1].currLocation_X & 0xffff);
            //key = key + ((jgrs[1].currLocation_Y & 0xffff) << 16);
            //if (this.railGridCells.ContainsKey(key))
            //{
            //    xx.displayX = this.railGridCells[key].displayX;
            //}

            //string str = JsonConvert.SerializeObject(jgrs);
            //hubContext.Clients.All.SendAsync("JgrStatus", str);

            //List<StationDataModel> stations = this.getStations();
            //str = JsonConvert.SerializeObject(stations);
            //hubContext.Clients.All.SendAsync("StationStatus", str);

            //var item = this.getCharges();
            //str = JsonConvert.SerializeObject(item);
            //hubContext.Clients.All.SendAsync("ChargingStatus", str);
            //}
            //}
        }

        /// <summary>
        /// 初始化数据库信息
        /// </summary>
        void InitDB()
        {
            if (dbHandler == null)
                return;

            //sqlite数据库初始化
            dbHandler.InitSqlite();

            //加载车的信息
            var dbjgrs = dbHandler.getJGRs();
            this.jgrs.Clear();
            foreach (var item in dbjgrs)
            {
                this.jgrs[item.jgrSN] = item;
            }
            //加载充电桩的信息
            var charges = dbHandler.getChargers();
            this.chargers.Clear();
            foreach (var charge in charges)
            {
                this.chargers.Add(charge.csSN, charge);
            }
            //加载工位信息
            var station = dbHandler.getSatations();
            this.stations.Clear();
            foreach (var st in station)
            {
                this.stations.Add(st.stationID, st);
            }
        }

        /// <summary>
        /// 加载地图数据
        /// </summary>
        void LoadGridConfig()
        {
            string filePath = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "files", "grid.xlsx");
            if (File.Exists(filePath))
            {
                try
                {
                    railGridCells.Clear();
                    storageGridCells.Clear();

                    FileStream fs = new FileStream(filePath, FileMode.Open, FileAccess.Read);
                    IWorkbook workbook = new XSSFWorkbook(fs);
                    ISheet sheet = workbook.GetSheet("位置定义");
                    if (sheet != null)
                    {
                        //先判断行列范围，
                        int rowLen = 0, colLen = 0;
                        for (int i = 0; i < 100; i++)
                        {
                            IRow row = sheet.GetRow(i);
                            if (row == null)
                                break;
                            NPOI.SS.UserModel.ICell cell = row.Cells[0];

                            string cellValue = "";
                            if (cell.CellType == CellType.String)
                            {
                                cellValue = cell.StringCellValue;
                            }
                            else if (cell.CellType == CellType.Numeric)
                            {
                                cellValue = cell.NumericCellValue.ToString();
                            }
                            else
                            {
                                cellValue = "";
                            }

                            if (string.IsNullOrEmpty(cellValue) == false)
                            {
                                rowLen = i;
                            }
                        }

                        List<NPOI.SS.UserModel.ICell> cells = sheet.GetRow(0).Cells;
                        for (int i = 1; i < cells.Count; i++)
                        {
                            NPOI.SS.UserModel.ICell cell = cells[i];
                            string cellValue = "";
                            if (cell.CellType == CellType.String)
                            {
                                cellValue = cell.StringCellValue;
                            }
                            else if (cell.CellType == CellType.Numeric)
                            {
                                cellValue = cell.NumericCellValue.ToString();
                            }
                            else
                            {
                                cellValue = "";
                            }

                            if (string.IsNullOrEmpty(cellValue) == false)
                            {
                                colLen = i;
                            }
                        }

                        maxDisplayX = colLen + 1;
                        maxDisplayY = rowLen + 1;
                        //显示的X\Y坐标位置的内容
                        int displayX = 0, displayY = 0;
                        for (int i = 1; i <= rowLen; i++)
                        {
                            displayY = i;
                            for (int j = 1; j <= colLen; j++)
                            {
                                displayX = j;
                                string cellValue = "";
                                NPOI.SS.UserModel.ICell cell = sheet.GetRow(i).GetCell(j, MissingCellPolicy.RETURN_NULL_AND_BLANK);
                                if (cell == null)
                                {
                                    continue;
                                }
                                else if (cell.CellType == CellType.String)
                                {
                                    cellValue = cell.StringCellValue;
                                }
                                else if (cell.CellType == CellType.Numeric)
                                {
                                    cellValue = cell.NumericCellValue.ToString();
                                }
                                else
                                {
                                    continue;
                                }

                                List<string> strValues = new List<string>();
                                //解析内容
                                if (cellValue.Contains('#') == false)
                                {
                                    strValues.Add(cellValue);
                                }
                                else
                                {
                                    //有多个重叠定义
                                    string[] strs = cellValue.Split('#');
                                    foreach (var item in strs)
                                    {
                                        strValues.Add(item.Trim());
                                    }
                                }

                                foreach (string item in strValues)
                                {
                                    //单元格只有一种定义
                                    GridCell gcell = new GridCell();
                                    gcell.displayX = displayX;
                                    gcell.displayY = displayY;

                                    int indexX = 0, indexY = 0, indexS = 0;
                                    indexX = item.IndexOf('X') + 1;
                                    indexY = item.IndexOf('Y');
                                    indexS = item.IndexOf('/');
                                    string strX = item.Substring(indexX, indexY - indexX);
                                    string strY = item.Substring(indexY + 1, indexS - indexY - 1);
                                    if (strX != null && strY != null)
                                    {
                                        gcell.positionX = Convert.ToInt32(strX);
                                        gcell.positionY = Convert.ToInt32(strY);
                                    }

                                    if (item.Contains("R-"))
                                    {
                                        if (railGridCells.Keys.Contains(gcell.GetKey()))
                                        {
                                            Console.WriteLine($"轨道数据中有重复定义的格子：x:{gcell.positionX} y：{gcell.positionY} ");
                                            continue;
                                        }
                                        railGridCells.Add(gcell.GetKey(), gcell);
                                    }
                                    else if (item.Contains("S-"))
                                    {
                                        if (storageGridCells.Keys.Contains(gcell.GetKey()))
                                        {
                                            Console.WriteLine($"仓储数据中有重复定义的格子：x:{gcell.positionX} y：{gcell.positionY} ");
                                            continue;
                                        }
                                        string pos = gcell.positionX.ToString() + "," + gcell.positionY.ToString();
                                        if(disableStorageCells.Contains(pos))
                                        {
                                            gcell.enableUse = false;
                                        } else
                                        {
                                            gcell.enableUse = true;
                                        }
                                        storageGridCells.Add(gcell.GetKey(), gcell);
                                    }

                                    if (item.Contains("/1"))
                                    {
                                        gcell.isEnable = true;
                                    }
                                    else
                                    {
                                        gcell.isEnable = false;
                                        gcell.enableUse = false;
                                    }
                                }
                            }
                        }
                    }
                    //关闭文件
                    fs.Close();
                }
                catch (Exception e)
                {
                    Console.WriteLine("轨道位置定义文件没能正常解析:" + e.Message);
                }
            }
        }

        /// <summary>
        /// 从TC中读取小车的信息
        /// </summary>
        /// <returns></returns>
        public List<JGR_Tc_Model> Get_Tc_Models()
        {
            List<JGR_Tc_Model> list = new List<JGR_Tc_Model>();
            try
            {
                //List<JGR_Tc_Model> railCars = GetJgrs().Where(item=> item.jgrType == 1).ToList();
                //if(railCars !=null && railCars.Count > 0)
                //{
                //    foreach (JGR_Tc_Model car in railCars)
                //    {
                //        byte[] readRFIDCmd = new byte[5];
                //        readRFIDCmd[0] = 0x05;
                //        readRFIDCmd[1] = 0x0F;
                //        WriteCmd(car.jgrSN, readRFIDCmd);
                //        Thread.Sleep(50);
                //    }
                //}
                //先获取小车的数量
                //在读取信息，并分段为小车的数据结构
                int jgrCounts = TcClient.GetInstance().ReadValue<Byte>("GVL_EXPORT.jgrAvailableTotal");
                int size = Marshal.SizeOf<JGR_Ads_Struct>();
                byte[] datas = TcClient.GetInstance().ReadBytes("GVL_EXPORT.jgrStatusArr", jgrCounts * size);
                if (datas == null || datas.Length < jgrCounts * size)
                {
                    return list;
                }
                byte[] buf = new byte[size];
                for (int i = 0; i < jgrCounts; i++)
                {
                    Array.Clear(buf);
                    Array.Copy(datas, i * size, buf, 0, size);
                    JGR_Ads_Struct model = AppHelper.BytesToStruct<JGR_Ads_Struct>(buf);
                    //出厂编号,30个长度带'\0'，有效最多29个字节
                    string jgrSN = getString(model.jgrSN);
                    if (this.jgrs.ContainsKey(jgrSN) == false)
                    {
                        Console.WriteLine("发现的车没有在数据库中配置，车的编号:"+jgrSN);
                        continue;
                    }

                    //JGR_Tc_Model jgr = new JGR_Tc_Model();
                    //jgr.jgrSN = jgrSN;
                    //车的数量以数据库配置为准
                    JGR_Tc_Model jgr = this.jgrs[jgrSN];
                    //  0:仓储车 , 1:运输车, 2:仓储位充电器，3：轨道位充电器
                    //jgr.jgrType = model.jgrType;
                    //车ID,取值范围0~63，不同类型的车/充电器的jgrID可以重复
                    //jgr.jgrID = model.jgrID;
                    //无线模块的地址，长度2字节
                    //jgr.rcAddr = model.rcAddr;
                    //无线模块的频率码，取值0~155，对应频率为 900+RC_FreqCode*0.2，单位MHz，默认频率码为0x32(50)	
                    //jgr.rcFreqCode = model.rcFreqCode;
                    //在线/离线状态 是否在线(通讯是否正常)
                    jgr.isOnline = model.isOnline;
                    //健康状态		=0:正常;   >=1:异常，异常编码看接口给
                    jgr.isHealthy = model.isHealthy;
                    //充电标志,1:充电中，0：未在充电
                    jgr.isCharging = model.isCharging;
                    //充电电压，单位:V
                    jgr.chargeVoltage = model.chargeVoltage;
                    //充电电流，单位:mA
                    jgr.chargeCurrent = model.chargeCurrent;
                    //电池剩余电量，取值 0~100	, 100表示满电量
                    jgr.electricity = model.electricity;
                    //驱动器通电/断电状态
                    jgr.isDriverPowerOn = model.isDriverPowerOn;
                    //(*工作模式		
                    //1: 正常工作模式，即自动控制模式，只需给出运动的目标坐标，动作序列小车自己控制，正常工作时使用;
                    //2：调试模式,即手动控制模式，用于调试，每一个动作都需要发送指令*)
                    jgr.workMode = model.workMode;
                    //行进/停止状态
                    //(* 车体运动状态
                    //0：静止状态
                    //1：X负向移动
                    //2：X正向移动
                    //3：Y负向移动
                    //4：Y正向移动*)
                    jgr.moveStatus = model.moveStatus;
                    //移动速度，单位:m/s
                    jgr.moveSpeed = model.moveSpeed;
                    //当前位置X
                    jgr.currLocation_X = model.currLocation_X;
                    //当前位置Y
                    jgr.currLocation_Y = model.currLocation_Y;
                    //目标位置X
                    jgr.destLocation_X = model.destLocation_X;
                    //目标位置Y
                    jgr.destLocation_Y = model.destLocation_Y;
                    /// <summary>
                    /// 显示位置X,Y坐标,需要根据实际物理坐标转为显示坐标
                    /// </summary>
                    long key = 0;
                    key = (model.currLocation_X & 0xffff);
                    key = key + ((model.currLocation_Y & 0xffff) << 16);
                    if (model.jgrType == (byte)JgrType.RailCar)
                    {
                        if (railGridCells.ContainsKey(key))
                        {
                            jgr.displayX = railGridCells[key].displayX;
                            jgr.displayY = railGridCells[key].displayY;
                        }
                    }
                    else if (model.jgrType == (byte)JgrType.StorageCar)
                    {
                        if (storageGridCells.ContainsKey(key))
                        {
                            jgr.displayX = storageGridCells[key].displayX;
                            jgr.displayY = storageGridCells[key].displayY;
                        }
                    }

                    //车轮是否在位置切换中
                    jgr.isWheelAltering = model.isWheelAltering;
                    //车轮位置
                    jgr.wheelPosition = model.wheelPosition;
                    //4个碰撞传感器，true：碰撞，false:未碰撞
                    jgr.collideSensor = new byte[4];
                    Array.Copy(model.collideSensor, jgr.collideSensor, 4);
                    //X加速度，单位 m/s2
                    jgr.acceleration_X = model.acceleration_X;
                    //Y加速度，单位 m/s2
                    jgr.acceleration_Y = model.acceleration_Y;
                    //Z加速度，单位 m/s2
                    jgr.acceleration_Z = model.acceleration_Z;
                    //XZ角度，单位:°
                    jgr.angleXZ = model.angleXZ;
                    //YZ角度，单位:°
                    jgr.angleYZ = model.angleYZ;
                    //吊篮限位标志，吊篮处于的位置	1：最高点；		2：最低点
                    jgr.basketPosition = model.basketPosition;
                    //吊篮抓夹状态 	1：闭合状态；	2：张开状态
                    jgr.basketGripperStatus = model.basketGripperStatus;
                    //吊篮是否在移动,吊篮运动状态	吊篮运动状态 0：静止;    1：正在上提；  2：正在下放
                    jgr.basketMoveStatus = model.basketMoveStatus;
                    //RFID
                    string rfId = getString(model.rfid);
                    jgr.rfid = rfId.StartsWith("RFID") ? rfId : "";
                    list.Add(jgr);
                }

                return list;
            }
            catch(Exception e)
            {
                Console.WriteLine("从TC读取小车消息异常：" + e.Message);
                return list;
            }
        }

        List<ChargingStationDataModel> getChargers()
        {
            List<ChargingStationDataModel> list = new List<ChargingStationDataModel>();
            //先获取小车的数量
            //在读取信息，并分段为小车的数据结构
            int counts = TcClient.GetInstance().ReadValue<Byte>("GVL_EXPORT.chargerAvailableTotal");
            int size = Marshal.SizeOf<Charge_Ads_Struct>();
            try
            {
                byte[] datas = TcClient.GetInstance().ReadBytes("GVL_EXPORT.chargerStatusArr", counts * size);
                if (datas == null || datas.Length < counts * size)
                {
                    return list;
                }
                byte[] buf = new byte[size];
                for (int i = 0; i < counts; i++)
                {
                    Array.Clear(buf);
                    Array.Copy(datas, i * size, buf, 0, size);
                    Charge_Ads_Struct model = AppHelper.BytesToStruct<Charge_Ads_Struct>(buf);
                    ChargingStationDataModel item = new ChargingStationDataModel();

                    //  0:仓储车 , 1:运输车, 2:仓储位充电器，3：轨道位充电器
                    item.csType = model.chargeType;
                    //出厂编号,30个长度带'\0'，有效最多29个字节
                    item.csSN = getString(model.chargeSN);
                    //车ID,取值范围0~63，不同类型的车/充电器的jgrID可以重复
                    item.csID = model.chargeID;
                    //无线模块的地址，长度2字节
                    item.rcAddr = model.rcAddr;
                    //无线模块的频率码，取值0~155，对应频率为 900+RC_FreqCode*0.2，单位MHz，默认频率码为0x32(50)	
                    item.rcFreqCode = model.rcFreqCode;
                    //在线/离线状态 是否在线(通讯是否正常)
                    item.isOnline = model.isOnline;
                    //健康状态		=0:正常;   >=1:异常，异常编码看接口给
                    item.isHealthy = model.isHealthy;
                    //充电标志,1:充电中，0：未在充电
                    item.isCharging = model.isCharging;
                    item.chargerStatus = model.chargerStatus;
                    //充电电压，单位:V
                    item.chargeVoltage = model.chargeVoltage;
                    //充电电流，单位:mA
                    item.chargeCurrent = model.chargeCurrent;
                    if(this.chargers.ContainsKey(item.csSN))
                    {
                        item.positionX = chargers[item.csSN].positionX;
                        item.positionY = chargers[item.csSN].positionY;
                        item.displayX = chargers[item.csSN].displayX;
                        item.displayY = chargers[item.csSN].displayY;
                    }
                    list.Add(item);
                }

                return list;
            }
            catch (Exception e)
            {
                Console.WriteLine("从TC读取充电桩的消息异常：" + e.Message);
                return list;
            }
        }

        string getString(byte[] data)
        {
            //判断非0的长度,0是字符串结尾
            int index = 0;
            for (int i = 0; i < data.Length; i++)
            {
                if (data[index] == 0)
                {
                    break;
                }
                index++;
            }
            return Encoding.ASCII.GetString(data, 0, index).Trim();
        }

        /// <summary>
        /// 设置新车或者充电桩的新位置
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <returns></returns>
        public string SetNewXY(string jgrSN, int x , int y)
        {
            string ret = "0";
            if(this.jgrs.ContainsKey(jgrSN) || this.chargers.ContainsKey(jgrSN))
            {
                short sx = Convert.ToInt16(x.ToString());
                short sy = Convert.ToInt16(y.ToString());
                byte[] data = new byte[9];
                data[1] = 0x11;
                //有正负
                data[3] = (byte)((sx >> 8) & 0xff);
                data[4] = (byte)(sx & 0xff);
                data[5] = (byte)((sy >> 8) & 0xff);
                data[6] = (byte)(sy & 0xff);
                ret = WriteCmd(jgrSN, data);
            }
            return ret;
        }
        /// <summary>
        /// 执行命令
        /// </summary>
        /// <param name="debugDto"></param>
        /// <returns>="0",正常；="xx"异常信息</returns>
        public string SetCmd(JgrDebugDto debugDto)
        {
            string ret = "0";
            byte[] data = null;

            //如果没有这辆车，或者车不是正常模式，则不让发车
            if (this.jgrs.ContainsKey(debugDto.SN) == false)
                return "未找到车，车的编号:" + debugDto.SN;

            if (this.jgrs[debugDto.SN].isOnline ==  0)
                return "小车没有在线";

            if (this.jgrs[debugDto.SN].isHealthy > 0)
                return "车处于异常状态，需要人工复位";

            var currJGR = this.jgrs[debugDto.SN];
            //如果车处于刹车状态，则解除刹车

            //处理各自动作，最理想的做法是把动作指令统一映射成ADS通信协议，写入ADS
            switch (debugDto.CmdCode)
            {
                /// <summary>
                /// 小车系统重启
                /// </summary>
                case JgrCmdCode.Restart:
                    if(currJGR.workMode != 2 )
                    {
                        return "小车没有处于调试模式";
                    }

                    break;
                /// <summary>
                /// 设置电源
                /// </summary>
                case JgrCmdCode.SetPower:
                    //if (debugDto.Parameters.ContainsKey("switch") == false)
                    //{
                    //    ret = "缺少参数\"switch\"";
                    //    break;
                    //}
                    //else
                    //{
                    //    data = new byte[8];
                    //    data[1] = 5;
                    //    data[3] = (byte)((debugDto.Parameters["switch"] == "on" || debugDto.Parameters["switch"] == "1") ? 1 : 2);
                    //    ret = WriteCmd(debugDto.SN, data);
                    //}
                    break;
                /// <summary>
                /// 设置终点
                /// </summary>
                case JgrCmdCode.SetEndPoint:
                    if (debugDto.Parameters.ContainsKey("X") == false || debugDto.Parameters.ContainsKey("Y") == false)
                    {
                        ret = "缺少参数\"X、Y\"";
                        break;
                    }
                    else
                    {
                        if (int.TryParse(debugDto.Parameters["X"], out endpointX) == false)
                        {
                            endpointX = 0;
                        }

                        if (int.TryParse(debugDto.Parameters["Y"], out endpointY) == false)
                        {
                            endpointY = 0;
                        }

                        if (endpointX == 0 || endpointY == 0)
                        {
                            endpointX = endpointY = 0;
                            ret = "X-Y坐标参数格式有问题!";
                        }
                        else
                        {
                            MovingPath path = new MovingPath();
                            path.jgrSN = debugDto.SN;
                            path.startLocationX = this.jgrs[debugDto.SN].currLocation_X;
                            path.startLocationY = this.jgrs[debugDto.SN].currLocation_Y;
                            path.destLocationX = endpointX;
                            path.destLocationY = endpointY;
                            ret = TryAddPath(debugDto.SN, path);
                        }
                    }
                    break;
                /// <summary>
                /// 移动到设置的终点
                /// </summary>
                case JgrCmdCode.StartMove:
                    //在设置终点中就完成动作了，不需要独立的触发命令

                    break;
                /// <summary>
                /// 正常中止，停在前进方向最近的一格上
                /// </summary>
                case JgrCmdCode.StopNormal:
                    data = new byte[8];
                    data[1] = 0x12;
                    data[3] = 2;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 急停，最短距离停下
                /// </summary>
                case JgrCmdCode.StopEmergency:
                    data = new byte[8];
                    data[1] = 0x12;
                    data[3] = 1;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 前进方向移动到下一格,X增加
                /// </summary>
                case JgrCmdCode.MoveForward:
                    if (this.jgrs[debugDto.SN].GetPathCounts() == 0)
                    {
                        int newX = this.jgrs[debugDto.SN].currLocation_X + 1;
                        int newY = this.jgrs[debugDto.SN].currLocation_Y;
                        //检测这一个格子是否可以前进，如果可以前进，则加入到路径中

                        MovingPath path = new MovingPath();
                        path.jgrSN = debugDto.SN;
                        path.startLocationX = this.jgrs[debugDto.SN].currLocation_X;
                        path.startLocationY = this.jgrs[debugDto.SN].currLocation_Y;
                        path.destLocationX = newX;
                        path.destLocationY = newY;

                        ret = TryAddPath(debugDto.SN, path);
                    }
                    break;
                /// <summary>
                /// 后退一格，X减少
                /// </summary>
                case JgrCmdCode.MoveBack:
                    if (this.jgrs[debugDto.SN].GetPathCounts() == 0)
                    {
                        int newX = this.jgrs[debugDto.SN].currLocation_X - 1;
                        int newY = this.jgrs[debugDto.SN].currLocation_Y;
                        //检测这一个格子是否可以前进，如果可以前进，则加入到路径中

                        MovingPath path = new MovingPath();
                        path.jgrSN = debugDto.SN;
                        path.startLocationX = this.jgrs[debugDto.SN].currLocation_X;
                        path.startLocationY = this.jgrs[debugDto.SN].currLocation_Y;
                        path.destLocationX = newX;
                        path.destLocationY = newY;

                        ret = TryAddPath(debugDto.SN, path);
                    }
                    break;
                /// <summary>
                /// 前进方向左侧移动一格，Y减少
                /// </summary>
                case JgrCmdCode.MoveLeft:
                    if (this.jgrs[debugDto.SN].GetPathCounts() == 0)
                    {
                        int newX = this.jgrs[debugDto.SN].currLocation_X;
                        int newY = this.jgrs[debugDto.SN].currLocation_Y - 1;
                        //检测这一个格子是否可以前进，如果可以前进，则加入到路径中

                        MovingPath path = new MovingPath();
                        path.jgrSN = debugDto.SN;
                        path.startLocationX = this.jgrs[debugDto.SN].currLocation_X;
                        path.startLocationY = this.jgrs[debugDto.SN].currLocation_Y;
                        path.destLocationX = newX;
                        path.destLocationY = newY;

                        ret = TryAddPath(debugDto.SN, path);
                    }
                    break;
                /// <summary>
                /// 前进方向右侧移动一格，Y增加
                /// </summary>
                case JgrCmdCode.MoveRight:
                    if (this.jgrs[debugDto.SN].GetPathCounts() == 0)
                    {
                        int newX = this.jgrs[debugDto.SN].currLocation_X;
                        int newY = this.jgrs[debugDto.SN].currLocation_Y + 1;
                        //检测这一个格子是否可以前进，如果可以前进，则加入到路径中

                        MovingPath path = new MovingPath();
                        path.jgrSN = debugDto.SN;
                        path.startLocationX = this.jgrs[debugDto.SN].currLocation_X;
                        path.startLocationY = this.jgrs[debugDto.SN].currLocation_Y;
                        path.destLocationX = newX;
                        path.destLocationY = newY;

                        ret = TryAddPath(debugDto.SN, path);
                    }
                    break;
                /// <summary>
                /// 车轮置顶
                /// </summary>
                case JgrCmdCode.SetWheelTop:
                    data = new byte[8];
                    data[1] = 6;
                    data[3] = 1;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 车轮置底
                /// </summary>
                case JgrCmdCode.SetWheelBottom:
                    data = new byte[8];
                    data[1] = 6;
                    data[3] = 3;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 车轮置在中间，四轮着地
                /// </summary>
                case JgrCmdCode.SetWheelMiddle:
                    data = new byte[8];
                    data[1] = 6;
                    data[3] = 2;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 吊篮抓取
                /// </summary>
                case JgrCmdCode.BasketUp:
                    data = new byte[8];
                    data[1] = 7;
                    data[3] = 5;
                    data[4] = Convert.ToByte(debugDto.Parameters["Z"]);
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                 /// <summary>
                /// 吊篮释放
                /// </summary>
                case JgrCmdCode.BasketDown:
                    data = new byte[8];
                    data[1] = 7;
                    data[3] = 6;
                    data[4] = Convert.ToByte(debugDto.Parameters["Z"]);
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 吊篮停止运动
                /// </summary>
                case JgrCmdCode.BasketStopMoving:
                    data = new byte[8];
                    data[1] = 7;
                    data[3] = 0;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 吊篮夹子张开
                /// </summary>
                case JgrCmdCode.GripperOpen:
                    data = new byte[8];
                    data[1] = 7;
                    data[3] = 3;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 吊篮夹子闭合
                /// </summary>
                case JgrCmdCode.GripperClose:
                    data = new byte[8];
                    data[1] = 7;
                    data[3] = 4;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                /// <summary>
                /// 开始充电
                /// </summary>
                case JgrCmdCode.StartCharging:
                    //先检查车的位置与充电桩的位置重叠
                    //然后给指定充电桩发送指令
                    foreach (var item in this.chargers)
                    {
                        if (item.Value.positionX == this.jgrs[debugDto.SN].currLocation_X && item.Value.positionY == this.jgrs[debugDto.SN].currLocation_Y)
                        {
                            //给充电桩发送伸出指令
                            data = new byte[8];
                            data[1] = 8;
                            data[3] = 1;
                            data[4] = 1;
                            ret = WriteCmd(item.Value.csSN, data);
                            if (ret != "0")
                                return ret;

                            //给充电桩发送充电指令
                            data[1] = 8;
                            data[3] = 0;
                            data[4] = 1;
                            ret = WriteCmd(item.Value.csSN, data);
                            if (ret != "0")
                                return ret;
                            //给车发送充电指令
                            data[1] = 8;
                            data[3] = 0;
                            data[4] = 1;
                            ret = WriteCmd(debugDto.SN, data);
                            if (ret != "0")
                                return ret;

                            break;
                        }
                    }
                    break;
                /// <summary>
                /// 停止充电
                /// </summary>
                case JgrCmdCode.StopCharging:
                    //先检查车的位置与充电桩的位置重叠
                    //然后给指定充电桩发送指令
                    foreach (var item in this.chargers)
                    {
                        if (item.Value.positionX == this.jgrs[debugDto.SN].currLocation_X && item.Value.positionY == this.jgrs[debugDto.SN].currLocation_Y)
                        {
                            //给充电桩发送停止充电指令
                            data = new byte[8];
                            data[1] = 8;
                            data[3] = 0;
                            data[4] = 0;
                            ret = WriteCmd(item.Value.csSN, data);
                            if (ret != "0")
                                return ret;

                            //给车发送停止充电指令
                            data[1] = 8;
                            data[3] = 0;
                            data[4] = 0;
                            ret = WriteCmd(debugDto.SN, data);
                            if (ret != "0")
                                return ret;

                            //给充电桩发送缩回指令
                            data[1] = 8;
                            data[3] = 1;
                            data[4] = 0;
                            ret = WriteCmd(item.Value.csSN, data);
                            if (ret != "0")
                                return ret;

                            break;
                        }
                    }
                    break;
                //调试模式
                case JgrCmdCode.DebugModel:
                    data = new byte[8];
                    data[1] = 9;
                    data[3] = 2;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                //正常模式
                case JgrCmdCode.NormalModel:
                    data = new byte[8];
                    data[1] = 9;
                    data[3] = 1;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                //车的驱动器通电
                case JgrCmdCode.MonitorPowerOn:
                    data = new byte[8];
                    data[1] = 5;
                    data[3] = 1;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                //车的驱动器断电
                case JgrCmdCode.MonitorPowerOff:
                    data = new byte[8];
                    data[1] = 5;
                    data[3] = 2;
                    ret = WriteCmd(debugDto.SN, data);
                    break;
                default:
                    //不带参数
                    break;
            }

            return ret;
        }

        /// <summary>
        /// 返回"0"是正常
        /// </summary>
        /// <param name="sn"></param>
        /// <param name="data"></param>
        /// <returns></returns>
        public string WriteCmd(string sn , byte[] data)
        {
            string? ret = null;
            try
            {
                mutex.WaitOne();
                ret = "0";
                //1.先写车或充电桩的序列号
                //2.再写命令字节数据
                //3.再写触发标志位
                //4.再回读状态位
                TcClient.GetInstance().WriteValue<String>("insert_random_frame.SN", sn);
                TcClient.GetInstance().WriteValue<byte[]>("insert_random_frame.FrameContentToSendVar", data);
                TcClient.GetInstance().WriteValue<byte>("insert_random_frame.insert", 1);
                //数据读走，状态位置为0
                int timeout = 20;
                while(timeout-- > 0)
                {
                    Thread.Sleep(30);
                    byte val = TcClient.GetInstance().ReadValue<Byte>("insert_random_frame.insert");
                    ret = val.ToString();
                    if (val == 0)
                    {
                        break;
                    }
                }
            }
            catch(Exception e)
            {
                ret = "发送命令失败:"+e.Message;
            } finally
            {
                mutex.ReleaseMutex();
            }
            
            return ret;
        }
        /// <summary>
        /// 尝试为车添加路径，路径需要拆解，并判断是否可行
        /// //目前假设路径上没有禁用的格子
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="path"></param>
        /// <returns></returns>
        string TryAddPath(string jgrSN, MovingPath path)
        {
            string ret = "0";
            if (path.startLocationX == path.destLocationX && path.startLocationY == path.destLocationY)
            {
                //目标一样
                return "起点与终点一致，不用移动";
            }
                
            List<MovingPath> paths = new List<MovingPath>();
            //检查路径是否合理，如果不算直线，则需要拆解为多条路径
            if(path.startLocationX == path.destLocationX || path.startLocationY == path.destLocationY)
            {
                //检查路径是否可行
                if(CheckPathAvaible(jgrSN,path))
                {
                    paths.Add(path);
                    this.jgrs[jgrSN].AddPath(paths);
                    return ret;
                }
                else
                {
                    return "移动路径没有被接受";
                }
            }
            else
            {
                //拆成多段路径
                //如果X轴能走到位置，则走X轴
                MovingPath path1 = new MovingPath();
                MovingPath path2 = new MovingPath();
                MovingPath path3 = new MovingPath();
                if (path.startLocationY > path.destLocationY)
                {
                    path1.startLocationX = path.startLocationX;
                    path1.startLocationY = path.startLocationY;
                    path1.destLocationX = path.startLocationX;
                    path1.destLocationY = path.destLocationY;

                    path2.startLocationX = path1.destLocationX;
                    path2.startLocationY = path1.destLocationY;
                    path2.destLocationX = path.destLocationX;
                    path2.destLocationY = path.destLocationY;

                    if(CheckPathAvaible(jgrSN,path1) && CheckPathAvaible(jgrSN, path2))
                    {
                        paths.Add(path1);
                        paths.Add(path2);
                        this.jgrs[jgrSN].AddPath(paths);
                        return ret;
                    }
                }
                else
                {
                    path1.startLocationX = path.startLocationX;
                    path1.startLocationY = path.startLocationY;
                    path1.destLocationX = path.destLocationX;
                    path1.destLocationY = path.startLocationY;

                    path2.startLocationX = path1.destLocationX;
                    path2.startLocationY = path1.destLocationY;
                    path2.destLocationX = path.destLocationX;
                    path2.destLocationY = path.destLocationY;
                    if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2))
                    {
                        paths.Add(path1);
                        paths.Add(path2);
                        this.jgrs[jgrSN].AddPath(paths);
                        return ret;
                    }
                }

                //从当前位置的上边，下边位置尝试移动
                for (int i = path.startLocationY -1; i >= 1; i--)
                {
                    path1 = new MovingPath();
                    path2 = new MovingPath();
                    path3 = new MovingPath();

                    path1.startLocationX = path.startLocationX;
                    path1.startLocationY = path.startLocationY;
                    path1.destLocationX = path.startLocationX;
                    path1.destLocationY = i;

                    path2.startLocationX = path1.destLocationX;
                    path2.startLocationY = path1.destLocationY;
                    path2.destLocationX = path.destLocationX;
                    path2.destLocationY = path1.destLocationY;

                    path3.startLocationX = path2.destLocationX;
                    path3.startLocationY = path2.destLocationY;
                    path3.destLocationX = path.destLocationX;
                    path3.destLocationY = path.destLocationY;

                    if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2) && CheckPathAvaible(jgrSN, path3))
                    {
                        paths.Add(path1);
                        paths.Add(path2);
                        paths.Add(path3);
                        this.jgrs[jgrSN].AddPath(paths);
                        return ret;
                    }
                }

                for (int i = path.startLocationY + 1; i <= 8 ; i++)
                {
                    path1 = new MovingPath();
                    path2 = new MovingPath();
                    path3 = new MovingPath();

                    path1.startLocationX = path.startLocationX;
                    path1.startLocationY = path.startLocationY;
                    path1.destLocationX = path.startLocationX;
                    path1.destLocationY = i;

                    path2.startLocationX = path1.destLocationX;
                    path2.startLocationY = path1.destLocationY;
                    path2.destLocationX = path.destLocationX;
                    path2.destLocationY = path1.destLocationY;

                    path3.startLocationX = path2.destLocationX;
                    path3.startLocationY = path2.destLocationY;
                    path3.destLocationX = path.destLocationX;
                    path3.destLocationY = path.destLocationY;

                    if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2) && CheckPathAvaible(jgrSN, path3))
                    {
                        paths.Add(path1);
                        paths.Add(path2);
                        paths.Add(path3);
                        this.jgrs[jgrSN].AddPath(paths);
                        return ret;
                    }
                }
            }
            
            return "移动路径没有被接受";
        }

        bool CheckPathAvaible(string jgrSN,MovingPath path)
        {
            Dictionary<long, GridCell> cells = null;
            bool ok = true;
            if (jgrs[jgrSN].jgrType == (byte)JgrType.StorageCar)
            {
                cells = storageGridCells;
            }
            else if (jgrs[jgrSN].jgrType == (byte)JgrType.RailCar)
            {
                cells = railGridCells;
            }
            else
            {
                return false;
            }

            if (path.startLocationX == path.destLocationX)
            {
                //Y轴移动
                int startY = path.startLocationY < path.destLocationY ? path.startLocationY : path.destLocationY;
                int stopY = path.startLocationY > path.destLocationY ? path.startLocationY : path.destLocationY;
                //检查路径是否可行
                for (int i = startY; i <= stopY; i++)
                {
                    long key = 0;
                    key = (path.startLocationX & 0xffff);
                    key = key + ((i & 0xffff) << 16);
                    if(cells.ContainsKey(key)==false || cells[key].isEnable == false)
                    {
                        return false;
                    }
                }
                return true;
            }
            else if (path.startLocationY == path.destLocationY)
            {
                //X轴移动
                int startX = path.startLocationX < path.destLocationX ? path.startLocationX : path.destLocationX;
                int stopX = path.startLocationX > path.destLocationX ? path.startLocationX : path.destLocationX;
                //检查路径是否可行
                for (int i = startX; i <= stopX; i++)
                {
                    long key = 0;
                    key = (i & 0xffff);
                    key = key + ((path.startLocationY & 0xffff) << 16);
                    if (cells.ContainsKey(key)==false || cells[key].isEnable == false)
                    {
                        return false;
                    }
                }
                return true;
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// 定时执行车辆移动的指令
        /// </summary>
        CancellationTokenSource ctsTask = new CancellationTokenSource();
        async Task movingJGRS()
        {
            ///选择没有冲突的路径，执行路径
            ///锁定执行的路径和周边的单元格
            ///读取车的实时位置，解锁已经经过的单元格
            ///没有冲突的路径可以同时执行
            //间隔时间1秒
            /*************************/
            //因为目前每个区域只有一辆车，所以不存在过多的调度问题，有路径可以直接运行
            //以下操作只是针对区域中只有一辆车的情况
            //以下操作只是针对区域中只有一辆车的情况
            //以下操作只是针对区域中只有一辆车的情况
            //以下操作只是针对区域中只有一辆车的情况
            /*************************/
            using (var timer = new PeriodicTimer(TimeSpan.FromSeconds(1)))
            {
                while (await timer.WaitForNextTickAsync(ctsTask.Token))
                {
                    var lsJgrs = jgrs.Values.ToList();
                    for(int i = 0; i< lsJgrs.Count;i++)
                    {
                        var item = Get_Tc_Models().Where(m=>m.jgrSN == lsJgrs[i].jgrSN).First();

                        //如果车正在运动，则不处理
                        //if(item.isOnline == 0 || item.isHealthy > 0 || item.isCharging == 1 || item.isDriverPowerOn == 0 || item.moveStatus != 0 )
                        //{
                        //    continue;
                        //}

                        if (item.isOnline == 0 || item.isHealthy > 0 || item.isCharging == 1  || item.moveStatus != 0)
                        {
                            continue;
                        }

                        MovingPath path = item.GetFirstPath();
                        if (path != null)
                        {
                            if (path.IsRuning == false)
                            {
                                //没开始则开始，如果已经到位置，则移除
                                if (path.startLocationX == (int)(item.currLocation_X) && path.startLocationY == (int)(item.currLocation_Y))
                                {
                                    //如果车的起始位置与路径起始位置一致，则发起移动
                                    //下指令移动
                                    path.IsRuning = true;
                                    path.StartTime = DateTime.Now;
                                    byte[] data = new byte[8];
                                    data[1] = 0x0a;
                                    if(path.destLocationX == path.startLocationX)
                                    {
                                        //移动Y
                                        short len = Convert.ToInt16((path.destLocationY - path.startLocationY).ToString());
                                        //有正负
                                        data[5] = (byte)((len >> 8) & 0xff);
                                        data[6] = (byte)(len & 0xff);
                                        WriteCmd(item.jgrSN, data);
                                    }
                                    else if(path.destLocationY == path.startLocationY)
                                    {
                                        //移动X
                                        short len = Convert.ToInt16((path.destLocationX - path.startLocationX).ToString());
                                        //有正负
                                        data[3] = (byte)((len >> 8) & 0xff);
                                        data[4] = (byte)(len & 0xff);
                                        WriteCmd(item.jgrSN, data);
                                    }
                                    else
                                    {
                                        //异常，正常是不会出现的
                                        item.RemoveFirstPath();
                                        i--;
                                    }
                                }
                                else
                                {
                                    //如果车的起始位置与路径起始位置不一致，则是有异常发生,移除路径
                                    item.RemoveFirstPath();
                                    i--;
                                    Console.WriteLine("规划的路径起点与当前位置不一致，规划的路径被抛弃");
                                }
                            }
                            else
                            {
                                //如果已经移动到位
                                if (path.destLocationX == (int)(item.currLocation_X) && path.destLocationY == (int)(item.currLocation_Y))
                                {
                                    item.RemoveFirstPath();
                                    //下以次还是检查这辆车，好发起新的路径移动
                                    i--;
                                }
                                else
                                {
                                    //如果移动过程中超时
                                    if (DateTime.Now - path.StartTime > TimeSpan.FromSeconds(120))
                                    {
                                        Console.WriteLine("小车移动超时,小车编号：" + item.jgrSN);
                                        //把超时的移动路径移走
                                        item.RemoveFirstPath();
                                        i--;
                                    }
                                }
                            }
                        } //end if (path != null)
                    }//end for
                }//end while
            }
        }

        public string moveJGCar(string jgrSN, int startLocationX, int startLocationY, int destLocationX, int destLocationY)
        {
            
            JGR_Tc_Model curCarState = GetJgrs().Where(item => item.jgrSN == jgrSN).First();
            //没开始则开始，如果已经到位置，则移除
            if (startLocationX == (int)(curCarState.currLocation_X) && startLocationY == (int)(curCarState.currLocation_Y))
            {
                byte[] data = new byte[8];
                data[1] = 0x0a;
                if (destLocationX == startLocationX)
                {
                    //移动Y
                    short len = Convert.ToInt16((destLocationY - startLocationY).ToString());
                    //有正负
                    data[5] = (byte)((len >> 8) & 0xff);
                    data[6] = (byte)(len & 0xff);
                    //移动仓储车到出库位
                    Console.WriteLine(jgrSN + "移动到位置: " + destLocationX + "," + destLocationY);
                    try
                    {
                        return WriteCmd(jgrSN, data);
                    } finally
                    {
                        Console.WriteLine(jgrSN + "移动到位置: " + destLocationX + "," + destLocationY+",指令发送完成");
                    }
                    
                }
                else if (destLocationY == startLocationY)
                {
                    //移动X
                    short len = Convert.ToInt16((destLocationX - startLocationX).ToString());
                    //有正负
                    data[3] = (byte)((len >> 8) & 0xff);
                    data[4] = (byte)(len & 0xff);
                    try
                    {
                        //移动仓储车到出库位
                        Console.WriteLine(jgrSN + "移动到位置: " + destLocationX + "," + destLocationY);
                        return WriteCmd(jgrSN, data);
                    } finally
                    {
                        Console.WriteLine(jgrSN + "移动到位置: " + destLocationX + "," + destLocationY + ",指令发送完成");
                    }
                    
                }
            }
            return "没有进行路径规划";
        } 
        public StationDataModel? getStationByNo(string stanCode)
        {
            return dbHandler.getStationByNo(stanCode);
        }
        /// <summary>
        /// 移动指定车辆到指定位置
        /// </summary>
        /// <param name="free_car"></param>
        /// <param name="positionX"></param>
        /// <param name="positionY"></param>
        public async void moveCarToDestAsync(JGR_Tc_Model free_car, int positionX, int positionY)
        {
            try
            {
                if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
                {
                    Console.WriteLine("没有任务id，不能执行当前业务");
                    return;
                }
                await waitAndMoveCar(free_car, positionX, positionY);
                //等待到达终点
                await waitArrived(free_car.jgrSN, (short)positionX, (short)positionY);
                //通知完成
                notifyTaskCompleted(new string[] { free_car.jgrSN });
            } catch (CancelTaskException ex)
            {
                Console.WriteLine($"异常信息：{ex.Message}");
                if (free_car != null)
                {
                    //发送取消任务成功通知
                    sendCancelTaskSuccess(new string[] { free_car.jgrSN });
                }
            }
        }
        /// <summary>
        /// 发送任务取消成功的消息
        /// </summary>
        private void sendCancelTaskSuccess(string[] jgrSNs)
        {
            RabbitmqService.getInstance().SendMsg(new MqMessage<CancelTaskNotify>()
            {
                msgId = _taskExecManager.genTaskId(),
                msgType = 5,
                msgData = new CancelTaskNotify()
                {
                    taskId = ThreadLocalManager.Get().taskId,
                    cancelTime = DateTime.Now
                }
            });
            removeCancelTask(ThreadLocalManager.Get().taskId);
            if(jgrSNs !=null &&  jgrSNs.Length>0)
            {
                //移除车辆占用
                foreach (var sn in jgrSNs)
                {
                    jgrTaskMap.Remove(sn, out _);
                }
            }
        }

        /// <summary>
        /// 移动车
        /// </summary>
        /// <param name="free_car"></param>
        /// <param name="positionX"></param>
        /// <param name="positionY"></param>
        /// <returns></returns>
        async Task waitAndMoveCar(JGR_Tc_Model free_car, int positionX, int positionY)
        {
            //判断车状态是否被其他任务占用
            await waitCarToFree(free_car);
            //规划路径并执行
            await planPathAndRun(free_car.jgrSN, positionX, positionY);
        }

        /// <summary>
        /// 等待车辆为空闲状态
        /// </summary>
        /// <param name="free_car"></param>
        /// <returns></returns>
        async Task waitCarToFree(JGR_Tc_Model free_car)
        {
            int times = 0;
            bool rs;
            do
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times) % 500 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("等待指定车辆超时"); 
                }
                await Task.Delay(TimeSpan.FromMilliseconds(200));
                rs = checkIsFree(free_car, false);
            } while (!rs);
            //占用车辆
            jgrTaskMap[free_car.jgrSN] = ThreadLocalManager.Get().taskId;
        }
        /// <summary>
        /// 发送异常通知消息
        /// </summary>
        /// <param name="reason"></param>
        private void sendExceptNotify(string reason)
        {
            RabbitmqService.getInstance().SendMsg(new MqMessage<TaskExceptNotify>() {
                msgId = _taskExecManager.genTaskId(),
                msgType = 4,
                msgData = new TaskExceptNotify()
                {
                    taskId = ThreadLocalManager.Get().taskId,
                    exceptReason = reason
                }
            });
        }

        /// <summary>
        /// 移动车到目标位置
        /// </summary>
        /// <param name="free_car"></param>
        /// <param name="positionX"></param>
        /// <param name="positionY"></param>
        public async void moveCarToDestAsync(JgrType jgrType, int positionX, int positionY)
        {
            JGR_Tc_Model? free_car = null;
            try
            {
                if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
                {
                    Console.WriteLine("没有任务id，不能执行当前业务");
                    return;
                }
                free_car = await waitFreeCar(jgrType);
                notifyCarInfo(free_car);
                //规划路径并执行
                await planPathAndRun(free_car.jgrSN, positionX, positionY);
                await waitArrived(free_car.jgrSN, (short)positionX, (short)positionY);
                //int current_x = free_car.currLocation_X;
                //int current_y = free_car.currLocation_Y;
                //if (free_car.currLocation_Y > 3)
                //{
                //    if (positionY > 3)
                //    {
                //        current_y -= 1;
                //    }
                //    else
                //    {
                //        current_y = (short)positionY;
                //    }
                //}
                //else
                //{
                //    current_x = (short)positionX;
                //}
                ////首次运动
                //JGR_Tc_Model curCarState = Get_Tc_Models().Where(item => item.jgrSN == free_car.jgrSN).First();
                //if (curCarState.currLocation_X != (short)current_x || curCarState.currLocation_Y != (short)current_y)
                //{
                //    await retryMoveCar(free_car.jgrSN, current_x, current_y);
                //    await waitArrived(free_car.jgrSN, (short)current_x, (short)current_y);
                //}
                //if (free_car.currLocation_X != (short)positionX)
                //{
                //    current_x = (short)positionX;
                //    curCarState = Get_Tc_Models().Where(item => item.jgrSN == free_car.jgrSN).First();
                //    await retryMoveCar(free_car.jgrSN, current_x, current_y);
                //    await waitArrived(free_car.jgrSN, (short)current_x, (short)current_y);
                //}
                ////再次运动到达目的地
                //curCarState = Get_Tc_Models().Where(item => item.jgrSN == free_car.jgrSN).First();
                //await retryMoveCar(free_car.jgrSN, positionX, positionY);
                notifyTaskCompleted(new string[] { free_car.jgrSN });
            }
            catch (CancelTaskException ex)
            {
                Console.WriteLine($"异常信息：{ex.Message}");
                if(free_car != null)
                {
                    //发送取消任务成功通知
                    sendCancelTaskSuccess(new string[] { free_car.jgrSN });
                }
            } 
        }
        /// <summary>
        /// 通知获取车辆信息给业务端
        /// </summary>
        private void notifyCarInfo(JGR_Tc_Model carInfo, string rfId="")
        {
            //发送消息给业务端
            RabbitmqService.getInstance().SendMsg(new MqMessage<GetCarNotify>()
                {
                    msgId = _taskExecManager.genTaskId(),
                    msgType = 1,
                    msgData = new GetCarNotify()
                    {
                        taskId = ThreadLocalManager.Get().taskId,
                        carId = carInfo.jgrSN,
                        carType = carInfo.jgrType,
                        rfId = rfId
                    }
                }) ;
        }

        /// <summary>
        /// 通知任务完成处理
        /// </summary>
        /// <param name="jgrSNs"></param>
        private void notifyTaskCompleted(string[] jgrSNs)
        {
            //移除车辆占用
            foreach(var sn in jgrSNs)
            {
                jgrTaskMap.Remove(sn, out _);
            }
            //通知任务完成
            RabbitmqService.getInstance().SendMsg(new MqMessage<TaskCompletedNotify>() { 
                    msgId = _taskExecManager.genTaskId(),
                    msgType = 2,
                    msgData = new TaskCompletedNotify()
                    {
                        taskId = ThreadLocalManager.Get().taskId
                    }
            });
            _taskExecManager.notifyTaskCompleted(ThreadLocalManager.Get().taskId);
        }

        /// <summary>
        /// 等待空闲车辆
        /// </summary>
        /// <param name="jgrType"></param>
        /// <returns></returns>
        async Task<JGR_Tc_Model> waitFreeCar(JgrType jgrType)
        {
            int times = 0;
            JGR_Tc_Model? curCarState = null;
            do {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times)%500 == 0)
                {
                    sendExceptNotify("等待空闲车超时");
                }
                await Task.Delay(TimeSpan.FromMilliseconds(200));
                curCarState = findFreeCar(jgrType);
            } while (curCarState == null);
            if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
            {
                throw new CancelTaskException("取消任务处理");
            }
            return curCarState;
        }

        /// <summary>
        /// 仓储车入库操作
        /// </summary>
        /// <param name="dto">入库参数</param>
        public async void storageCarToInAsync(JGR_Tc_Model railCar, InStorageDTO dto)
        {
            JGR_Tc_Model? storageCarInfo = null;
            try
            {
                if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
                {
                    Console.WriteLine("没有任务id，不能执行当前业务");
                    return;
                }
                //等待仓盒信息
                string rfId = await awaitStorage(railCar);
                //移动车辆
                waitAndMoveCar(railCar, inAmrPos[0], inAmrPos[1]);
                //等待直到获取仓储车
                storageCarInfo = await waitFreeCar(JgrType.StorageCar);
                notifyCarInfo(storageCarInfo, rfId);
                //int current_x = storageCarInfo.currLocation_X;
                //int current_y = storageCarInfo.currLocation_Y;
                //规划路径并执行
                await planPathAndRun(storageCarInfo.jgrSN, inPos[0], inPos[1] - 1);
                //到位判断等待
                await waitArrived(storageCarInfo.jgrSN, (short)inPos[0], (short)(inPos[1] - 1));
                ////根据仓储车朝向原则上y轴减去1为车的位置
                //if (current_x != inPos[0] || current_y != inPos[1] - 1)
                //{
                //    //移动车到入库位置
                //    await retryMoveCar(storageCarInfo.jgrSN, inPos[0], inPos[1] - 1);
                //    //到位判断等待
                //    await waitArrived(storageCarInfo.jgrSN, (short)inPos[0], (short)(inPos[1] - 1));
                //}
                //读取轨道车是否到达入库位置
                await waitArrived(railCar.jgrSN, (short)inAmrPos[0], (short)inAmrPos[1]);
                //发送抓取指令
                byte[] data = new byte[8];
                data[1] = 7;
                data[3] = 5;
                data[4] = 0x80;
                await retrySendCmd(storageCarInfo.jgrSN, data);

                //读取吊篮状态是否已恢复到0的位置
                await waitBasketPositionReset(storageCarInfo.jgrSN, 1);

                //移动仓储车到入料位
                int dest_x = int.Parse(dto.storagePos.Split(",")[0]);
                //仓储车相对位置减1
                int dest_y = int.Parse(dto.storagePos.Split(",")[1]) - 1;
                //目标层数
                int dest_z = int.Parse(dto.storagePos.Split(",")[2]);
                //移动车到入库位置
                await planPathAndRun(storageCarInfo.jgrSN, dest_x, dest_y);
                // await retryMoveCar(storageCarInfo.jgrSN, dest_x, dest_y);
                await waitArrived(storageCarInfo.jgrSN, (short)dest_x, (short)dest_y);
                //执行下放抓斗命令
                data = new byte[8];
                data[1] = 7;
                data[3] = 6;
                data[4] = Convert.ToByte(dest_z);
                await retrySendCmd(storageCarInfo.jgrSN, data);
                //读取吊篮状态
                await waitBasketPositionReset(storageCarInfo.jgrSN, 2);
                //将任务完成状态放到集合中等待查询读取
                notifyTaskCompleted(new string[] { dto.carId, storageCarInfo.jgrSN });
            } catch (CancelTaskException ex)
            {
                Console.WriteLine($"异常信息：{ex.Message}");
                string[] cars = new string[1];
                cars[0] = dto.carId;
                if (storageCarInfo != null)
                {
                    cars = new string[2]; 
                    cars[0] = dto.carId;
                    cars[1] = storageCarInfo.jgrSN;
                }
                //发送取消任务成功通知
                sendCancelTaskSuccess(cars);
            }
        }

        async Task<string> awaitStorage(JGR_Tc_Model railCar)
        {
            int times = 0;
            string rfId = "";
            do
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times) % 1000 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("等待放置仓盒超时");
                }
                await Task.Delay(TimeSpan.FromSeconds(1));
                rfId = GetJgrs().Where(car => railCar.jgrSN.Equals(car.jgrSN)).First().rfid;
            } while(string.IsNullOrEmpty(rfId) || string.IsNullOrWhiteSpace(rfId));
            return rfId;
        }

        /// <summary>
        /// 等待仓储车吊篮状态复位到0的位置
        /// </summary>
        /// <param name="jgrSN">仓储车id</param>
        /// <param name="checkGripperState">验证gripper状态</param>
        /// <returns></returns>
        async Task waitBasketPositionReset(string jgrSN, int checkGripperState)
        {
            int times = 0;
            JGR_Tc_Model? carState = null;
            bool isReady = false;
            do
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times) % 1000 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("等待仓储车吊篮状态复位超时");
                }
                await Task.Delay(TimeSpan.FromMilliseconds(100));
                carState = Get_Tc_Models().Where(item => item.jgrSN == jgrSN).First();
                isReady = carState.basketPosition == 0 && carState.basketGripperStatus == checkGripperState;
            } while (!isReady);
        }



        /// <summary>
        /// 重试指令
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="data"></param>
        /// <returns></returns>
        async Task retrySendCmd(string jgrSN, byte[] data)
        {
            int times = 0;
            string ret;
            do
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times) % 1000 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("发送车辆操作指令超时");
                }
                await Task.Delay(TimeSpan.FromMilliseconds(100));
                //车辆到达入料位置，发起下放抓取指令
                ret = WriteCmd(jgrSN, data);
            } while (ret != "0");
        }

        /// <summary>
        /// 可重试移动车位置，暂不考虑退出
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="dx"></param>
        /// <param name="dy"></param>
        /// <returns></returns>
        async Task retryMoveCar(string jgrSN, int dx, int dy)
        {
            int times = 0;
            string str;
            JGR_Tc_Model carState = Get_Tc_Models().Where(item => item.jgrSN == jgrSN).First();
            int current_x = carState.currLocation_X;
            int current_y = carState.currLocation_Y;
            do
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                if ((++times) % 1000 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("移动车辆指令重试超时");
                }
                await Task.Delay(TimeSpan.FromMilliseconds(100));
                str = moveJGCar(carState.jgrSN, current_x, current_y, dx, dy);
            } while (!"0".Equals(str));
        }

        /// <summary>
        /// 判断是否达到指定坐标位置，此处暂不考虑长时间未到达的情况，后期可于此做退出机制
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="dx"></param>
        /// <param name="dy"></param>
        /// <returns></returns>
        public async Task waitArrived(string jgrSN, short dx, short dy)
        {
            int times = 0;
            JGR_Tc_Model curCarState = Get_Tc_Models().Where(item => item.jgrSN == jgrSN).First();
            while (curCarState.currLocation_X != dx || curCarState.currLocation_Y != dy)
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {

                    throw new CancelTaskException("取消指令操作");
                }
                if ((++times) % 500 == 0)
                {
                    //发送超时消息
                    sendExceptNotify("等待车辆到达超时");
                }
                await Task.Delay(TimeSpan.FromMilliseconds(200));
                curCarState = Get_Tc_Models().Where(item => item.jgrSN == jgrSN).First();
            }
        }
        /// <summary>
        /// 出库操作逻辑
        /// </summary>
        /// <param name="storagePos">存储位置</param>
        /// <param name="amrDx">amr运动目标坐标位置x</param>
        /// <param name="amrDy">amr运动目标坐标位置y</param>
        /// <exception cref="NotImplementedException"></exception>
        public async void storageCarToOutAsync(string storagePos, int amrDx, int amrDy)
        {
            JGR_Tc_Model? railCar = null;
            JGR_Tc_Model? storageCarInfo = null;
            try
            {
                if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
                {
                    Console.WriteLine("没有任务id，不能执行当前业务");
                    return;
                }
                //呼叫轨道车到出库位
                railCar = await waitFreeCar(JgrType.RailCar);
                notifyCarInfo(railCar);
                //移动车辆到出库位
                planPathAndRun(railCar.jgrSN, outAmrPos[0], outAmrPos[1]);
                //获取仓储车
                storageCarInfo = await waitFreeCar(JgrType.StorageCar);
                //移动仓储车到储位拿取
                //int current_x = storageCarInfo.currLocation_X;
                //int current_y = storageCarInfo.currLocation_Y;

                int dx = int.Parse(storagePos.Split(',')[0]);
                int dy = int.Parse(storagePos.Split(',')[1]) - 1;
                int dz = int.Parse(storagePos.Split(',')[2]);
                await planPathAndRun(storageCarInfo.jgrSN, dx, dy);
                //到位判断等待
                await waitArrived(storageCarInfo.jgrSN, (short)dx, (short)dy);
                Console.WriteLine(storageCarInfo.jgrSN + "已经到达位置: " + dx + "," + dy);
                ////根据仓储车朝向原则上y轴减去1为车的位置
                //if (current_x != dx || current_y != dy)
                //{
                //    //移动车到抓取位置
                //    await retryMoveCar(storageCarInfo.jgrSN, dx, dy);
                //    //到位判断等待
                //    await waitArrived(storageCarInfo.jgrSN, (short)dx, (short)dy);
                //}

                //移动到位发送抓取指令
                byte[] data = new byte[8];
                data[1] = 7;
                data[3] = 5;
                data[4] = Convert.ToByte(dz);
                await retrySendCmd(storageCarInfo.jgrSN, data);
                Console.WriteLine(storageCarInfo.jgrSN + "发送抓取指令,指令内容：", string.Join(',', data));
                //读取吊篮状态
                await waitBasketPositionReset(storageCarInfo.jgrSN, 1);
                Console.WriteLine(storageCarInfo.jgrSN + "等待吊篮上升到最高层，状态为抓取状态：");
                await planPathAndRun(storageCarInfo.jgrSN, outPos[0], outPos[1] - 1);
                ////移动仓储车到出库位
                //await retryMoveCar(storageCarInfo.jgrSN, outPos[0], outPos[1] - 1);
                //到位判断等待
                await waitArrived(storageCarInfo.jgrSN, (short)outPos[0], (short)(outPos[1] - 1));
                Console.WriteLine(storageCarInfo.jgrSN + "已经到达位置: " + outPos[0].ToString() + "," + (outPos[1] - 1).ToString());
                //等待轨道车是否到达出库位
                await waitArrived(railCar.jgrSN, (short)outAmrPos[0], (short)outAmrPos[1]);
                Console.WriteLine(railCar.jgrSN + "已经到达位置: " + outAmrPos[0].ToString() + "," + outAmrPos[1].ToString());
                //发送下放指令
                data = new byte[8];
                data[1] = 7;
                data[3] = 6;
                data[4] = 0x80;
                await retrySendCmd(storageCarInfo.jgrSN, data);
                Console.WriteLine(storageCarInfo.jgrSN + "发送下放指令,指令内容：", string.Join(",", data));
                //读取吊篮状态
                await waitBasketPositionReset(storageCarInfo.jgrSN, 2);
                Console.WriteLine(storageCarInfo.jgrSN + "吊篮复位，状态为闭合状态。");
                //先移动一格等待
                //await planPathAndRun(jgrSN, outAmrPos[0]-1, outAmrPos[1]);
                //await waitArrived(jgrSN, (short)(outAmrPos[0] - 1), (short)outAmrPos[1]);
                //移动到指定位置
                await planPathAndRun(railCar.jgrSN, amrDx, amrDy);
                //发送指令让车移动到拣料工位
                //发送指令让车移动到拣料工位
                //await retryMoveCar(jgrSN, selectStationPos[0], selectStationPos[1]);
                await waitArrived(railCar.jgrSN, (short)amrDx, (short)amrDy);
                Console.WriteLine(railCar.jgrSN + "移动车辆到指定工位，工位坐标：" + amrDx.ToString() + "," + amrDy.ToString());
                //将任务完成状态放到集合中等待查询读取
                notifyTaskCompleted(new string[] { storageCarInfo.jgrSN, railCar.jgrSN });
                Console.WriteLine("完成出库任务");
            }
            catch (CancelTaskException ex)
            {
                Console.WriteLine($"异常信息：{ex.Message}");
                string[] cars;
                if (railCar == null && storageCarInfo == null)
                {
                    cars = Array.Empty<string>();
                }
                else if (railCar == null && storageCarInfo != null)
                {
                    cars = new string[1]
                    {
                        storageCarInfo.jgrSN
                    };
                }
                else if (railCar != null && storageCarInfo == null)
                {
                    cars = new string[1]
                    {
                        railCar.jgrSN
                    };
                }
                else
                {
                    cars = new string[2]
                    {
                        railCar.jgrSN,
                        storageCarInfo.jgrSN
                    };
                }
                //发送取消任务成功通知
                sendCancelTaskSuccess(cars);
            }
            
        }
        /// <summary>
        /// 移库操作
        /// </summary>
        /// <param name="storageCarInfo"></param>
        /// <param name="dto"></param>
        public async void moveStorageAsync(MoveStorageDTO dto)
        {
            JGR_Tc_Model? storageCarInfo = null;
            try
            {
                if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
                {
                    Console.WriteLine("没有任务id，不能执行当前业务");
                    return;
                }
                storageCarInfo = await waitFreeCar(JgrType.StorageCar);
                notifyCarInfo(storageCarInfo);
                int dx = int.Parse(dto.sourcePos.Split(',')[0]);
                int dy = int.Parse(dto.sourcePos.Split(',')[1]) - 1;
                int dz = int.Parse(dto.sourcePos.Split(',')[2]);
                await planPathAndRun(storageCarInfo.jgrSN, dx, dy);
                //到位判断等待
                await waitArrived(storageCarInfo.jgrSN, (short)dx, (short)dy);
                ////获取仓储车当前位置
                //int current_x = storageCarInfo.currLocation_X;
                //int current_y = storageCarInfo.currLocation_Y;
                ////移动仓储车到源仓位位置
                //int dx = int.Parse(dto.sourcePos.Split(',')[0]);
                //int dy = int.Parse(dto.sourcePos.Split(',')[1]) - 1;
                //int dz = int.Parse(dto.sourcePos.Split(',')[2]);
                ////根据仓储车朝向原则上y轴减去1为车的位置
                //if (current_x != dx || current_y != dy)
                //{
                //    //移动车到抓取位置
                //    await retryMoveCar(storageCarInfo.jgrSN, dx, dy);
                //    //到位判断等待
                //    await waitArrived(storageCarInfo.jgrSN, (short)dx, (short)dy);
                //}

                //移动到位发送抓取指令
                byte[] data = new byte[8];
                data[1] = 7;
                data[3] = 5;
                data[4] = Convert.ToByte(dz);
                await retrySendCmd(storageCarInfo.jgrSN, data);
                //读取吊篮状态
                await waitBasketPositionReset(storageCarInfo.jgrSN, 1);
                //更新目标位置
                dx = int.Parse(dto.destPos.Split(',')[0]);
                dy = int.Parse(dto.destPos.Split(',')[1]) - 1;
                dz = int.Parse(dto.destPos.Split(',')[2]);
                await planPathAndRun(storageCarInfo.jgrSN, dx, dy);
                //移动车到抓取位置
                //await retryMoveCar(storageCarInfo.jgrSN, dx, dy);
                //到位判断等待
                await waitArrived(storageCarInfo.jgrSN, (short)dx, (short)dy);
                //移动到位发送抓取指令
                data = new byte[8];
                data[1] = 7;
                data[3] = 6;
                data[4] = Convert.ToByte(dz);
                await retrySendCmd(storageCarInfo.jgrSN, data);
                //读取吊篮状态
                await waitBasketPositionReset(storageCarInfo.jgrSN, 2);
                //将任务完成状态放到集合中等待查询读取
                notifyTaskCompleted(new string[] { storageCarInfo.jgrSN });
            } catch(CancelTaskException ex)
            {
                Console.WriteLine($"异常信息：{ex.Message}");
                string[] cars = Array.Empty<string>();
                if(storageCarInfo != null)
                {
                    cars = new string[1] { storageCarInfo.jgrSN };
                }
                //发送取消任务成功通知
                sendCancelTaskSuccess(cars);
            }
        }
        /// <summary>
        /// 规划路线并下发路线
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="destX"></param>
        /// <param name="destY"></param>
        /// <returns></returns>
        public async Task planPathAndRun(string jgrSN, int destX, int destY)
        {
            if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
            {
                throw new CancelTaskException("取消任务处理");
            }
            List<MovingPath> paths = tryPlanPath(jgrSN, new()
            {
                jgrSN = jgrSN,
                startLocationX = this.jgrs[jgrSN].currLocation_X,
                startLocationY = this.jgrs[jgrSN].currLocation_Y,
                destLocationX = destX,
                destLocationY = destY
            });
            if (paths != null && paths.Count > 0)
            {
                for(int i=0; i<paths.Count; i++)
                {
                    if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                    {
                        throw new CancelTaskException("取消任务处理");
                    }
                    var path = paths[i];
                   await retryMoveCar(jgrSN, path.destLocationX, path.destLocationY);
                   if(i < paths.Count - 1)
                    {
                        //到位判断等待
                        await waitArrived(jgrSN, (short)path.destLocationX, (short)path.destLocationY);
                        Console.WriteLine(jgrSN + "已经到达位置: " + path.destLocationX + "," + path.destLocationY);
                    }
                }
                //jgrs[jgrSN].AddPath(paths);
            }
        }
        /// <summary>
        /// 规划路线
        /// </summary>
        /// <param name="jgrSN"></param>
        /// <param name="path"></param>
        /// <returns></returns>
        public List<MovingPath> tryPlanPath(string jgrSN, MovingPath path)
        {
            try
            {
                if (cancelTaskIds.Contains(ThreadLocalManager.Get().taskId))
                {
                    throw new CancelTaskException("取消任务处理");
                }
                return planPath(jgrSN, path);
            }
            catch
            {
                Task.Delay(TimeSpan.FromSeconds(2)).Wait();
                return tryPlanPath(jgrSN, path);
            }
        }

        List<MovingPath> planPath(string jgrSN, MovingPath path)
        {
            if (path.startLocationX == path.destLocationX && path.startLocationY == path.destLocationY)
            {
                //目标一样
                return new();
            }

            List<MovingPath> paths = new List<MovingPath>();
            //检查路径是否合理，如果不算直线，则需要拆解为多条路径
            if (path.startLocationX == path.destLocationX || (path.startLocationY == path.destLocationY && path.startLocationY<4))
            {
                //检查路径是否可行
                if (CheckPathAvaible(jgrSN, path))
                {
                    paths.Add(path);
                    return paths;
                }
            }

            //拆成多段路径
            //如果X轴能走到位置，则走X轴
            MovingPath path1 = new MovingPath();
            MovingPath path2 = new MovingPath();
            if (path.startLocationY > path.destLocationY)
            {
                path1.startLocationX = path.startLocationX;
                path1.startLocationY = path.startLocationY;
                path1.destLocationX = path.startLocationX;
                path1.destLocationY = path.destLocationY;

                path2.startLocationX = path1.destLocationX;
                path2.startLocationY = path1.destLocationY;
                path2.destLocationX = path.destLocationX;
                path2.destLocationY = path.destLocationY;

                if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2))
                {
                    paths.Add(path1);
                    paths.Add(path2);
                    return paths;
                }
            }
            else
            {
                path1.startLocationX = path.startLocationX;
                path1.startLocationY = path.startLocationY;
                path1.destLocationX = path.destLocationX;
                path1.destLocationY = path.startLocationY;

                path2.startLocationX = path1.destLocationX;
                path2.startLocationY = path1.destLocationY;
                path2.destLocationX = path.destLocationX;
                path2.destLocationY = path.destLocationY;
                if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2))
                {
                    paths.Add(path1);
                    paths.Add(path2);
                    return paths;
                }
            }

            //从当前位置的上边，下边位置尝试移动
            for (int i = path.startLocationY - 1; i >= 1; i--)
            {
                path1 = new MovingPath();
                path2 = new MovingPath();
                MovingPath path3 = new MovingPath();

                path1.startLocationX = path.startLocationX;
                path1.startLocationY = path.startLocationY;
                path1.destLocationX = path.startLocationX;
                path1.destLocationY = i;

                path2.startLocationX = path1.destLocationX;
                path2.startLocationY = path1.destLocationY;
                path2.destLocationX = path.destLocationX;
                path2.destLocationY = path1.destLocationY;

                path3.startLocationX = path2.destLocationX;
                path3.startLocationY = path2.destLocationY;
                path3.destLocationX = path.destLocationX;
                path3.destLocationY = path.destLocationY;

                if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2) && CheckPathAvaible(jgrSN, path3))
                {
                    paths.Add(path1);
                    paths.Add(path2);
                    paths.Add(path3);
                    return paths;
                }
            }

            for (int i = path.startLocationY + 1; i <= 8; i++)
            {
                path1 = new MovingPath();
                path2 = new MovingPath();
                MovingPath path3 = new MovingPath();

                path1.startLocationX = path.startLocationX;
                path1.startLocationY = path.startLocationY;
                path1.destLocationX = path.startLocationX;
                path1.destLocationY = i;

                path2.startLocationX = path1.destLocationX;
                path2.startLocationY = path1.destLocationY;
                path2.destLocationX = path.destLocationX;
                path2.destLocationY = path1.destLocationY;

                path3.startLocationX = path2.destLocationX;
                path3.startLocationY = path2.destLocationY;
                path3.destLocationX = path.destLocationX;
                path3.destLocationY = path.destLocationY;

                if (CheckPathAvaible(jgrSN, path1) && CheckPathAvaible(jgrSN, path2) && CheckPathAvaible(jgrSN, path3))
                {
                    paths.Add(path1);
                    paths.Add(path2);
                    paths.Add(path3);
                    return paths;
                }
            }
            throw new Exception("路线拥挤，请等待");
        }
        /// <summary>
        /// 查找空车
        /// </summary>
        /// <param name="jgrType"></param>
        /// <returns></returns>
        public JGR_Tc_Model? findFreeCar(JgrType jgrType)
        {
            if (ThreadLocalManager.Get() == null ||
                string.IsNullOrEmpty(ThreadLocalManager.Get().taskId)
                || string.IsNullOrWhiteSpace(ThreadLocalManager.Get().taskId))
            {
                Console.WriteLine("没有任务id，不能执行当前业务");
                return null;
            }
            JGR_Tc_Model? free_car = null;
            //查找空车
            List<JGR_Tc_Model> carStatus = GetJgrs();
            if (carStatus != null && carStatus.Count > 0)
            {
                foreach (var car in carStatus)
                {
                    if (car.jgrType == (byte)jgrType && checkIsFree(car, true))
                    {
                        free_car = car;
                        if (!jgrTaskMap.ContainsKey(car.jgrSN))
                        {
                            jgrTaskMap[car.jgrSN] = ThreadLocalManager.Get().taskId;
                        }
                        break;
                    }
                }
            }
            return free_car;
        }
        /// <summary>
        /// 车辆是否空闲
        /// </summary>
        /// <param name="car"></param>
        /// <returns></returns>
        private bool checkIsFree(JGR_Tc_Model car, bool isCheckRfId)
        {
            if(car.jgrType == (byte)JgrType.RailCar)
            {
                return car.moveStatus == 0
                             && (!isCheckRfId || (string.IsNullOrEmpty(car.rfid)
                             || string.IsNullOrWhiteSpace(car.rfid)))
                             && (!jgrTaskMap.ContainsKey(car.jgrSN)
                             || jgrTaskMap[car.jgrSN] == ThreadLocalManager.Get().taskId);
            } else if(car.jgrType == (byte)JgrType.StorageCar)
            {
                return car.moveStatus == 0 && car.basketPosition == 0 && car.basketGripperStatus == 2
                             && (!jgrTaskMap.ContainsKey(car.jgrSN)
                             || jgrTaskMap[car.jgrSN] == ThreadLocalManager.Get().taskId);
            }
            return false;
        }

        /// <summary>
        /// 获取仓储结构
        /// </summary>
        /// <returns></returns>
        public Dictionary<long, GridCell> getStorageGridCells()
        {
            return storageGridCells;
        }

        public bool checkStoragePos(int x, int y)
        {
            var storageGridCells = getStorageGridCells();
            if (storageGridCells != null && storageGridCells.Count > 0)
            {
                //判断储位是否为不能出入库的储位
                foreach (var cell in storageGridCells)
                {
                    if (cell.Value.positionX == x && cell.Value.positionY == y)
                    {
                        if (!cell.Value.enableUse)
                        {
                            return false;
                        }
                        return true;
                    }
                }
            }
            return false;
        }

        public void notifyCancelTask(string taskId)
        {
            try
            {
                mutex.WaitOne();
                cancelTaskIds.Add(taskId);
            }
            finally
            {
                mutex.ReleaseMutex();
            }
        }
        public void removeCancelTask(string taskId)
        {
            try
            {
                mutex.WaitOne();
                cancelTaskIds.Remove(taskId);
            }
            finally
            {
                mutex.ReleaseMutex();
            }
        }
    }

    public interface IJGR
    {
        bool Init(IServiceProvider services);

        bool ResetADS();

        void SetHub(IHubContext<ChatHub> hub);

        /// <summary>
        /// 获得车的清单，目前充电桩和车是一个结构
        /// </summary>
        /// <returns></returns>
        List<JGR_Tc_Model> GetJgrs();

    }


}