using CodeLibrary;
using OnlineStore.Common;
using OnlineStore.LoadCSVLibrary;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms.VisualStyles;

namespace OnlineStore.DeviceLibrary
{
    partial class RobotEquip
    {
        #region 机器人相关命令
        /// <summary>
        /// 收到命令反馈
        /// </summary>
        public const string rtn_ok = "ok";
        /// <summary>
        /// 命令执行完成反馈
        /// </summary>
        const string rtn_done = "done";
        const string cmd_store = "store";
        const string cmd_inlet = "inlet";
        const string cmd_outlet = "outlet";
        const string cmd_ngbox = "ngbox";
        const string cmd_high = "high";
        const string cmd_low = "low";
        const string cmd_up = "up";
        const string cmd_down = "down";
        const string cmd_standby = "standby";
        const string cmd_home = "home";
        /// <summary>
        /// 获取指定命令
        /// </summary>
        /// <param name="place">地点</param>
        /// <param name="index">编号</param>
        /// <param name="point">点位</param>
        /// <returns></returns>
        public static string GetCmd(string place, string point, int index = 0)
        {
            StringBuilder sb = new StringBuilder();
            if (index == 0)
            {
                if (point.Equals(""))
                {
                    sb.Append(place);
                }
                else
                {
                    sb.Append(place);
                    sb.Append("-");
                    sb.Append(point);
                }

            }
            else
            {
                sb.Append(place);
                sb.Append(index);
                sb.Append("-");
                sb.Append(point);
            }
            return sb.ToString();
        }
        string cmdResponse = "";
        bool robotOnline = true;
        /// <summary>
        /// 机器人移动成功
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        public bool RobotMoveEnd(string cmd)
        {
            JAKA.JAKABean bean;
            if (!GetRobotInfo(out bean))
            {
                return false;
            }
            if (!bean.RobotData.Online)
            {
                SetWarnMsg("无法读取机器人状态，因机器人离线");
                Alarm(AlarmType.RobotError);
                robotOnline = false;
                return false;
            }
            else
            {
                ClearSpecifiedAlarm("无法读取机器人状态，因机器人离线");
            }
            try
            {
                if (robotOnline && bean.RobotData.RecvMsg.Equals("") && (DateTime.Now - sendTime).TotalSeconds > 10)
                {
                    SendCmd(curcmd);
                    LogUtil.info($"机器人命令10秒内未收到反馈，重发命令:【{curcmd}】");
                }
                if (!robotOnline && bean.RobotData.RecvMsg.Contains("home") && bean.RobotData.RecvMsg.Contains("done"))
                {
                    SendCmd(curcmd);
                    LogUtil.info($"重发机器人离线前的命令:【{curcmd}】");
                    robotOnline = true;
                }
                if (!cmdResponse.Equals(bean.RobotData.RecvMsg) && !bean.RobotData.RecvMsg.Equals(""))
                {
                    cmdResponse = bean.RobotData.RecvMsg;
                    if ((cmdResponse.Contains(GetCmd_InletUp()))
                        && cmdResponse.Contains("done"))
                    {
                        insafeAtInlet = true;
                    }
                    else//if ((cmdResponse.Contains(GetCmd_Standby()) && cmdResponse.Contains("done")))
                    {
                        insafeAtInlet = false;
                    }
                    if ((cmdResponse.Contains(GetCmd_OutletDown())) && cmdResponse.Contains("done"))
                    {
                        insafeAtOutlet = true;
                    }
                    else
                    {
                        insafeAtOutlet = false;
                    }
                    LogUtil.info($"RobotMove Cmd Response:【{cmdResponse}】");
                }
                string[] recv = bean.RobotData.RecvMsg.Split(',');
                if (GetCurCmd().Equals(cmd) && recv != null && recv.Length == 2
                    && recv[0].Equals(cmd) && recv[1].Equals(rtn_done)
                    && (bean.RobotData.RobotStatus.inpos == 1))
                    return true;
            }
            catch (Exception ex)
            {
                LogUtil.error($"RobotMoveEnd:【{cmd}】", ex);
                return false;
            }
            return false;
        }
        private bool insafeAtInlet = false;
        private bool insafeAtOutlet = false;
        public bool RobotInSafeAtInlet()
        {
            return insafeAtInlet;
        }
        public bool RobotInSafeAtOutlet()
        {
            return insafeAtOutlet;
        }
        public string curcmd = "";
        DateTime sendTime = DateTime.Now;
        /// <summary>
        /// 发送命令
        /// </summary>
        /// <param name="cmd"></param>
        void SendCmd(string cmd)
        {
            curcmd = cmd;
            MoveInfo.WaitList.Add(WaitResultInfo.WaitRobotMove(cmd));
            JAKA.JAKABean bean;
            if (!GetRobotInfo(out bean))
            {
                return;
            }
            if (!bean.RobotData.Online)
            {
                SetWarnMsg($"无法发送命令，因机器人离线:{Config.Robot_IP}");
                return;
            }
            else
            {
                ClearSpecifiedAlarm("无法发送命令，因机器人离线");
            }
            JAKAServer.SendCmd(bean.RemoteEndPoint, cmd);
            sendTime = DateTime.Now;
            LogUtil.info($"Send RobotMove Cmd:【{cmd}】");
        }
        /// <summary>
        /// 获取机器人当前命令
        /// </summary>
        /// <returns></returns>
        string GetCurCmd()
        {
            JAKA.JAKABean bean;
            if (!GetRobotInfo(out bean))
            {
                return "";
            }
            return bean.RobotData.CurCmd;
        }
        /// <summary>
        /// 机器人是否在线
        /// </summary>
        /// <returns></returns>
        public bool RobotIsOnline()
        {
            JAKA.JAKABean bean;
            if (!GetRobotInfo(out bean))
            {
                return false;
            }
            if (!bean.RobotData.Online)
            {
                return false;
            }
            return true;
        }
        #endregion
        #region 机器人命令
        public static string GetCmd_InletLow(int width)
        {
            return GetCmd(cmd_inlet, cmd_low, width);
        }
        public static string GetCmd_InletUp()
        {
            return GetCmd(cmd_inlet, cmd_up);
        }
        public static string GetCmd_OutletHigh()
        {
            return GetCmd(cmd_outlet, cmd_high);
        }
        public static string GetCmd_OutletDown()
        {
            return GetCmd(cmd_outlet, cmd_down);
        }
        public static string GetCmd_NGBoxHigh()
        {
            return GetCmd(cmd_ngbox, cmd_high);
        }
        public static string GetCmd_NGBoxDown()
        {
            return GetCmd(cmd_ngbox, cmd_down);
        }
        public static string GetCmd_StoreHigh(int index)
        {
            return GetCmd(cmd_store, cmd_high, index);
        }
        public static string GetCmd_StoreLow(int index)
        {
            return GetCmd(cmd_store, cmd_low, index);
        }
        public static string GetCmd_StoreUp()
        {
            return GetCmd(cmd_store, cmd_up);
        }
        public static string GetCmd_StoreDown()
        {
            return GetCmd(cmd_store, cmd_down);
        }
        public static string GetCmd_Standby()
        {
            return GetCmd(cmd_standby, "");
        }
        public static string GetCmd_Home()
        {
            return GetCmd(cmd_home, "");
        }
        #endregion
        #region 机器人
        public bool GetRobotInfo(out JAKA.JAKABean bean)
        {
            bean = JAKAServer.GetJAKABean(Config.Robot_IP);
            if (bean == null)
            {
                SetWarnMsg($"无法获取机器人信息，因不存在的机器人:{Config.Robot_IP}");
                return false;
            }
            return true;
        }
        /// <summary>
        /// 上电
        /// </summary>
        /// <param name="on">true:开启</param>
        public void PowerOn(bool on)
        {
            if (GetRobotInfo(out JAKA.JAKABean bean))
            {
                if (on)
                {
                    if (bean.RobotData.RobotStatus.powered_on == 0)
                        bean.PowerOn();
                }
                else
                {
                    if (bean.RobotData.RobotStatus.powered_on == 1)
                        bean.PowerOff();
                }
            }
        }
        /// <summary>
        /// 上使能
        /// </summary>
        /// <param name="enable">true:开启</param>
        public void EnableRobot(bool enable)
        {
            if (GetRobotInfo(out JAKA.JAKABean bean))
            {
                if (enable)
                {
                    if (bean.RobotData.RobotStatus.enabled == 0)
                        bean.EnableRobot();
                }
                else
                {
                    if (bean.RobotData.RobotStatus.enabled == 1)
                        bean.DisableRobot();
                }

            }

        }
        /// <summary>
        /// 启动程序
        /// </summary>
        /// <param name="start"></param>
        public void StartProg(bool start)
        {
            if (GetRobotInfo(out JAKA.JAKABean bean))
            {
                if (start)
                {
                    bean.Run();
                }

                else
                    bean.Abort();
            }
        }
        /// <summary>
        /// 机器人运动中止
        /// </summary>
        public void MotionAbort()
        {
            if (GetRobotInfo(out JAKA.JAKABean bean))
            {
                bean.MotionAbort();
            }
        }
        #endregion

        #region 行走机构
        int targetIndex = 0;
        private void MoveAxisToStore()
        {
            switch (MoveInfo.MoveParam.PosInfo.Cid)
            {
                case "T01-1":
                    targetIndex = 1;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P5_Store1, Config.MoveAxis_P5_Store1_Speed);
                    break;
                case "T01-2":
                    targetIndex = 2;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P6_Store2, Config.MoveAxis_P6_Store2_Speed);
                    break;
                case "T01-3":
                    targetIndex = 3;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P7_Store3, Config.MoveAxis_P7_Store3_Speed);
                    break;
                case "T01-4":
                    targetIndex = 4;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P8_Store4, Config.MoveAxis_P8_Store4_Speed);
                    break;
                case "T01-5":
                    targetIndex = 5;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P9_Store5, Config.MoveAxis_P9_Store5_Speed);
                    break;
                case "T01-6":
                    targetIndex = 6;
                    MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P10_Store6, Config.MoveAxis_P10_Store6_Speed);
                    break;

            }
        }
        #endregion
        protected override bool CheckWaitResult(DeviceMoveInfo moveInfo, WaitResultInfo wait)
        {
            if (wait.WaitType.Equals(WaitEnum.W201_RobotMove))
            {
                if (RobotMoveEnd(curcmd))
                {
                    wait.IsEnd = true;
                }
            }
            return wait.IsEnd;
        }

        /// <summary>
        /// 设置料仓状态
        /// </summary>
        /// <param name="deviceStatus">设备上报服务端的状态</param>
        /// <param name="runStatus">设备显示的状态</param>
        private void SetBoxStatus(DeviceStatus deviceStatus, RunStatus runStatus, InOutPosInfo posInfo = null)
        {
            if (posInfo != null)
            {
                lastPosInfo = posInfo;
            }

            this.deviceStatus = deviceStatus;
            this.runStatus = runStatus;
        }

        #region 入库
        private DateTime startInStoreTime = DateTime.Now;
        public override bool StartInstore(InOutParam param)
        {
            if (isInSuddenDown || isNoAirpressure_Check ||
           (!runStatus.Equals(RunStatus.Runing))
           || (!MoveInfo.MoveType.Equals(MoveType.None)))
            {
                LogUtil.error(Name + " 启动入库出错，忙碌或报警中 ，storeStatus=" + runStatus + ",MoveType=" + MoveInfo.MoveType + ",isInSuddenDown=" + isInSuddenDown + ",isNoAirpressure_Check=" + isNoAirpressure_Check);
                return false;
            }

            startInStoreTime = DateTime.Now;
            LogInfo(" 启动入库【" + param.PosInfo.ToStr() + "】 ");
            param.MoveP = new LineMoveP(Config, param.PosInfo.PosId);
            //LogInfo("LoadInoutParam:" + JsonHelper.SerializeObject(param.MoveP));
            MoveInfo.NewMove(MoveType.InStore, param);
            SetBoxStatus(DeviceStatus.InStoreExecute, RunStatus.Busy, param.PosInfo);
            MoveInfo.NextMoveStep(StepEnum.InStore_01_Wait);
            return true;
        }

        protected override void InstoreProcess()
        {
            if (MoveInfo.IsInWait)
            {
                CheckWait(MoveInfo);
            }
            if (MoveInfo.IsInWait)
            {
                return;
            }
            InstoreExecute();
        }
        #endregion

        #region 出库
        private static object outStoreObject = new object();
        public void StartExecuctOut(InOutParam param)
        {
            bool result = false;
            result = StartOutstore(param);
            if (!result)
            {
                lock (outStoreObject)
                {
                    if (MoveInfo.MoveType.Equals(MoveType.OutStore) && MoveInfo.MoveParam.PosInfo.Cid.Equals(param.PosInfo.Cid))
                    {
                        LogUtil.error(Name + " 出库命令【" + param.PosInfo.ToStr() + "】重复，【" + MoveInfo.MoveParam.PosInfo.Cid + "】出库执行中");
                        return;
                    }

                    //List<InOutParam> reviceList = new List<InOutParam>();
                    //reviceList.AddRange(waitOutStoreList);
                    //reviceList = (from m in reviceList where m.PosInfo.Cid.Equals(param.PosInfo.Cid) select m).ToList<InOutParam>();
                    //if (reviceList.Count == 0)
                    //{
                    //    LogInfo(" 执行出库【" + param.PosInfo.ToStr() + "】失败，加入等待队列");
                    //    waitOutStoreList.Enqueue(param);
                    //}
                }
            }
        }
        private DateTime startOutStoreTime = DateTime.Now;
        /// <summary>
        /// 关闭出库前的碗里检查
        /// </summary>
        public bool CloseOutStoreCheck = false;
        string[] existCids = ConfigAppSettings.GetValue(Setting_Init.Line_CID).Split(',');
        public override bool StartOutstore(InOutParam param)
        {
            if (!runStatus.Equals(RunStatus.Runing) || !MoveInfo.MoveType.Equals(MoveType.None))
                return false;
            if (isInSuddenDown || isNoAirpressure_Check
                || !runStatus.Equals(RunStatus.Runing)
                || !MoveInfo.MoveType.Equals(MoveType.None))
            {
                SetWarnMsg(Name + " 启动出库【" + param.PosInfo.ToStr() + "】失败，忙碌或报警中 ，storeStatus：" + runStatus + ",MoveType：" + MoveInfo.MoveType + ",isInSuddenDown：" + isInSuddenDown + ",isNoAirCheck：" + isNoAirpressure_Check);
                return false;
            }

            if (!existCids.Contains(param.PosInfo.Cid))
            {
                SetWarnMsg(Name + " 启动出库【" + param.PosInfo.ToStr() + $"】出错，找不到料仓信息【{param.PosInfo.Cid}】");
                return false;
            }
            startOutStoreTime = DateTime.Now;
            param.MoveP = new LineMoveP(Config, param.PosInfo.PosId);
            SetBoxStatus(DeviceStatus.OutStoreExecute, RunStatus.Busy, param.PosInfo);
            MoveInfo.NewMove(MoveType.OutStore, param);
            LogInfo("启动出库【" + param.PosInfo.ToStr() + "】  ");
            //LogInfo("LoadInoutParam:" + JsonHelper.SerializeObject(param.MoveP));
            MoveInfo.NextMoveStep(StepEnum.OutStore_01_Wait);

            return true;
        }

        protected override void OutstoreProcess()
        {
            if (MoveInfo.IsInWait)
            {
                CheckWait(MoveInfo);
            }
            if (MoveInfo.IsInWait)
            {
                return;
            }
            OutstoreExecute();
        }
        #endregion
    }
}