using Aitex.Core.Common;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using athosRT.Devices;
using athosRT.FSM;
using athosRT.Modules.EFEMs.Tasks;
using athosRT.tool;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static athosRT.Modules.EFEMs.Routine.PickRoutine;
using static MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase.RobotBaseDevice;
using EV = athosRT.tool.EV;

namespace athosRT.Modules.EFEMs.Routine
{

    public class PickExtendRoutine : ModuleRoutineBase, FSM.IRoutine
    {
        protected bool MultiWaferSize = Singleton<DeviceDefineManager>.Instance.GetValue<bool>(nameof(MultiWaferSize)).GetValueOrDefault();
        private SCConfigItem _scPickTimeout = (SCConfigItem)null;
        private int _timeout = 0;
        private int _axisTimeout = 0;
        private int _axisSpeed = 0;
        public double WaitForPickTime = 0.0;
        private LoadPort _lpDevice;
        protected bool NeedRobotGripAndUngrip = SC.GetValue<bool>("System.EnableRobotGripAndUngrip");
        protected readonly bool HaveMotionAxis = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("MotionAxisInstalled").GetValueOrDefault();
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        public ModuleName Source { get; set; }

        public int Slot { get; set; }

        public Hand Blade { get; set; }
        public MpntTaskEnum TaskEnum { get; set; }
        public RobotBaseDevice robot;
        protected Aligner aligner = null;




        public PickExtendRoutine(ModuleName module) : base(module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            aligner = DEVICE.GetDevice<Aligner>("Aligner");
            _scPickTimeout = SC.GetConfigItem("Robot.TimeLimitForPickWafer");
            if (HaveMotionAxis)
            {
                _axisTimeout = SC.GetValue<int>("MotionAxis.MoveTimeout");
                _axisSpeed = SC.GetValue<int>("MotionAxis.AutoSpeed");
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            _scPickTimeout = SC.GetConfigItem("Robot.TimeLimitForPickWafer");
            if (HaveMotionAxis)
            {
                _axisTimeout = SC.GetValue<int>("MotionAxis.MoveTimeout");
                _axisSpeed = SC.GetValue<int>("MotionAxis.AutoSpeed");
            }
            _timeout = _scPickTimeout.IntValue * 1000;
            if (!ModuleHelper.IsLoadLock(Source) || TaskEnum == MpntTaskEnum.ParamNg)
            {
                return RState.Failed;
            }
            else
            {
                return Runner.Start(ModuleName.Robot, "Robot");
            }
        }
        /// <summary>
        /// 注意 此处会按照不同的task类型进行任务的下发
        /// G1 GB G4
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            switch (TaskEnum)
            {
                case MpntTaskEnum.G1:
                    Runner
                        .Run(PickExtendStep.CheckBeforePick,NullFun, CheckBeforePick, _timeout)
                        .End(PickExtendStep.RobotGoto, RobotGoto, WaitRobotMotion, _timeout)
                        ;
                    break;
                case MpntTaskEnum.GB:
                    Runner
                        .Run(PickExtendStep.CheckBladeWaferIsExist, CheckBladeWaferIsExist, WaitRobotReady, _timeout)
                        .Run(PickExtendStep.CheckBeforePick,NullFun ,CheckBeforePick, _timeout)
                        .End(PickExtendStep.RobotGoto, RobotGoto, WaitRobotReady, _timeout)
                        ;
                    break;
                case MpntTaskEnum.G4:
                    Runner
                        .Run(PickExtendStep.GripRobotBlade, GripRobotBlade, CheckRobotNoBusy, _timeout)
                        .Run(PickExtendStep.RobotGoto, RobotGotoPickRetracted, WaitRobotMotion,_timeout)
                        .Run(PickExtendStep.CheckBladeWaferIsExist, CheckBladeWaferIsExist, WaitRobotReady, _timeout)
                        .End(PickExtendStep.CheckAfterPick, NullFun,CheckAfterPick, _timeout)
                        ;
                    break;
                default:
                    return RState.Failed;
            }
            return Runner.Status;
        }

        private bool CheckRobotNoBusy()
        {
            if (!robot.IsBusy)
            {
                LogObject.Info("robot", "is not busy");
                return true;
            }
            else
            {
                //LogObject.Warning("robot", "is busy");
                return false;
            }
        }

        private bool WaitRobotReady()
        {
            if (robot.IsReady())
            {
                LogObject.Info("robot", "is ready");
                return true;
            }
            else
            {
                //LogObject.Error("robot", "not ready");
                return false;
            }
        }

        /// <summary>
        /// 将Robot的伸展到指定槽的位置
        /// </summary>
        /// <returns></returns>
        private bool RobotGoto()
        {
            if (robot.GoTo(new object[4] {
            (RobotArmEnum)Blade,
            Source.ToString(),
            Slot,
            RobotPostionEnum.PickExtend}))
            {
                LogObject.Info("Robot", "PickExtend has Goto");
                return true;
            }
            else
            {
                LogObject.Error("Robot", "PickExtend Cannot Goto");
                return false;
            }
        }

        private bool RobotGotoPickRetracted()
        {
            if (robot.GoTo(new object[4] {
            (RobotArmEnum)Blade,
            Source.ToString(),
            Slot,
            RobotPostionEnum.PickRetracted}))
            {
                LogObject.Info("Robot", "PickExtend has Goto");
                return true;
            }
            else
            {
                LogObject.Error("Robot", "PickExtend Cannot Goto");
                return false;
            }
        }

        private bool CheckBladeWaferIsExist()
        {
            //this.robot, this.Blade, this._timeout
            if (!robot.ReadParameter(new object[3] { "CheckWaferIsPresence", (Blade == Hand.Blade1 ? 0 : 1), _existInterval }))
            {
                //手臂上有wafer 无法pick
                return false;
            }
            return true;
        }

        private bool CheckAfterPick()
        {
            switch (Blade)
            {
                case Hand.Blade1:
                    if (!CheckSeneorHasWafer(ModuleName.Robot, (int)Blade))
                    {
                        LogObject.Error("Robot",$"{Blade} not Has Wafer");
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                        return false;
                    }
                    break;
                case Hand.Blade2:
                    for (int index = 0; index < robot.Blade2Slots; ++index)
                    {
                        if (!CheckSeneorHasWafer(ModuleName.Robot, 1 + index))
                        {
                            LogObject.Error("Robot", $"{Blade} {index} not Has Wafer");
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                            return false;
                        }
                    }
                    break;
                default:
                    for (int slot1 = 0; slot1 < robot.Blade2Slots + 1; ++slot1)
                    {
                        if (!CheckSeneorHasWafer(ModuleName.Robot, slot1))
                        {
                            LogObject.Error("Robot", $"{Blade} {slot1} not Has Wafer");
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                            return false;
                        }
                    }
                    break;
            }
            return true;
        }

        private bool WaitRobotMotion()
        {
            if (robot.IsReady())
            {
                LogObject.Info("robot","is ready");
                return true;
            }
            else
            {
                //LogObject.Error("robot","not ready");
                return false;
            }
        }

        private bool GripRobotBlade()
        {
            //this.Blade, this.robot, this._timeout
            if (NeedRobotGripAndUngrip)
            {
                if (robot.CheckToPostMessage(RobotMsg.Grip, (RobotArmEnum)Blade, Source))
                {
                    return true;
                }
                return false;
            }
            return true;
        }

        private bool CheckBeforePick()
        {
            //"Check wafer information", this.Source, this.Slot, this.Blade
            if (!CheckRobotMotionInterlock(Source, Slot, out _))
            {
                //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.TransferPrepareFailed, (object)string.Format("target {0}{1:D2},{2}", (object)chamber.ToString(), (object)(slot + 1), (object)reason));
                return false;
            }
            switch (Blade)
            {
                case Hand.Blade1:
                    //为了让efemUI上显示wafer
                    if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                        Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                    else if (!Singleton<WaferManager>.Instance.CheckWafer(Source, Slot, WaferStatus.Normal))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, (object)string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)(Slot + 1)));
                        return false;
                    }
                    if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, 0, WaferStatus.Empty))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                        return false;
                    }
                    if (!CheckSensorNoWafer(ModuleName.Robot, 0))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                        return false;
                    }
                    if (ModuleHelper.IsBuffer(Source) && !CheckSeneorHasWafer(Source, Slot))
                    {
                        EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                        return false;
                    }
                    break;
                case Hand.Blade2:
                    bool flag1 = true;
                    for (int index = 0; index < robot.Blade2Slots; ++index)
                    {
                        if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                            Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                        else
                            flag1 = Singleton<WaferManager>.Instance.CheckWafer(Source, Slot + index, WaferStatus.Normal);
                        if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, (int)(Blade + index), WaferStatus.Empty))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (!CheckSensorNoWafer(ModuleName.Robot, (int)(Blade + index)))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (ModuleHelper.IsBuffer(Source))
                        {
                            flag1 = CheckSeneorHasWafer(Source, Slot + index);
                            if (!flag1)
                            {
                                EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                                return false;
                            }
                        }
                    }
                    if (!flag1)
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)(Slot + 1)));
                        return false;
                    }
                    break;
                default:
                    bool flag2 = true;
                    for (int slot1 = 0; slot1 < robot.Blade2Slots + 1; ++slot1)
                    {
                        if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                            Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                        else
                            flag2 = Singleton<WaferManager>.Instance.CheckWafer(Source, Slot, WaferStatus.Normal);
                        if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, slot1, WaferStatus.Empty))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (!CheckSensorNoWafer(ModuleName.Robot, slot1))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (ModuleHelper.IsBuffer(Source) && Blade == Hand.Both)
                        {
                            flag2 &= this.CheckSeneorHasWafer(Source, Slot + slot1);
                            if (!flag2)
                            {
                                EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                                return false;
                            }
                        }
                    }
                    if (!flag2)
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)Slot));
                        return false;
                    }
                    break;
            }
            if (!ModuleHelper.IsCoolingBuffer(Source) || DEVICE.GetDevice<IoCoolingBuffer>(Source.ToString()).CheckPinUp())
                return true;
            EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, buffer isn't pick position.");
            return false;
        }

        public void Abort()
        {
            
        }

        protected bool CheckRobotMotionInterlock(ModuleName chamber, int slot, out string reason)
        {
            reason = string.Empty;
            if (robot.RobotState == RobotStateEnum.Error)
            {
                reason = "robot is error.";
                return false;
            }
            if (!robot.IsReady())
            {
                reason = string.Format("robot isn't Ready.");
                return false;
            }
            if (chamber == ModuleName.Aligner)
            {
                if (aligner.Moving)
                {
                    reason = string.Format("aligner is moving.");
                    return false;
                }
                if (robot.IsReady())
                    return true;
                reason = string.Format("aligner isn't Ready.");
                return false;
            }
            if (ModuleHelper.IsLoadPort(chamber))
            {
                LoadPortBaseDevice device = DEVICE.GetDevice<LoadPortBaseDevice>(chamber.ToString());
                if (!device.IsEnableTransferWafer(out reason))
                {
                    reason = string.Format($"{chamber} isn't Ready.The reason is {reason}");
                    return false;
                }
                if (device.MapError)
                {
                    reason = string.Format("{0} Map has crossed error.", (object)chamber.ToString());
                    return false;
                }
                if (chamber.Equals(ModuleName.LP1))
                {
                    if (!DeviceModel.SensorSMIF1PODOPEN.Value)
                    {
                        reason = string.Format("{0} SMIF1PODOPEN signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                    if (!DeviceModel.SensorSMIF1PODPRESENT.Value)
                    {
                        reason = string.Format("{0} SensorSMIF1PODPRESENT signal is Off,Foup is presence.", (object)chamber.ToString());
                        return false;
                    }
                    if (!DeviceModel.SensorSMIF1READY.Value)
                    {
                        reason = string.Format("{0} SensorSMIF1READY signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                }
                else if (chamber.Equals(ModuleName.LP2))
                {
                    if (!DeviceModel.SensorSMIF2PODOPEN.Value)
                    {
                        reason = string.Format("{0} SMIF2PODOPEN signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                    if (!DeviceModel.SensorSMIF2PODPRESENT.Value)
                    {
                        reason = string.Format("{0} SensorSMIF2PODPRESENT signal is Off,Foup is not presence .", (object)chamber.ToString());
                        return false;
                    }
                    if (!DeviceModel.SensorSMIF2READY.Value)
                    {
                        reason = string.Format("{0} SensorSMIF2READY signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                }
                return true;
            }
            if (ModuleHelper.IsLoadLock(chamber))
                return DEVICE.GetDevice<LoadLockDevice>(chamber.ToString()).IsEnableTransferWafer(out reason);
            if (ModuleHelper.IsCoolingBuffer(chamber))
            {
                IoCoolingBuffer device = DEVICE.GetDevice<IoCoolingBuffer>(chamber.ToString());
                if (device.Busy)
                {
                    reason = "buffer is not Idle";
                    return false;
                }
                if (device.CheckPinUp())
                    return true;
                reason = "buffer pin not up position";
                return false;
            }
            if (ModuleHelper.IsAligner(chamber))
            {
                Aligner device = DEVICE.GetDevice<Aligner>(chamber.ToString());
                if (device.Busy)
                {
                    reason = "Aligner is not Idle";
                    return false;
                }
                return true;
            }
            reason = "error target";
            return false;
        }
        protected bool CheckSensorNoWafer(ModuleName chamber, int slot)
        {
            if (SC.GetValue<bool>("System.IsSimulatorMode") || chamber != ModuleName.Robot)
                return Singleton<WaferManager>.Instance.CheckNoWafer(chamber, slot);
            return slot == 0 ? !this.robot.IsWaferPresenceOnBlade1 : !this.robot.IsWaferPresenceOnBlade2;
        }
        protected bool CheckSeneorHasWafer(ModuleName chamber, int slot)
        {
            if (SC.GetValue<bool>("System.IsSimulatorMode") || chamber != ModuleName.Robot)
                return Singleton<WaferManager>.Instance.CheckHasWafer(chamber, slot);
            return slot == 0 ? this.robot.IsWaferPresenceOnBlade1 : this.robot.IsWaferPresenceOnBlade2;
        }
        public enum PickExtendStep {
            CheckBladeWaferIsExist,
            CheckBeforePick,
            RobotGoto,
            WaitRobotMotion,
            GripRobotBlade,
            CheckAfterPick,
            WaitRobotReady,
            CheckRobotNoBusy
        }
    }
}