using ConfigHelper;
using CtuDeviceLib;
using DeviceLibrary;
using DeviceLibrary.CtuService;
using log4net;
using log4net.Core;
using log4net.Util;
using Mushiny.Model;
using OnlineStore.Common;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using static System.Windows.Forms.VisualStyles.VisualStyleElement;

namespace Mushiny
{
    /// <summary>
    /// 牧星CTU料箱机器人
    /// </summary>
    public class CTU
    {

        public static readonly ILog LOGGER = Common.LOGGER;
        public List<string> curLimitAreas = new List<string>();
        public CTU(string name, string ip, int basketCnt = 4)
        {
            Name = name;
            IP = ip;
            PathPoints = new List<PathPoint>();
            waitSendPkgs = new ConcurrentQueue<PkgData>();
            FaultInfo = new CTUFaultInfo();
            loadBaskets(basketCnt);
        }
        CtuTaskBase ctuTask;
        public CtuTaskBase CtuTask
        {
            get { return ctuTask; }
            set
            {
                if (ctuTask != null)//有任务
                {
                    //打印上一次运行记录
                    LogUtil.info($"【{Name}】任务记录 【{ctuTask.Name}】 {TaskLog.ToLog()}");
                    //上报给服务器
                    Task.Factory.StartNew(new Action(() =>
                    {
                        SMFManager.UploadTaskInfo(TaskLog.clone());
                    }));

                    TaskLog = new CtuTaskLog();
                    TaskLog.StartPoint = ctuTask.DstName;
                    TaskLog.BoxCntInBasket = GetFullBaskets()?.Count ?? 0;
                    ctuTask.Dispose();
                    ctuTask = null;
                }
                else //无任务
                {
                    TaskLog = new CtuTaskLog();
                    TaskLog.StartPoint = CurLandMark.ToString();
                    TaskLog.BoxCntInBasket = GetFullBaskets()?.Count ?? 0;
                }

                ctuTask = value;
                if (value != null)
                {
                    TaskLog.BoxCode = value.boxCode;
                    TaskLog.EndPoint = value.DstName;
                    LogUtil.info($"【{Name}】设置任务 当前位置【{CurLandMark}】【{SOC}%】【{value?.Name}】【{value?.boxCode}】【{value?.DstName}】");
                }
            }
        }

        public CtuTaskLog TaskLog = new CtuTaskLog();
        /// <summary>
        /// 是否是调试
        /// </summary>
        public bool IsDebug { get; set; } = false;



        internal void Process()
        {
            try
            {
                if (ctuTask != null)
                {
                    if (ctuTask.runInfo.NeedResetFault)
                    {
                        ClearUnPuttedBox();
                        if (!ctuTask.MoveInfo.IsStep(RunStep.Wait))
                        {
                            ctuTask.ProcessReset();
                        }
                        ctuTask.runInfo.ClearFault();
                        ctuTask.MoveInfo.Info("异常恢复，流程重置");
                    }
                    if (Enabled)
                    {
                        if (ctuTask.MoveInfo.IsStep(RunStep.CtuTask_ProcessFinished))
                        {
                            if (RobotManager.IsReadyUpdate)
                            {
                                ctuTask.MoveInfo.Info("动作已完成");
                                return;
                            }
                            if (NeedToCharge(ctuTask))
                            {
                                //是否已有该类型车去充电，有则不准备去充电
                                var ctuInPreCharge = CTUManager.GetEnabledCTUs()?.Find(s => s.CtuTask != null && chargePreProcess(s.CtuTask));//这种情况针对一个充电桩
                                if (ctuInPreCharge != null)
                                {

                                }
                                else
                                {
                                    CtuTask = new AutoChargeTask(this);
                                }

                            }
                        }

                    }


                    if (CheckWait(ctuTask.MoveInfo))
                    {
                        return;
                    }
                    ctuTask.Excute();
                }
                else
                {
                    //TODO: 判断是出库CTU还是入库CTU，设置寻找出库或者入库任务
                    CtuTask = CtuTaskBase.SearchTask(this);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error($"[{Name}] Process 异常", ex);
            }

        }
        public bool CheckWait(LineMoveInfo MoveInfo)
        {
            if (MoveInfo == null) return false;
            //当等待超过一分钟时，需要打印提示 
            TimeSpan span = DateTime.Now - MoveInfo.LastSetpTime;
            string NotOkMsg = "";
            if (MoveInfo.WaitList.Count <= 0)
            {
                MoveInfo.EndStepWait();
                return false;
            }
            bool isOk = true;
            if (MoveInfo.OneWaitCanEndStep)
            {
                isOk = false;
            }

            //MoveInfo.WaitList.ForEach((w) => {
            //    if (w.WaitType.Equals(WaitEnum.W014_Msg))
            //    {
            //        w.IsEnd = true;
            //        ShowMsg(w.ActionMsg, w.Data, $"{genMsgCode(1)}");
            //    }
            //});
            foreach (WaitResultInfo wait in MoveInfo.WaitList)
            {
                NotOkMsg = wait.ToStr();
                if (wait.WaitType.Equals(WaitEnum.W003_Time))
                {
                    wait.IsEnd = (span.TotalMilliseconds >= wait.TimeMSeconds);
                }

                if (wait.IsEnd)
                {
                    if (MoveInfo.OneWaitCanEndStep)
                    {
                        isOk = true;
                        break;
                    }
                }
                else
                {
                    if (!MoveInfo.OneWaitCanEndStep)
                    {
                        isOk = false;
                        break;
                    }
                }
            }
            if (isOk)
            {
                MoveInfo.EndStepWait();
                return false;
            }
            return true;
        }
        bool NeedToManual()
        {
            //if (!CTUManager.InletGroup.IsInGroup(Name))
            //{
            //    var instore = TaskUtil.Manual_GetInStore();
            //    if (instore == null) return false;
            //    if (CTUManager.ManualGroup.CtuNames != null && CTUManager.ManualGroup.CtuNames.Count > 0)
            //    {
            //        if (!CtuGroup.IsInGroups(Name))//有一辆
            //        {
            //            if (!CTUManager.ManualGroup.IsFull)//但是手动线数量车不满，再添加
            //            {
            //                CTUManager.ManualGroup.AddCtu(Name);
            //            }
            //        }
            //        if (CTUManager.ManualGroup.CtuNames.Contains(Name))
            //        {
            //            return true;
            //        }
            //    }
            //    else //没有设置ctu
            //    {
            //        if (!CtuGroup.IsInGroups(Name))//添加一台
            //        {
            //            CTUManager.ManualGroup.AddCtu(Name);
            //            return true;
            //        }
            //    }
            //}
            return false;
        }
        /// <summary>
        /// 充电前准备
        /// </summary>
        /// <param name="task"></param>
        /// <returns></returns>
        bool chargePreProcess(CtuTaskBase task)
        {
            if (task is ChargeTask || task is AutoChargeTask)
            {
                return true;
            }
            return false;
        }
        /// <summary>
        /// 判断CTU是否需要充电
        /// </summary>
        /// <param name="ctu"></param>
        /// <param name="task"></param>
        bool NeedToCharge(CtuTaskBase task)
        {
            if (task is ChargeTask || task is AutoChargeTask)
            {
                return false;
            }
            //是否要进行强制充电
            var forceChargeSoc = ConfigAppSettings.GetValue(Setting_Init.Charge_ForceChargeSoc, 20);
            if (SOC <= forceChargeSoc)
            {
                //检查当前ctu是否是电量最低的，是则准备去充电
                var minSOC = CTUManager.GetEnabledCTUs()?.Find(s => s.SOC <= SOC);
                if (minSOC != null && minSOC.IP.Equals(IP))
                {
                    return true;
                }
            }
            else
            {
                int standMinute = ConfigAppSettings.GetIntValue("在待机点停留一定时间充电", 5, "单位分钟");
                if (task is StandbyTask && task.MoveInfo.IsTimeOut(60 * standMinute))
                {
                    //检查当前ctu是否是电量最低的，是则准备去充电
                    var minSOC = CTUManager.GetEnabledCTUs()?.Find(s => s.SOC <= SOC && s.CtuTask != null && s.CtuTask is StandbyTask);
                    if (minSOC != null && minSOC.IP.Equals(IP))
                    {
                        return true;
                    }
                }
            }
            return false;
        }

        public string PrefixName { get { return $"【{Name}】【{IP}】"; } }
        /// <summary>
        /// 背篓
        /// </summary>
        List<Basket> Baskets { get; set; }

        public List<Basket> GetEnabledBaskets()
        {
            return Baskets.FindAll(s => !s.Shield);
        }
        /// <summary>
        /// 所有启用且有料箱的背篓
        /// </summary>
        /// <returns></returns>
        public List<Basket> GetFullBaskets()
        {
            return GetEnabledBaskets()?.FindAll(b => !b.Shield && !string.IsNullOrEmpty(b.BoxCode) && b.IsPutted);
        }
        void loadBaskets(int basketCnt)
        {
            string buff = ConfigAppSettings.GetValue($"Buff_{Name}_Baskets", "");
            if (string.IsNullOrEmpty(buff))
            {
                Baskets = new List<Basket>();
                for (int i = 0; i < basketCnt; i++)
                {
                    Baskets.Add(new Basket()
                    {
                        Row = (byte)(i + 1),
                    });

                }
            }
            else
            {
                Baskets = JsonHelper.DeserializeJsonToList<Basket>(buff);
                if (Baskets != null && Baskets.Count == basketCnt)
                {
                    LogInfo($"加载背篓缓存完成：【{buff}】");
                }
                else
                {
                    Baskets = new List<Basket>();
                    for (int i = 0; i < basketCnt; i++)
                    {
                        Baskets.Add(new Basket()
                        {
                            Row = (byte)(i + 1),
                        });

                    }
                }
            }
        }

        public void ClearUnPuttedBox()
        {
            Baskets.ForEach(s =>
            {
                if (!string.IsNullOrEmpty(s.BoxCode))
                {
                    if (!s.IsPutted)
                    {
                        LOGGER.Info($"{PrefixName} 清空未放料箱的背篓【{s.Row}】【{s.BoxCode}】");
                        ClearBasket(s.Row);
                    }
                }
            });
        }
        void saveBaskets()
        {
            ConfigAppSettings.SaveValue($"Buff_{Name}_Baskets", JsonHelper.SerializeObject(Baskets));
        }
        public bool Enabled
        {
            get
            {
                return ConfigAppSettings.GetValue(Common.Mushiny_ + "EnabledCtus", new string[] { "" }).Contains(Name);
            }
        }
        public int ID { get; set; }
        public string Name { get; set; }
        public string IP { get; set; }
        public uint AGVID { get; set; }
        private float curDir;
        /// <summary>
        /// 当前角度
        /// </summary>
        public float CurDir
        {
            get { return curDir; }
            set
            {
                if (value != curDir)
                {
                    LOGGER.Info($"{PrefixName}角度变化【{curDir}】->【{value}】");
                }

                curDir = value;
            }
        }
        /// <summary>
        /// 当前执行的路径包
        /// </summary>
        PathDataPkg CurPathDataPkg { get; set; }
        SendPathPkgState _sendCurPathPkgOk;
        /// <summary>
        /// 发送当前路径包的状态
        /// </summary>
        public SendPathPkgState SendCurPathPkgOk
        {
            get { return _sendCurPathPkgOk; }
            set
            {
                if (value != _sendCurPathPkgOk)
                {
                    LOGGER.Info($"{PrefixName}路径包【{CurPathDataPkg?.TimeStamp}】状态变化 【{_sendCurPathPkgOk}】->【{value}】");
                }
                _sendCurPathPkgOk = value;
            }
        }
        /// <summary>
        /// 最后发送路径包的时间
        /// </summary>
        DateTime lastSendPathPkgTime = DateTime.Now;
        /// <summary>
        /// 被占用的地标
        /// </summary>
        public List<uint> UsedPoint = new List<uint>();

        /// <summary>
        /// 移动中的点
        /// </summary>
        public List<uint> MovePoints = new List<uint>();
        /// <summary>
        /// 是否有空背篓
        /// </summary>
        /// <param name="row"></param>
        /// <returns></returns>
        public bool HasEmptyBasket(out byte row)
        {
            row = 0;
            var basket = Baskets.Find(s => !s.Shield && string.IsNullOrEmpty(s.BoxCode));
            if (basket != null)
            {
                row = basket.Row;
                return true;
            }
            else
                return false;

        }
        public bool HasEmptyBasket()
        {
            var basket = Baskets.Find(s => !s.Shield && string.IsNullOrEmpty(s.BoxCode));
            if (basket != null)
            {

                return true;
            }
            else
                return false;

        }
        /// <summary>
        /// 所有背篓是否为空
        /// </summary>
        /// <returns></returns>
        public bool AllBasketIsEmpty()
        {
            var basket = Baskets.Find(s => !s.Shield && !string.IsNullOrEmpty(s.BoxCode));
            if (basket != null)
            {
                return false;
            }
            return true;
        }
        /// <summary>
        /// 获取第一个满背篓
        /// </summary>
        /// <returns></returns>
        public Basket GetFirstFullBasket()
        {
            var basket = Baskets.Find(s => !s.Shield && !string.IsNullOrEmpty(s.BoxCode) && s.IsPutted);
            return basket;
        }
        public List<Basket> GetAllBaskets()
        {
            return Baskets;
        }
        public Basket GetBasket(byte row)
        {
            var basket = Baskets.Find(s => s.Row == row);
            return basket;
        }
        public bool HasEmptyBasket(byte minRow, out byte row)
        {
            row = 0;
            var basket = Baskets.Find(s => !s.Shield && string.IsNullOrEmpty(s.BoxCode) && s.Row >= minRow);
            if (basket != null)
            {
                row = basket.Row;
                return true;
            }
            else
                return false;

        }
        public bool HasEmptyBasket(out Basket row)
        {
            row = null;
            var basket = Baskets.Find(s => !s.Shield && string.IsNullOrEmpty(s.BoxCode));
            if (basket != null)
            {
                row = basket;
                return true;
            }
            else
                return false;
        }
        /// <summary>
        /// 设置背篓信息
        /// </summary>
        /// <param name="row"></param>
        /// <param name="boxCode"></param>
        /// <returns></returns>
        public bool SetBasketWithBoxCode(byte row, string boxCode, string dstName)
        {
            var find = Baskets.Find(s => s.Row == row);
            if (find != null)
            {
                find.BoxCode = boxCode;
                find.UpdateTime = DateTime.Now;
                find.DstName = dstName;
                find.IsPutted = false;
                find.LocateCode = Basket.BoxCodeToLocateCode(boxCode);
                saveBaskets();
                LOGGER.Info($"{PrefixName} 背篓信息【{row}】设置【{find.BoxCode}】【{find.LocateCode}】【{dstName}】根据料箱码");
                return true;
            }
            return false;
        }
        public bool SetBasketWithLocateCode(byte row, string locateCode, string dstName, bool isInBasket = false)
        {
            var find = Baskets.Find(s => s.Row == row);
            if (find != null)
            {
                find.BoxCode = Basket.LocateCodeToBoxCode(locateCode);
                find.UpdateTime = DateTime.Now;
                find.LocateCode = locateCode;
                find.IsPutted = isInBasket;
                if (!string.IsNullOrEmpty(dstName))
                    find.DstName = dstName;
                saveBaskets();
                LOGGER.Info($"{PrefixName} 背篓信息【{row}】设置【{find.BoxCode}】【{find.LocateCode}】【{dstName}】根据定位码");
                return true;
            }
            return false;
        }
        public void ClearBasket(byte row)
        {
            var find = Baskets.Find(s => s.Row == row);
            if (find != null)
            {
                find.DstName = "";
                find.BoxCode = "";
                find.LocateCode = "";
                find.UpdateTime = DateTime.Now;
                find.IsPutted = false;
                saveBaskets();
                LOGGER.Info($"{PrefixName} 背篓信息【{row}】清空背篓 根据层号");
            }
        }
        public void ClearBasketByBoxCode(string boxCode)
        {
            var find = Baskets.Find(s => s.BoxCode == boxCode);
            if (find != null)
            {
                find.DstName = "";
                find.BoxCode = "";
                find.LocateCode = "";
                find.UpdateTime = DateTime.Now;
                find.IsPutted = false;
                saveBaskets();
                LOGGER.Info($"{PrefixName} 背篓信息 【{boxCode}】 清空背篓【{find.Row}】信息 根据料箱号");
            }
        }
        public void ClearBasketByLocateCode(string locateCode)
        {
            var find = Baskets.Find(s => s.LocateCode == locateCode);
            if (find != null)
            {
                find.DstName = "";
                find.BoxCode = "";
                find.LocateCode = "";
                find.IsPutted = false;
                find.UpdateTime = DateTime.Now;
                saveBaskets();
                LOGGER.Info($"{PrefixName} 背篓信息 【{locateCode}】 清空背篓【{find.Row}】信息 根据定位码");
            }
        }
        /// <summary>
        /// 路径运行结束
        /// </summary>
        public bool PathPointFinish
        {
            get
            {
                if (CurPathDataPkg == null || CurPathDataPkg.TimeStamp == 0)
                    return true;
                else
                {
                    if (CurPathDataPkg.PathPoints == null)
                        return true;
                    if (CurPathDataPkg.PathPoints.Count == 0) return true;
                    var lastPathPoint = CurPathDataPkg.PathPoints[CurPathDataPkg.PathPoints.Count - 1];
                    var rtn = lastPathPoint.PointState == PathPointState.Finish;
                    return rtn;
                }
            }
        }
        /// <summary>
        /// 故障信息
        /// </summary>
        public CTUFaultInfo FaultInfo { get; set; }
        /// <summary>
        /// 当前地标
        /// </summary>
        public uint CurLandMark { get; private set; }
        /// <summary>
        /// 当前状态
        /// </summary>
        public RobotState RobotState { get; set; }
        /// <summary>
        /// 剩余电量
        /// </summary>
        public byte SOC { get; set; }
        /// <summary>
        /// 电池寿命
        /// </summary>
        public byte SOH { get; set; }
        /// <summary>
        /// 上一个地标
        /// </summary>
        public uint PreLandMark { get; private set; }
        /// <summary>
        /// 当前任务完成时发送的状态
        /// </summary>
        public string CurTaskFinishState { get; set; }
        /// <summary>
        /// 目标地码
        /// </summary>
        public uint DestPointCode { get; private set; }
        /// <summary>
        /// 组别
        /// </summary>
        public string Group { get; set; }
        private RunState runState;
        public RunState RunState
        {
            get { return runState; }
            set
            {
                if (value != runState)
                {
                    LOGGER.Info($"{PrefixName}运行状态变化 【{runState}】->【{value}】");
                }
                runState = value;
            }
        }
        /// <summary>
        /// 路径任务
        /// </summary>
        public List<PathPoint> PathPoints { get; set; }
        public delegate void PathActionFinishEventHandler(ActionFInishArg actionFInish);
        public delegate void FaultChangeEventHandler(FaultEventArg faultEventArg);

        public delegate void PointCodeChangedEventHandler(CTU ctu, uint addrCode);
        public event PointCodeChangedEventHandler PointCodeChanged;
        public event PathActionFinishEventHandler PathActionFinish;
        public event FaultChangeEventHandler FaultChange;

        private ConcurrentQueue<PkgData> waitSendPkgs;
        /// <summary>
        /// 等待同步料仓信息
        /// 1标识要等待
        /// </summary>
        byte waitSyncBunkerInfo;
        /// <summary>
        /// 设置当前地标
        /// </summary>
        /// <param name="landmark"></param>
        void setLandmark(uint landmark)
        {
            if (CurLandMark == landmark) return;
            PreLandMark = CurLandMark;
            CurLandMark = landmark;
            PointCodeChanged?.Invoke(this, CurLandMark);
            LOGGER.Info($"{PrefixName}地标变化：当前={CurLandMark}，上一个={PreLandMark}");
            if (UsedPoint != null)
            {
                if (UsedPoint.Remove(PreLandMark))
                    LOGGER.Info($"{PrefixName}移除占用的地标：{PreLandMark}");
                AddUsedPoint(CurLandMark);
            }
            if (MovePoints != null)
            {
                MovePoints.Remove(PreLandMark);
                AddMovePoint(CurLandMark);
            }
            pointCodeLastUpdateTime = DateTime.Now;
        }
        /// <summary>
        /// 地码更新的最后时间
        /// </summary>
        public DateTime pointCodeLastUpdateTime = DateTime.Now;

        /// <summary>
        /// 清除路径点占用
        /// </summary>
        /// <param name="exceps">排除清除的点</param>
        public void ClearUsedPoints(List<uint> exceps)
        {
            if (exceps == null || exceps.Count == 0)
            {
                UsedPoint.Clear();
                UsedPoint.Add(CurLandMark);
            }
            else
            {
                var needRemain = new List<uint>();
                foreach (var item in exceps)
                {
                    if (UsedPoint.Contains(item))
                    {
                        if (!needRemain.Contains(item))
                            needRemain.Add(item);
                    }
                }
                UsedPoint.Clear();
                AddUsedPoint(CurLandMark);
                AddUsedPoints(needRemain);
            }
        }
        /// <summary>
        /// 清除路径点的已占用节点
        /// </summary>
        public void ClearUsedPoints(bool clearCur = false)
        {
            UsedPoint.Clear();
            if (!clearCur)
                UsedPoint.Add(CurLandMark);
        }

        public void ClearMovePoints(bool clearCur = false)
        {
            MovePoints.Clear();
            if (!clearCur)
                MovePoints.Add(CurLandMark);
        }
        public void AddUsedPoint(uint point)
        {
            if (!UsedPoint.Contains(point))
            {
                UsedPoint.Add(point);
            }
        }
        public void AddMovePoint(uint point)
        {
            if (!MovePoints.Contains(point))
            {
                MovePoints.Add(point);
            }
        }
        public void AddUsedPoints(List<uint> points)
        {
            if (points != null && points.Count > 0)
            {
                foreach (var item in points)
                {
                    AddUsedPoint(item);
                }
            }
        }
        public void OnPathActionFinish()
        {
            PathActionFinish?.Invoke(null);
        }

        string bytesTostr(byte[] bytes)
        {
            string rtnStr = "";
            if (bytes != null)
            {
                for (int i = 0; i < bytes.Length; i++)
                {
                    rtnStr += " ";
                    rtnStr += bytes[i].ToString("X2");
                }
            }
            return rtnStr.Trim();
        }
        DateTime lastrecvTime = DateTime.Now;
        DateTime lastLoginTime = DateTime.Now;
        public bool isLogin = false;
        bool isReset = false;
        public void HandleResponse(TcpClientBean client, byte[] Msg)
        {
            try
            {
                if (PkgData.ParseCmd(Msg, out byte cmdWord))
                {
                    if (!isLogin)
                    {
                        if (AGVID != 0)
                        {
                            if ((DateTime.Now - lastLoginTime).TotalSeconds > 3)
                            {
                                var relogin = new ReLoginPkg()
                                {
                                    AgvID = AGVID,
                                    TimeStamp = PkgData.genTimeStamp(),
                                };
                                lastLoginTime = DateTime.Now;
                                waitSendPkgs.Enqueue(relogin);
                                isReset = false;
                                LOGGER.Info($"{PrefixName} 发送重新登录命令包【{relogin.TimeStamp}】【{bytesTostr(relogin.ToBytes())}】");
                            }
                        }

                    }
                    if (PkgData.IsHeartBeat(cmdWord))
                    {
                        var heartBeatPkg = new HeartBeatPkg();
                        if (heartBeatPkg.Parse(Msg))
                        {
                            setLandmark(heartBeatPkg.Location.AddrCode);
                            var heartbeatRtn = new HeartBeatRtnPkg(heartBeatPkg.TimeStamp);
                            heartbeatRtn.AgvID = heartBeatPkg.AgvID;
                            waitSendPkgs.Enqueue(heartbeatRtn);
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析心跳包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    if (PkgData.IsRealTimePkg(cmdWord))
                    {
                        LOGGER.Debug($"{PrefixName} 收到实时包【{bytesTostr(Msg)}】");
                        var rtn = new RealtimePkg();
                        if (rtn.Parse(Msg))
                        {
                            AGVID = rtn.AgvID;
                            // CurDir = rtn.CurAngle;
                            if (rtn.Location != null)
                            {
                                setLandmark(rtn.Location.AddrCode);
                            }
                            var heartbeatRtn = new HeartBeatRtnPkg(PkgData.genTimeStamp());
                            heartbeatRtn.AgvID = rtn.AgvID;
                            waitSendPkgs.Enqueue(heartbeatRtn);
                            LOGGER.Debug($"{Name} 回复实时包【{rtn.TimeStamp}】【{bytesTostr(heartbeatRtn.ToBytes())}】");
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析实时数据包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsLoginPkg(cmdWord))//登陆包
                    {
                        lastrecvTime = DateTime.Now;
                        var login = new LoginDataPkg();
                        if (login.Parse(Msg))
                        {
                            DebugInfo($"收到登录包 {bytesTostr(Msg)}");
                            AGVID = login.AgvID;
                            setLandmark(login.Location.AddrCode);
                            if (Enabled)
                                if (!UsedPoint.Contains(CurLandMark))
                                {
                                    UsedPoint.Add(CurLandMark);
                                }
                            SOH = login.Battery.SOH;
                            SOC = login.Battery.SOC;
                            FaultInfo = new CTUFaultInfo();
                            FaultInfo.FaultFlag = (FaultFlag)login.FaultFlag;
                            FaultInfo.BatteryFaultFlag = login.Battery.FaultFlag;
                            RobotState = (RobotState)login.CurState;
                            waitSyncBunkerInfo = login.WaitStoreSyncFlag;
                            LOGGER.Info($"{PrefixName} 收到登录包：AGVID【{login.AgvID}】地址码【{CurLandMark}】当前状态【{RobotState}】等待同步料仓标识（1要等待）【{waitSyncBunkerInfo}】 电量【{SOC}】电池寿命【{SOH}】故障标志位【{FaultInfo.FaultFlag}】电池故障标志位【{FaultInfo.BatteryFaultFlag}】");
                            var rtn = new LoginDataRtnPkg();
                            rtn.AgvID = AGVID;
                            rtn.TimeStamp = PkgData.genTimeStamp();
                            rtn.BackSysFlag = new byte[] { 0x10, 0x00 };
                            rtn.BackSysVer = new byte[] { 0x10, 0x00 };
                            waitSendPkgs.Enqueue(rtn);
                            RunState = RunState.Online;
                            isLogin = true;
                            lastLoginTime = DateTime.Now;
                            LOGGER.Info($"{PrefixName} 回复登录包【{rtn.TimeStamp}】【{bytesTostr(rtn.ToBytes())}】");
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析登录包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsActionResonse(cmdWord))
                    {
                        var rtn = new ActionDataRtnPkg();
                        if (rtn.Parse(Msg))
                        {
                            if ((ActionCode)rtn.ActionCode == ActionCode.归位扫描)
                            {
                                isReset = true;
                            }
                            else if ((ActionCode)rtn.ActionCode == ActionCode.停到最近二维码)
                            {
                                StopAtNearPointCode = true;
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】停到最近二维码完成");
                            }
                            LOGGER.Info($"{PrefixName} 收到命令码【{(ActionCode)rtn.ActionCode}】【{bytesTostr(Msg)}】");
                            LOGGER.Info($"{PrefixName} 回复命令码【{(ActionCode)rtn.ActionCode}】,回复时间戳【{rtn.TimeStamp}】");
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析命令回复数据包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsStorePkg(cmdWord))//AGV归位完成才发送
                    {
                        var store = new StoreInfoPkg();
                        if (store.Parse(Msg))
                        {
                            StringBuilder sb = new StringBuilder();
                            store?.ContainerCodes.ForEach(c => sb.Append($"{c}#"));
                            LOGGER.Info($"{PrefixName} 收到料仓信息包:【{sb.ToString()}】");//【{bytesTostr(Msg)}】

                            var rtn = new ContainerRtnPkg
                            {
                                AgvID = AGVID,
                                TimeStamp = PkgData.genTimeStamp(),
                                FrameLenght = 13
                            };
                            UpdateStoreInfo(store?.ContainerCodes);
                            waitSendPkgs.Enqueue(rtn);
                            RunState = RunState.Executing;
                            LOGGER.Info($"{PrefixName} 回复料仓信息包时间戳【{rtn.TimeStamp}】");
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析料仓信息包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsStatusPkg(cmdWord))//每间隔10秒发送
                    {
                        var recv = new StatusDataPkg();
                        if (recv.Parse(Msg))
                        {
                            //if (isReset)
                            //{
                            //    RunState = RunState.Executing;
                            //}
                            AGVID = recv.AgvID;
                            SOC = recv.Battery.SOC;
                            SOH = recv.Battery.SOH;
                            FaultInfo.BatteryFaultFlag = recv.Battery.FaultFlag;
                            FaultInfo.FaultFlag = (FaultFlag)Common.ConvertToUInt(recv.FaultFlag);
                            LogStatusInfo($"收到状态数据包,AGVID【{AGVID}】,SOC电量【{SOC}】,SOH电池寿命【{SOH}】，故障标志位【{FaultInfo.FaultFlag}】");//【{bytesTostr(Msg)}】
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析状态数据包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsFaultPkg(cmdWord))//故障包，发生故障才发 
                    {
                        var recv = new FaultDataPkg();
                        if (recv.Parse(Msg))
                        {
                            FaultInfo.FaultFlag = (FaultFlag)Common.ConvertToUInt(recv.FaultFlag);
                            RobotState = (RobotState)recv.CurState;
                            FaultInfo.FaultID = recv.FaultID;
                            FaultInfo.HasFault = recv.HasFault();
                            FaultInfo.FaultData1 = recv.FaultData1;
                            FaultInfo.FaultData2 = recv.FaultData2;
                            //这几个故障无需重新登录,自恢复即可
                            //0x3001
                            // 0x3002
                            //0x3003
                            //0x3004
                            //0x3005
                            if (FaultInfo.FaultID[0] == 0x30)
                            {
                                if (FaultInfo.FaultID[1] >= 0x01 && FaultInfo.FaultID[1] <= 0x05)
                                {
                                    //无需重登录
                                }
                                else
                                {
                                    isLogin = false;
                                }
                            }

                            if (FaultInfo.FaultID[0] == 0x21)
                            {
                                if (FaultInfo.FaultID[1] == 0x03 || FaultInfo.FaultID[1] == 0x04)
                                {
                                    //无需重登录
                                }
                                else
                                {
                                    isLogin = false;
                                }
                            }

                            if (FaultInfo.FaultID[0] == 0x40)
                            {
                                if (FaultInfo.FaultID[1] == 0x01)
                                {
                                    //无需重登录
                                }
                                else
                                {
                                    isLogin = false;
                                }
                            }

                            if (FaultInfo.FaultID[0] == 0x40)
                            {
                                if (FaultInfo.FaultID[1] == 0x11)
                                {
                                    //无需重登录
                                }
                                else
                                {
                                    isLogin = false;
                                }
                            }


                            if (FaultInfo.FaultID[0] == 0x70)
                            {
                                if (FaultInfo.FaultID[1] == 0x07) { }
                                //无需重登录
                                else
                                {
                                    isLogin = false;
                                }
                            }

                            if (FaultInfo.FaultID[0] == 0x90)
                            {
                                if (FaultInfo.FaultID[1] == 0x02) { }
                                //无需重登录
                                else
                                {
                                    isLogin = false;
                                }
                            }
                            FaultInfo.NeedResetFault = !isLogin;
                            if (!FaultInfo.HasFault)//故障已被清除，清空异常数据
                            {
                                FaultInfo = new CTUFaultInfo();
                            }
                            if (!isLogin)
                            {
                                //清除占用
                                ClearUsedPoints();
                                RunState = RunState.Offline;
                            }
                            LogFaultInfo($"收到故障数据包:当前状态【{RobotState}】， 是否有故障【{FaultInfo.HasFault}】,故障id【{bytesTostr(FaultInfo.FaultID)}】,故障数据1【{FaultInfo.FaultData1}】，故障数据2【{FaultInfo.FaultData2}】【{bytesTostr(Msg)}】");
                            FaultChange?.Invoke(new FaultEventArg()
                            {
                                CTU = this,
                                FinishState = CurTaskFinishState,
                                HasFault = FaultInfo.HasFault,
                                NeedResetFault = FaultInfo.NeedResetFault,
                                FaultID = FaultInfo.FaultID,
                                FaultStr = $"故障id【{bytesTostr(FaultInfo.FaultID)}】,故障数据1【{FaultInfo.FaultData1}】，故障数据2【{FaultInfo.FaultData2}】"
                            });
                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析故障数据包失败【{bytesTostr(Msg)}】");
                        }
                    }
                    else if (PkgData.IsPathRtnPkg(cmdWord, out bool result))//路径应答包
                    {
                        var recv = new PathDataRtnPkg();
                        if (recv.Parse(Msg))
                        {
                            if (recv.IsOK())
                            {
                                LOGGER.Info($"{PrefixName} 路径包【{recv.PathTimeStamp}】的应答包正常【{bytesTostr(Msg)}】");
                                if (CurPathDataPkg.TimeStamp == recv.PathTimeStamp)
                                {
                                    SendCurPathPkgOk = SendPathPkgState.Success;
                                    LOGGER.Info($"{PrefixName} 当前路径包【{recv.PathTimeStamp}】发送成功");
                                }
                                else
                                {
                                    SendCurPathPkgOk = SendPathPkgState.NotMatch;
                                    LOGGER.Info($"{PrefixName} 当前路径包【{CurPathDataPkg?.TimeStamp}】与收到的路径回复包【{recv.PathTimeStamp}】不一致");
                                }
                            }
                            else
                            {
                                SendCurPathPkgOk = SendPathPkgState.Fail;
                                LOGGER.Warn($"{PrefixName} 路径包的应答包错误【{bytesTostr(Msg)}】");
                            }

                        }
                        else
                        {
                            LOGGER.Error($"{PrefixName} 解析路径包的应答包失败【{bytesTostr(Msg)}】");
                        }

                    }
                    handleActionFinishResonse(cmdWord, Msg);
                }
                else
                {
                    LOGGER.Error($"{PrefixName} 解析命令字失败【{bytesTostr(Msg)}】");
                }
                process(client, Msg);
            }
            catch (Exception ex)
            {
                LOGGER.Error($"{PrefixName} 处理响应异常【{bytesTostr(Msg)}】", ex);
            }
        }
        void DebugInfo(string msg)
        {
            LOGGER.Debug($"{PrefixName}{msg}");
        }
        void LogInfo(string msg)
        {
            LOGGER.Info($"{PrefixName}{msg}");
        }
        string preStatusStr = "";
        void LogStatusInfo(string msg)
        {
            if (preStatusStr != msg)
            {
                LOGGER.Info($"{PrefixName}{msg}");
            }
            preStatusStr = msg;
        }
        string preFaultStr = "";
        void LogFaultInfo(string msg)
        {
            if (preFaultStr != msg)
            {
                LOGGER.Warn($"{PrefixName}{msg}");
            }
            preFaultStr = msg;
        }
        /// <summary>
        /// 停到最近二维码完成
        /// </summary>
        public bool StopAtNearPointCode = false;
        /// <summary>
        /// 收到的动作完成数据包处理
        /// </summary>
        /// <param name="cmdWord"></param>
        /// <param name="msgs"></param>
        void handleActionFinishResonse(byte cmdWord, byte[] msgs)
        {
            try
            {
                if (PkgData.IsActionFinishResonse(cmdWord))
                {
                    var recv = new ActionFinishPkg();
                    if (recv.Parse(msgs))
                    {
                        uint addrcode = Common.ConvertToUInt(recv.AddrCodeID);
                        var finishCode = (ActionFinishCode)recv.ActionCode;
                        setLandmark(addrcode);
                        string locateCode = Common.ConvertToStr(recv.ContainerCode);
                        string shelveCode = Common.ConvertToStr(recv.ShelvesCode);
                        string boxCode = Basket.LocateCodeToBoxCode(locateCode);
                        LOGGER.Info($"{PrefixName} 收到动作完成数据包,地标码【{addrcode}】，完成动作【{finishCode}】，朝向【{Common.ConvertToUshort(recv.Dir)}】,货架码=【{shelveCode}】，料箱码=【{boxCode}】，定位码=【{locateCode}】【{bytesTostr(msgs)}】");
                        CurDir = Common.ConvertToUshort(recv.Dir);
                        // if (DestPointCode == addrcode)
                        {
                            if (finishCode == ActionFinishCode.整个路径执行完成)
                            {
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】整个路径结束");
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = PathPointFinish,
                                    BoxCode = boxCode,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                            }
                            else if (finishCode == ActionFinishCode.往货架放货完成)
                            {
                                containerCodeIntoShelve(locateCode, shelveCode);
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = PathPointFinish,
                                    BoxCode = boxCode,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】往货架【{shelveCode}】放货完成");
                            }
                            else if (finishCode == ActionFinishCode.往料仓放货完成)
                            {
                                //更新料仓的料箱定位码
                                containerCodeIntoBaket(locateCode);
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = PathPointFinish,
                                    BoxCode = boxCode,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】往料仓放货完成");
                            }
                            else if (finishCode == ActionFinishCode.从货架取货完成)
                            {
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    Finished = true,
                                    FinishState = CurTaskFinishState,
                                    BoxCode = boxCode,
                                    ActionFinishCode = finishCode,
                                    PointCode = addrcode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】从货架【{shelveCode}】取货完成");
                            }
                            else if (finishCode == ActionFinishCode.从料仓取货完成)
                            {
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    Finished = true,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                    BoxCode = boxCode,
                                    PointCode = addrcode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】从料仓取货完成");
                            }
                            else if (finishCode == ActionFinishCode.充电对接完成)
                            {
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = true,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】充电对接完成");
                            }
                            else if (finishCode == ActionFinishCode.充满电)
                            {
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = true,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】充满电");
                            }
                            else if (finishCode == ActionFinishCode.转弯完成)//退出充电
                            {
                                PathActionFinish?.Invoke(new ActionFInishArg()
                                {
                                    CTU = this,
                                    PointCode = addrcode,
                                    Finished = PathPointFinish,
                                    FinishState = CurTaskFinishState,
                                    ActionFinishCode = finishCode,
                                });
                                LOGGER.Info($"{PrefixName} 【{CurPathDataPkg.TimeStamp}】转弯完成");
                            }
                            else
                            {
                                //PathActionFinish?.Invoke(new ActionFInishArg()
                                //{
                                //    CTU = this,
                                //    Finished = true,
                                //    FinishState = CurTaskFinishState,
                                //    ActionFinishCode = finishCode,
                                //    BoxCode = boxCode,
                                //});
                                LOGGER.Info($"{PrefixName} 【{finishCode}】动作完成");
                            }

                        }
                        var rtn = new ActionFinishRtnPkg()
                        {
                            AgvID = this.AGVID,
                            TimeStamp = PkgData.genTimeStamp(),
                            ResponseCmd = recv.ActionCode,
                            FrameLenght = 14
                        };
                        waitSendPkgs.Enqueue(rtn);
                        LOGGER.Info($"{PrefixName} 发送动作完成数据回复包【{bytesTostr(rtn.ToBytes())}】");
                    }
                    else
                    {
                        LOGGER.Error($"{PrefixName} 解析动作完成数据包失败【{bytesTostr(msgs)}】");
                    }
                }
            }
            catch (Exception ex)
            {
                LOGGER.Error($"{PrefixName} handleActionFinishResonse error:", ex);
            }

        }
        /// <summary>
        /// 料箱放入料仓，记录料箱定位码
        /// </summary>
        /// <param name="locateCode"></param>
        void containerCodeIntoBaket(string locateCode)
        {
            var basket = Baskets.Find(s => locateCode.Equals(s.LocateCode));
            if (basket != null)
            {
                basket.IsPutted = true;
                LOGGER.Info($"{PrefixName} 料箱【{locateCode}】【{basket.BoxCode}】放入背篓【{basket.Row}】完成");
            }
            saveBaskets();
        }
        /// <summary>
        /// 料箱放入货架，更新料箱定位码
        /// </summary>
        /// <param name="locateCode"></param>
        void containerCodeIntoShelve(string locateCode, string shelveCode)
        {
            var basket = Baskets.Find(s => locateCode.Equals(s.LocateCode));
            if (basket != null)
            {
                LOGGER.Info($"{PrefixName} 料箱【{locateCode}】【{basket.BoxCode}】放入货架【{shelveCode}】,清空背篓【{basket.Row}】");
                ClearBasketByLocateCode(locateCode);
            }
            saveBaskets();
        }
        /// <summary>
        /// 更新料箱在背篓的位置
        /// </summary>
        /// <param name="locateCodes"></param>
        void UpdateStoreInfo(List<string> locateCodes)
        {
            //if (locateCodes == null || locateCodes.Count == 0)
            //{
            //    Baskets.ForEach(s =>
            //    {
            //        if (!string.IsNullOrEmpty(s.BoxCode) && !s.IsPutted)
            //        {
            //            LOGGER.Info($"{PrefixName} 所有定位码为空，清除未放箱状态的背篓【{s.Row}】【{s.BoxCode}】");
            //            ClearBasket(s.Row);
            //        }

            //    }
            //    );
            //    return;
            //}
            var detectBaskets = new Dictionary<int, string>();

            for (int i = 0; i < locateCodes.Count; i++)
            {
                string code = locateCodes[i];
                //if (string.IsNullOrEmpty(code)) continue;
                detectBaskets[i + 1] = code;
            }
            if (detectBaskets.Count == 0)
            {
                return;
            }

            //foreach (var item in Baskets)
            //{
            //    if (!item.IsPutted) continue;
            //    if (locateCodes?.Contains(item.LocateCode) ?? false)
            //    {
            //        if (item.Shield)
            //        {
            //            item.Shield = false;
            //            LOGGER.Info($"{PrefixName} 背篓屏蔽状态变化 存在料箱 启用【{item.Shield}】【{item.Row}】【{item.BoxCode}】【{item.LocateCode}】");
            //        }

            //    }
            //    else
            //    {
            //        if (!item.Shield)
            //        {
            //            item.Shield = true;
            //            LOGGER.Info($"{PrefixName} 背篓屏蔽状态变化 不存在料箱 屏蔽【{item.Shield}】【{item.Row}】【{item.BoxCode}】【{item.LocateCode}】");
            //        }

            //    }
            //}

            foreach (var item in detectBaskets)
            {
                var row = item.Key;
                var code = item.Value;
                if (string.IsNullOrEmpty(code))
                {
                    var hasRecordInBasket = Baskets.Find(s => !string.IsNullOrEmpty(s.BoxCode));
                    if (hasRecordInBasket == null)
                    {
                        ClearBasket((byte)row);
                        LOGGER.Info($"{PrefixName}料仓信息包确认 背篓【{row}】无料箱定位码【{code}】，清空该层记录");
                    }
                    continue;
                }
                if (code.StartsWith("XXX")) continue;
                var finds = Baskets.FindAll(s => code.Equals(s.LocateCode));
                if (finds != null)
                {
                    if (finds.Count > 1)
                    {
                        LOGGER.Error($"{PrefixName}料仓信息包确认【{code}】 存在多个背篓中，异常");
                        continue;
                    }
                    else if (finds.Count == 1)
                    {
                        var basket = finds[0];
                        if (basket.Row == row)
                        {
                            basket.IsPutted = true;
                            LOGGER.Info($"{PrefixName}料仓信息包确认【{code}】 背篓【{basket.Row}】料箱定位码【{code}】确认");
                        }
                        else //料箱位置与检测到的不符
                        {
                            LOGGER.Info($"{PrefixName}料仓信息包确认 【{basket.BoxCode}】料箱定位码【{code}】由背篓【{basket.Row}】转移到背篓【{row}】");
                            SetBasketWithLocateCode((byte)row, basket.LocateCode, basket.DstName, true);
                            ClearBasket(basket.Row);
                        }
                    }
                    else //现有背篓没有记录该料箱
                    {
                        LOGGER.Info($"{PrefixName}料仓信息包确认 料箱定位码【{code}】在背篓【{row}】，但无记录1,写入");
                        SetBasketWithLocateCode((byte)row, code, "", true);
                    }
                }
                else //现有背篓没有记录该料箱
                {
                    LOGGER.Info($"{PrefixName}料仓信息包确认 料箱定位码【{code}】在背篓【{row}】，但无记录2,写入");
                    SetBasketWithLocateCode((byte)row, code, "", true);
                }
            }
        }
        /// <summary>
        /// 创建路径点
        /// </summary>
        /// <param name="pathPoints"></param>
        public bool CreatePathPoint(List<PathPoint> pathPoints, uint dstPointCode, string finishState)
        {
            try
            {
                CurTaskFinishState = finishState;
                DestPointCode = dstPointCode;
                PathPoints = pathPoints;
                CurPathDataPkg = new PathDataPkg();
                CurPathDataPkg.TimeStamp = PkgData.genTimeStamp();
                CurPathDataPkg.AgvID = AGVID;
                CurPathDataPkg.PathPoints = PathPoints.ToList();
                CurPathDataPkg.PathLength = (byte)pathPoints?.Count;
                int len = 0;
                CurPathDataPkg.PathPoints?.ForEach(s => len += s.ToBytes().Length);
                CurPathDataPkg.FrameLenght = (ushort)(10 + len);
                waitSendPkgs.Enqueue(CurPathDataPkg);
                SendCurPathPkgOk = SendPathPkgState.None;
                ClearUsedPoints();
                lastSendPathPkgTime = DateTime.Now;
                ClearMovePoints();
                CurPathDataPkg.PathPoints.ForEach(s =>
                {
                    AddMovePoint(s.LandMark);
                    AddUsedPoint(s.LandMark);
                    LOGGER.Info($"{PrefixName} 添加路径点并占用：{s.Msg}");
                });
                //CurPathDataPkg.PathPoints.ForEach(s => LOGGER.Info($"{PrefixName}路径点：{s.Msg}")); ;
                LOGGER.Info($"{PrefixName} 创建路径点，时间戳【{CurPathDataPkg.TimeStamp}】【{bytesTostr(CurPathDataPkg.ToBytes())}】");
                return true;
            }
            catch (Exception ex)
            {
                LOGGER.Error($"{PrefixName} 创建路径点异常:{ex.ToString()}");
            }
            return false;
        }
        /// <summary>
        /// 重发当前执行的路径点
        /// </summary>
        public void ResendPathPoint(uint dstPointCode)
        {
            DestPointCode = dstPointCode;
            CurPathDataPkg = new PathDataPkg();
            CurPathDataPkg.TimeStamp = PkgData.genTimeStamp();
            CurPathDataPkg.AgvID = AGVID;
            CurPathDataPkg.PathPoints = PathPoints.ToList();
            CurPathDataPkg.PathLength = (byte)PathPoints?.Count;
            int len = 0;
            CurPathDataPkg.PathPoints?.ForEach(s => len += s.ToBytes().Length);
            CurPathDataPkg.FrameLenght = (ushort)(10 + len);
            waitSendPkgs.Enqueue(CurPathDataPkg);
            SendCurPathPkgOk = SendPathPkgState.None;
            lastSendPathPkgTime = DateTime.Now;
            CurPathDataPkg.PathPoints.ForEach(s =>
            {
                if (!UsedPoint.Contains(s.LandMark))
                {
                    UsedPoint.Add(s.LandMark);
                }
                LOGGER.Info($"{PrefixName} 添加路径点并占用：{s.Msg}");
            });
            //CurPathDataPkg.PathPoints.ForEach(s => LOGGER.Info($"{PrefixName}路径点：{s.Msg}")); ;
            LOGGER.Info($"{PrefixName} 重发路径点，时间戳【{CurPathDataPkg.TimeStamp}】【{bytesTostr(CurPathDataPkg.ToBytes())}】");
        }
        public void ManualPathPoint(List<PathPoint> pathPoints)
        {
            if (AGVID <= 0) return;
            if (RunState != RunState.Executing)
            {
                MessageBox.Show($"{PrefixName}未归位扫描，不允许手动");
                return;
            }
            CurPathDataPkg = new PathDataPkg();
            CurPathDataPkg.TimeStamp = PkgData.genTimeStamp();
            CurPathDataPkg.AgvID = AGVID;
            CurPathDataPkg.PathPoints = pathPoints.ToList();
            CurPathDataPkg.PathLength = (byte)pathPoints?.Count;
            int len = 0;
            CurPathDataPkg.PathPoints?.ForEach(s => len += s.ToBytes().Length);
            CurPathDataPkg.FrameLenght = (ushort)(10 + len);
            waitSendPkgs.Enqueue(CurPathDataPkg);
            SendCurPathPkgOk = SendPathPkgState.None;
            LOGGER.Info($"{PrefixName}手动发送路径数据包【{CurPathDataPkg.TimeStamp}】 【{bytesTostr(CurPathDataPkg.ToBytes())}】");
        }
        public void CreateAction(ActionDataPkg actionData, bool manual = false)
        {
            var find = waitSendPkgs.Where(s => s.CmdWord == actionData.CmdWord).FirstOrDefault();
            if (find == null)
            {
                waitSendPkgs.Enqueue(actionData);
                if (manual)
                    LOGGER.Info($"{PrefixName}手动发送【{actionData.TimeStamp}】 {bytesTostr(new byte[] { actionData.ActionCode })} 单独数据包");
                else
                    LOGGER.Info($"{PrefixName}发送【{actionData.TimeStamp}】 {bytesTostr(new byte[] { actionData.ActionCode })} 单独数据包");
            }
        }
        public void ClearPathPoint()
        {
            CurPathDataPkg = new PathDataPkg();
            PathPoints.Clear();
        }

        public void StopAtNearLandmark()
        {
            var reset = 停到最近二维码(out PathPoint pathPoint);
            waitSendPkgs.Enqueue(reset);
        }
        ActionDataPkg 归位扫描(out PathPoint pathPoint)
        {
            ActionDataPkg dataPkg = new ActionDataPkg();
            dataPkg.TimeStamp = PkgData.genTimeStamp();
            dataPkg.AgvID = AGVID;
            pathPoint = PathPoint.GenGuiweiscan(CurLandMark);
            dataPkg.ActionCode = (byte)pathPoint.Action;
            dataPkg.PropVal = pathPoint.PropVal;
            dataPkg.ActionParams = pathPoint.ActionParam;
            dataPkg.FrameLenght = (ushort)(11 + dataPkg?.ActionParams.Count);

            return dataPkg;
        }
        ActionDataPkg 停到最近二维码(out PathPoint pathPoint)
        {
            ActionDataPkg dataPkg = new ActionDataPkg();
            dataPkg.TimeStamp = PkgData.genTimeStamp();
            dataPkg.AgvID = AGVID;
            pathPoint = PathPoint.GenStopAtNearLandmark(CurLandMark);
            dataPkg.ActionCode = (byte)pathPoint.Action;
            dataPkg.PropVal = pathPoint.PropVal;
            dataPkg.ActionParams = pathPoint.ActionParam;
            dataPkg.FrameLenght = (ushort)(11 + dataPkg?.ActionParams.Count);
            StopAtNearPointCode = false;
            return dataPkg;
        }

        static Dictionary<byte, string> cmdwordExplain = new Dictionary<byte, string>()
        {
            { 0x01,"路径数据包"},
             { 0x02,"动作命令包"},
                 { 0x03,"心跳包"},
     { 0x80,"重新登录命令包"},
          { 0xF0,"登录回复包"},
               { 0xF1,"料箱信息回复包"},
   { 0xF2,"RFID信息回复包"},
      { 0x04,"强制故障清除命令包"},
         { 0xA0,"应答包-异常"},
            { 0xA1,"应答包-正常"},
        };
        void process(TcpClientBean client, byte[] msg)
        {
            switch (RunState)
            {
                case RunState.Online:
                    if ((DateTime.Now - lastrecvTime).TotalSeconds > 6)
                    {
                        RunState = RunState.Reset;
                        var reset = 归位扫描(out PathPoint pathPoint);
                        waitSendPkgs.Enqueue(reset);
                        LOGGER.Info($"{PrefixName} 归位扫描【{reset.TimeStamp}】【{reset.CmdWord}】【{bytesTostr(reset.ToBytes())}】");
                    }
                    break;
                case RunState.Reset:
                    break;
                case RunState.Executing:
                    try
                    {
                        if (CurPathDataPkg != null)
                        {
                            if (ConfigAppSettings.GetValue(Common.Mushiny_ + "EnableAutoReSendPathPointWhenTimeout", false))
                            {
                                if (CurPathDataPkg.CmdWord == 0x01 && SendCurPathPkgOk == SendPathPkgState.None && (DateTime.Now - lastSendPathPkgTime).TotalSeconds > 10)
                                {
                                    ResendPathPoint(DestPointCode);
                                    lastSendPathPkgTime = DateTime.Now;
                                    LOGGER.Info($"【{Name}】自动重发路径包【{JsonHelper.SerializeObject(CurPathDataPkg)}】");
                                }
                            }

                        }
                    }
                    catch (Exception ex)
                    {
                        LOGGER.Error($"自动重发路径异常【{JsonHelper.SerializeObject(CurPathDataPkg)}】", ex);
                    }

                    break;
            }
            int tryCnt = 5;
            while (waitSendPkgs.Count > 0 && tryCnt > 0)
            {
                if (waitSendPkgs.TryPeek(out var item))
                {
                    try
                    {
                        var waitSend = item.ToBytes();
                        int len = client.ClientSocket.Send(waitSend);
                        if (len == waitSend.Length)
                        {
                            item.Sended = true;
                            if (item.CmdWord != 0x03)
                            {
                                if (cmdwordExplain.ContainsKey(item.CmdWord))
                                {
                                    LOGGER.Info($"{PrefixName} 发送完成：时间戳【{item.TimeStamp}】命令字【{cmdwordExplain[item.CmdWord]}】【{bytesTostr(waitSend)}】");
                                }
                                else
                                {
                                    LOGGER.Info($"{PrefixName} 发送完成：时间戳【{item.TimeStamp}】【{item.CmdWord}】【{bytesTostr(waitSend)}】");
                                }
                            }

                            if (item.CmdWord == 0x01)//路径包命令
                            {
                                lastSendPathPkgTime = DateTime.Now;
                            }
                            waitSendPkgs.TryDequeue(out _);
                        }
                        else
                        {
                            tryCnt--;
                        }
                    }
                    catch (Exception ex)
                    {
                        LOGGER.Error($"{PrefixName}【{item.TimeStamp}】发送{item.CmdWord}数据包异常", ex);
                    }
                }

                Thread.Sleep(60);
            }
        }

        public string GetStateStr()
        {
            string ctuError = FaultInfo.HasFault ? FaultInfo.ToShowStr() : "";
            {
                if (RunState == RunState.Executing)
                {
                    return $"{ctuTask?.MoveInfo?.Msg}";
                }
                else if (RunState == RunState.Offline)
                {
                    return $"CTU故障[{ctuError}]";
                }
                else if (RunState == RunState.Reset)
                {
                    return $"回原中";
                }
            }
            return ctuTask?.MoveInfo?.Msg;
        }
        /// <summary>
        /// 启用中的显示异常
        /// </summary>
        public void ShowAgvState()
        {
            if (FaultInfo.HasFault && (Common.GetFaultId(FaultInfo.FaultID, out FaultId faultId)))
            {
                if (MushinyManager.canShow(faultId))
                {
                    Msg.add($"【{Name}】【{CurLandMark}】故障：【{faultId}】", MsgLevel.warning);
                }
            }
        }
    }

    public class ActionFInishArg : EventArgs
    {
        public CTU CTU { get; set; }

        /// <summary>
        /// 当前地码
        /// </summary>
        public uint PointCode { get; set; }
        public bool Finished { get; set; }
        public string BoxCode { get; set; }
        public string FinishState { get; set; }
        public ActionFinishCode ActionFinishCode { get; set; }
    }
    public class FaultEventArg : EventArgs
    {
        public CTU CTU { get; set; }

        public string BoxCode { get; set; }

        public string FinishState { get; set; }
        public bool HasFault { get; set; }
        public bool NeedResetFault { get; set; }
        public string FaultStr { get; set; }
        /// <summary>
        /// 故障id
        /// </summary>
        public byte[] FaultID { get; set; }
    }
    /// <summary>
    /// 发送数据包的状态
    /// </summary>
    public enum SendPathPkgState
    {
        None,
        /// <summary>
        /// 成功
        /// </summary>
        Success,
        /// <summary>
        /// 失败
        /// </summary>
        Fail,
        /// <summary>
        /// 不匹配
        /// </summary>
        NotMatch
    }
    /// <summary>
    /// 故障信息
    /// </summary>
    public class CTUFaultInfo
    {
        /// <summary>
        /// 故障标志位
        /// </summary>
        public FaultFlag FaultFlag { get; set; }
        /// <summary>
        /// 电池故障标志位
        /// </summary>
        public byte BatteryFaultFlag { get; set; }
        /// <summary>
        /// 是否有故障
        /// </summary>
        public bool HasFault { get; set; }
        /// <summary>
        /// 需要重置的异常
        /// </summary>
        public bool NeedResetFault { get; set; }
        /// <summary>
        /// 故障id
        /// </summary>
        public byte[] FaultID { get; set; }

        /// <summary>
        /// 故障数据1
        /// </summary>
        public int FaultData1 { get; set; }
        /// <summary>
        /// 故障数据2
        /// </summary>
        public int FaultData2 { get; set; }

        public string ToStr()
        {
            return $"【{(HasFault ? "有" : "无")}故障】【{FaultFlag}】故障id【{(Common.GetFaultId(FaultID, out FaultId faultId) ? faultId.ToString() : Common.BytesTostr(FaultID))}】" +
                $"故障数据1【{FaultData1}】故障数据2【{FaultData2}】电池故障标志位【{BatteryFaultFlag}】";
        }
        public string ToUploadStr()
        {
            return $"故障标志【{FaultFlag}】故障id【{(Common.GetFaultId(FaultID, out FaultId faultId) ? faultId.ToString() : Common.BytesTostr(FaultID))}】" +
                $"故障数据1【{FaultData1}】故障数据2【{FaultData2}】电池故障标志位【{BatteryFaultFlag}】";
        }
        public string ToShowStr()
        {
            return $"{(Common.GetFaultId(FaultID, out FaultId faultId) ? faultId.ToString() : Common.BytesTostr(FaultID))}";
        }
    }
    public enum FaultId
    {
        无,
        下主控系统故障_时钟_存储 = 0x0001,
        下主控系统参数异常,
        上主控系统故障_时钟_存储,
        上主控系统参数异常,
        待机状态下丢码 = 0x2001,
        直行中丢码,
        直行中错码,
        直行过程因错码停下_要求路径重算,
        直行末尾点丢码,
        转弯中丢码,
        转弯中错码_要求路径重算,
        校正过程中丢码,
        地址码贴歪严重,
        直行过程中左右偏差过大 = 0x2010,
        直行过程中角度偏差过大,
        位姿融合失败,
        直行过程中发生角度切换,
        充电对接过程中左右偏差过大,
        动作码参数异常 = 0x2020,
        命令码参数异常,

        上主控失联 = 0x2100,
        货架码不匹配,
        料箱码不匹配,
        货架码丢失,
        料箱码丢失,
        上下调整超限,
        前后调整超限,
        升降过程异常,
        伸缩过程异常,
        旋转过程异常,
        挡杆动作异常,
        放料箱过程发现货架_料仓已存在料箱,
        取料箱过程发现货架_料仓无料箱,
        取料箱前发现货叉已存在料箱,
        行走过程中料仓状态变动,
        行走过程中货叉状态变动,

        货叉取货后发现货物丢失 = 0x2110,
        往料仓放货后发现货物丢失,
        宽度调节过程异常,
        宽度调节偏差超限,
        货叉动作参数有误 = 0x2120,
        雷达失联 = 0x3000,
        直行前进中前雷达触发停止,
        直行后退中后雷达触发停止,
        转弯过程中雷达触发停止,
        充电退出过程中雷达触发停止,
        取放货对接退出过程中雷达触发停止,

        雷达模块报警 = 0x3006,

        急停触发 = 0x3010,
        触边触发,

        下视摄像头失联 = 0x4001,
        下视摄像头通信异常,
        下视摄像头数据异常,

        货叉摄像头失联 = 0x4011,
        货叉摄像头通信异常,
        货叉摄像头数据异常,

        惯导模块失联 = 0x4031,
        惯导模块通信异常,
        惯导模块数据异常,

        拉线编码器失联 = 0x4041,
        拉线编码器通信异常,
        拉线编码器数据异常,

        料仓货位传感器异常 = 0x4051,
        货叉货位传感器异常,
        升降下限位传感器异常,
        升降上限位传感器异常,
        伸缩前限位传感器异常,
        伸缩后限位传感器异常,
        旋转零位传感器异常,
        旋转左限位传感器异常,
        旋转右限位传感器异常,
        货叉避障传感器触发 = 0x4060,
        宽度调节限位传感器异常,

        底盘环境温度异常 = 0x4081,
        烟雾报警,

        外设模块失联 = 0x5001,
        外设模块通信异常,
        液晶屏失联,
        顶端灯带失联,
        语音播报模块失联,


        电机失联 = 0x6001,
        电机报出故障,
        电机长时间无响应,
        电机非法复位,

        电池通信失联 = 0x7001,
        电池电量异常,
        电池过压报警,
        电池欠压报警,
        电池温度过高,
        电池温度过低,
        电池剩余电量过低,

    }
}