using OnlineStore.Common;
using OnlineStore.LoadCSVLibrary;
using System;
using System.Collections.Concurrent;
using JAKA;
using System.Threading.Tasks;

namespace OnlineStore.DeviceLibrary
{
    public partial class RobotEquip : EquipBase
    {

        /// <summary>
        /// 行走机构
        /// </summary>
        public AxisBean MoveAxis = null;
        /// <summary>
        /// 压紧轴
        /// </summary>
        public AxisBean CompressAxis = null;
        /// <summary>
        /// 轴列表
        /// </summary>
        public AxisBean[] axisArray;
        public JAKAServer JAKAServer;
        public Client_Config Config;
        public InOutPosInfo lastPosInfo;
        public string executeTime = "";
        public DeviceStatus deviceStatus = DeviceStatus.ResetMove;
        public string CID = "";
        /// <summary>
        /// 等待出库的队列
        /// </summary>
        public ConcurrentQueue<InOutParam> waitOutStoreList = new ConcurrentQueue<InOutParam>();
        public RobotEquip(Client_Config config, int id)
        {
            baseConfig = config;
            this.Config = config;
            this.DeviceID = id;
            CID = config.CID;
            IsDebug = config.IsDebug.Equals(1);
            Name = (" " + "机器人" + " ").ToUpper();
            Init();
            InitConnectServerTimer();
            ledProcessTimer.Elapsed += LedProcess;
            IoCheckTimer.Elapsed += IoCheckTimerProcess;
            MoveInfo = new DeviceMoveInfo(" 机器人 ");
            InitAllAxis();
            lastPosInfo = new InOutPosInfo();
            LoadCameraConfig();
            string[] clientIps = new string[] { config.Robot_IP };
            JAKAServer = new JAKAServer(clientIps);
            StartRobotService();
        }
        public void StartRobotService()
        {
            Task.Factory.StartNew(delegate
            {
                JAKAServer.Start();
                PowerOn(true);
                System.Threading.Thread.Sleep(2000);
                EnableRobot(true);
                System.Threading.Thread.Sleep(2000);
            });

        }
        public void StopRobotService()
        {
            try
            {
                Task.Factory.StartNew(delegate
                {
                    EnableRobot(false);
                    System.Threading.Thread.Sleep(2000);
                    PowerOn(false);
                    JAKAServer.Stop();
                });
            }
            catch (Exception e)
            {
                LogUtil.error("StopRobotService 出错", e);
            }

        }
        internal void InitAllAxis()
        {
            MoveAxis = new AxisBean(Config.MoveAxis, Name);
            CompressAxis = new AxisBean(Config.CompressAxis, Name);
            axisArray = new AxisBean[] {
                MoveAxis,CompressAxis
            };
        }

        public override bool StartRun()
        {
            if (CanStartRun().Equals(false))
            {
                return false;
            }
            bool rtn = false;
            Task task = Task.Factory.StartNew(new Action(() => { rtn = OpenAllAxis(); }));
            task.Wait();
            if (!rtn)
            {
                CloseAllAxis();
                return false;
            }
            SetAllTimer(false);
            SetConnectServerTimer(false);
            MoveInfo.EndMove();
            SetBoxStatus(DeviceStatus.ResetMove, RunStatus.HomeMoving);
            MoveInfo.NewMove(MoveType.RHome);
            LogInfo("开始 原点返回：");
            StartReset();
            SetConnectServerTimer(true);
            SetAllTimer(true);
            return true;
        }

        public override bool Reset()
        {
            StopMove();
            bool rtn = false;
            Task task = Task.Factory.StartNew(new Action(() => { rtn = OpenAllAxis(); }));
            task.Wait();
            if (!rtn)
            {
                CloseAllAxis();
                return false;
            }
            mainTimer.Enabled = false;
            SetBoxStatus(DeviceStatus.ResetMove, RunStatus.Reset);
            MoveInfo.NewMove(MoveType.Reset);

            LogInfo("开始重置：  ");
            StartReset();
            mainTimer.Enabled = true;
            return true;
        }


        #region 复位处理

        private void StartReset()
        {
            ClearWarnMsg();
            alarmType = AlarmType.None;
            isInSuddenDown = false;
            isNoAirpressure_Check = false;
            MoveInfo.NextMoveStep(StepEnum.Robot_Reset_01_Wait);
            LogInfo($"复位 {MoveInfo.SLog} ：轴开始复位");
            StartProg(true);
            curcmd = "";
            MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(2000));
            isInPro = false;
        }

        protected override void ResetProcess()
        {
            if (MoveInfo.MoveType.Equals(MoveType.Reset) || MoveInfo.MoveType.Equals(MoveType.RHome))
            {
                MoveResetPro();
            }
        }
        static int axisDiff = 20000;
        private bool CheckTongsPos()
        {
            return false;
        }
        private void MoveResetPro()
        {
            if (MoveInfo.IsInWait)
            {
                CheckWait(MoveInfo);
            }
            if (MoveInfo.IsInWait)
            {
                return;
            }
            switch (MoveInfo.MoveStep)
            {
                case StepEnum.Robot_Reset_01_Wait:
                    NextMoveStep(StepEnum.Robot_Reset_02_CheckArmState, "检查机器人是否在原点");
                    break;
                case StepEnum.Robot_Reset_02_CheckArmState:
                    NextMoveStep(StepEnum.Robot_Reset_03_ArmToP1, "机器人回原点");
                    SendCmd(GetCmd_Home());
                    break;
                case StepEnum.Robot_Reset_03_ArmToP1:
                    NextMoveStep(StepEnum.Robot_Reset_04_CheckTray, "检查机械臂是否有料盘");
                    break;
                case StepEnum.Robot_Reset_04_CheckTray:
                    if (BufferDataManager.TrayInRobotInfo == null)
                    {
                        NextMoveStep(StepEnum.Robot_Reset_05_AllAxisToP1, "压紧轴无料盘，行走机构和压紧轴回待机点");
                        MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P1, Config.MoveAxis_P1_Speed);
                        CompressAxis.AbsMove(MoveInfo, Config.CompressAxis_P1, Config.CompressAxis_P1_Speed);
                    }
                    else
                    {
                        NextMoveStep(StepEnum.Robot_Reset_06_MoveAxisToP1, "压紧轴有料盘，行走机构回待机点");
                        MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P1, Config.MoveAxis_P1_Speed);
                    }
                    break;
                case StepEnum.Robot_Reset_05_AllAxisToP1:
                    NextMoveStep(StepEnum.Robot_Reset_08_Finish, "复位完成");
                    break;
                case StepEnum.Robot_Reset_06_MoveAxisToP1:
                    NextMoveStep(StepEnum.Robot_Reset_07_ClearCompress, "将压紧轴上的料盘送到指定位置");
                    break;
                case StepEnum.Robot_Reset_07_ClearCompress:

                    if (BufferDataManager.TrayInRobotInfo != null)
                    {
                        MoveInfo.EndMove();
                        runStatus = RunStatus.Runing;
                        BufferDataManager.TrayInRobotInfo = null;
                    }
                    else
                    {
                        NextMoveStep(StepEnum.Robot_Reset_08_Finish, "复位完成");
                    }
                    break;
                case StepEnum.Robot_Reset_08_Finish:
                    MoveInfo.EndMove();
                    runStatus = RunStatus.Runing;
                    break;
            }
        }


        #endregion
        /// <summary>
        /// 是否在安全位置
        /// </summary>
        /// <returns></returns>
        public bool IsInSafePosition()
        {
            return true;
        }
        private void NextMoveStep(StepEnum step, string msg)
        {
            MoveInfo.NextMoveStep(step);
            LogInfo($" {MoveInfo.SLog}：{msg}");
        }
        private string preWriteStr = "";
        private string curWriteStr = "";
        void LogInOutErrInfo(string msg)
        {
            curWriteStr = msg;
            if (curWriteStr != preWriteStr)
            {
                preWriteStr = curWriteStr;
                LogUtil.error(curWriteStr);
            }

        }
        int SigLastTime = 2;//5秒
        protected override void OnTimerProcess()
        {
            if (!runStatus.Equals(RunStatus.Runing))
                return;
        }
        private void CheckInStoreProcess()
        {
            if (StoreManager.Client.inletEquip.UpdownIsUp() && BufferDataManager.TrayInInletInfo != null)
            {
                LogInfo("机器人执行入库【" + BufferDataManager.TrayInInletInfo.ToStr() + "】");
                StartInstore(new InOutParam(BufferDataManager.TrayInInletInfo));
            }
        }
        private void ExecuteOutListProcess()
        {
            try
            {
                if (waitOutStoreList.Count > 0)
                {
                    InOutParam param = null;
                    bool result = waitOutStoreList.TryDequeue(out param);
                    if (result && param != null)
                    {
                        LogInfo("执行排队的出库【" + param.PosInfo.ToStr() + "】");
                        StartExecuctOut(param);
                        return;
                    }

                }
            }
            catch (Exception ex)
            {
                LogUtil.error(Name + "处理出库排队列表出错：" + ex.ToString());
            }
        }
        protected override void BaseTimerProcess()
        {
            if (isInSuddenDown || isNoAirpressure_Check)
            {
                return;
            }
            SecurityAccessProcess();
            NGBoxProcess();
            if (runStatus.Equals(RunStatus.Runing))
            {
                ////ExecuteOutListProcess();
                if (StoreManager.IsConnectServer)
                {
                    GetRobotTask();
                }
                CheckInStoreProcess();
            }
            BusyMoveProcess();
            IOTimeOutProcess();
            if (NoErrorAlarm())
            {
                CheckAxisAlarm();
            }
        }
        protected override void IOTimeOutProcess()
        {
            try
            {
                TimeSpan span = DateTime.Now - preIoTimerOutTime;
                if (span.TotalSeconds > 3 && alarmType.Equals(AlarmType.IoSingleTimeOut))
                {
                    preIoTimerOutTime = DateTime.Now;

                    if (runStatus < RunStatus.Runing || isInSuddenDown || isNoAirpressure_Check)
                    {
                        return;
                    }
                    ////若BOX和出料都没有在等待Io的过程中则此Io超时异常可能已经处理过
                    if (MoveInfo.IsInWait.Equals(false) && NoErrorAlarm())
                    {
                        LogUtil.info(Name + "清理信号超时报警【" + WarnMsg + "】 ");
                        alarmType = AlarmType.None;
                        ClearWarnMsg();
                    }
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("IOTimeOutProcess出错：", ex);
            }
        }
        internal override void StopMove()
        {

            MoveInfo.EndMove();
            MoveAxis.SuddenStop();
            CompressAxis.SuddenStop();
            StartProg(false);
            CloseAllAxis();
            curcmd = "";
        }

        public override void StopRun()
        {
            SetConnectServerTimer(false);
            SetAllTimer(false);
            StopMove();
            SetBoxStatus(DeviceStatus.None, RunStatus.Wait);
        }

        public override string GetMoveStr()
        {
            string msg = "";
            int tLength = 15;

            msg += "runStatus：  " + runStatus + "\n";
            msg += "runStep：  " + MoveInfo.MoveStep + "\n";
            msg += "alarm：  " + alarmType + "\n";



            return msg;
        }

        #region 轴运动

        private DateTime checkAlarmTime = DateTime.Now;
        public void CheckAxisAlarm()
        {
            if (alarmType.Equals(AlarmType.AxisAlarm) || alarmType.Equals(AlarmType.AxisMoveError))
            {
                return;
            }
            TimeSpan span = DateTime.Now - checkAlarmTime;
            //在回原点，复位，出入库时，检测报警间隔减小
            if (MoveInfo.MoveType.Equals(MoveType.None))
            {
                if (span.TotalSeconds < 5)
                {
                    return;
                }
            }
            else if (span.TotalSeconds < 1)
            {
                return;
            }

            checkAlarmTime = DateTime.Now;
            foreach (AxisBean axisInfo in axisArray)
            {
                if (AxisIsAlarm(axisInfo))
                {
                    return;
                }
            }
        }


        public bool OpenAllAxis(bool isCheck = true)
        {
            foreach (AxisBean axis in axisArray)
            {
                string outMsg = "";
                if (!axis.Open(true, out outMsg))
                {
                    LogUtil.info(outMsg);
                    return false;
                }
            }
            return true;
        }

        public void CloseAllAxis()
        {
            foreach (AxisBean axis in axisArray)
            {
                axis.SuddenStop();
                axis.ServoOff();
            }
        }


        #endregion

        #region NG箱
        bool preNgSig = false;
        bool curNgSig = false;
        AlarmType preAlram = AlarmType.None;
        void NGBoxProcess()
        {
            preNgSig = curNgSig;
            curNgSig = IOValue(IO_Type.NGBox_TrayCheck).Equals(IO_VALUE.HIGH);
            if (curNgSig == true && preNgSig == false)
            {
                //BufferDataManager.AddTrayInNGBox(new InOutPosInfo());
            }
            if (NGBoxAlarm())
            {
                SetWarnMsg($"NG箱中的料盘需要处理，数量大于{validityCnt}或者料盘放置时间大于{validityPeriod}分钟");
                if (alarmType <= AlarmType.AxisAlarm)
                {
                    preAlram = alarmType;
                    Alarm(AlarmType.NGBoxWarn);
                }
            }
            else
            {
                ClearSpecifiedAlarm($"NG箱中的料盘需要处理，数量大于");
                if (isInSuddenDown || isNoAirpressure_Check)
                {
                    return;
                }
                if (alarmType.Equals(AlarmType.NGBoxWarn))
                {
                    alarmType = preAlram;
                }
            }
        }
        #endregion
        #region 门禁光栅
        protected void SecurityAccessProcess()
        {
            try
            {
                if(!MoveStop)
                {
                    if (IOValue(IO_Type.SafetyLightGrids).Equals(IO_VALUE.LOW))
                    {
                        Pause();
                        LogUtil.info("安全光栅触发，停止行走机构和机器人运动");
                    }
                }


            }
            catch (Exception ex)
            {
                LogUtil.error(Name + " 出错 ：" + ex.ToString());
            }

        }
        public void Pause()
        {
            MoveStop = true;
            CompressAxis.SuddenStop();
            MoveAxis.SuddenStop();
            RobotPause();
            MoveInfo.CanWhileCount = 5;
        }
        public void Resume()
        {
            MoveStop = false;
            RobotResume();
        }
        #endregion


    }
}