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.Devices.PA;
using athosRT.FSM;
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.Numerics;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Linq;
using static athosRT.Modules.FLP.FlipperEntity;
using EV = athosRT.tool.EV;

namespace athosRT.Modules.EFEMs.Routine
{
    public class SwapRoutine : ModuleRoutineBase, FSM.IRoutine
    {
        private SCConfigItem _scSwapTimeout = (SCConfigItem)null;
        private int _timeout = 0;
        public LoadPort _lpDevice;
        private int _offsetXPick;
        private int _offsetYPick;
        private int _offsetZPick;
        private int _offsetXPlace;
        private int _offsetYPlace;
        private int _offsetZPLace;
        private Hand _pickHand;
        private int _alignerdelay = 0;
        private int _coolingdelay = 0;
        private int delaytime = 0;
        protected int OffsetX;
        protected int OffsetY;
        protected int OffsetZ;

        public ModuleName Source { get; set; }

        public int Slot { get; set; }

        public Hand PlaceBlade { get; set; }

        RobotBaseDevice robot;
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        protected Aligner aligner = null;
        protected IoCoolingBuffer buffer1 = null;
        protected IoCoolingBuffer buffer2 = null;
        protected PreAligner aligner1 = null;
        protected PreAligner aligner2 = null;
        protected LoadLockDevice ll1 = null;
        protected LoadLockDevice ll2 = null;
        protected IoCoolingBuffer _ioCoolBuffer;

        private bool _IsFlipper = false;

        public SwapRoutine(ModuleName module) : base(module)
        {
            Module = module;
            Name = "Swap Routine";
            _scSwapTimeout = SC.GetConfigItem("Robot.TimeLimitForPlaceWafer");
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            aligner = DEVICE.GetDevice<Aligner>("Aligner");
            buffer1 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer1");
            buffer2 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer2");
            aligner1 = DEVICE.GetDevice<PreAligner>("Aligner1");
            aligner2 = DEVICE.GetDevice<PreAligner>("Aligner2");
        }


        public RState Monitor()
        {
            if (_IsFlipper)
                Runner
                    .Wait(SwapStep.WaitForSwapIdle, CheckFlipperIsIdle, _timeout)
                    .Run(SwapStep.NotifyFlipperPrepare, NotifyFlipperOpen, CheckFlipperOpen, _timeout)
                    .Run(SwapStep.CheckBlade1WaferIsExist, fCheckBlade1WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBlade2WaferIsExist, fCheckBlade2WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBeforeSwap, NullFun, fCheckBeforeSwap, _timeout)
                    //.Run(SwapStep.SwapWafer, fSwapWafer, fWaitRobotMotion, _timeout)
                    .Run(SwapStep.PickWafer, fPickWafer, fWaitRobotMotion, _timeout)
                    .Run(SwapStep.PlaceWafer, fPlaceWafer, fWaitRobotMotion, _timeout)
                    .Delay(SwapStep.TimeDelay, delaytime)
                    .Run(SwapStep.NotifyFlipperOver, NotifyFlipperClose, CheckFlipperIsIdle, _timeout)
                    .End(SwapStep.CheckAfterSwap, NullFun, fCheckAfterSwap, _timeout);
            else 
                if(ModuleHelper.IsBuffer(Source))
                Runner
                    .Run(SwapStep.CoolBufferMoveUP, fCoolBufferMoveUP, fWaitCoolBufferMoveUp, _timeout)
                    .Run(SwapStep.CheckBlade1WaferIsExist,fCheckBlade1WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBlade2WaferIsExist,fCheckBlade2WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBeforeSwap,NullFun,fCheckBeforeSwap,_timeout)
                    //.Run(SwapStep.SwapWafer,fSwapWafer, fWaitRobotMotion, _timeout)
                    .Run(SwapStep.PickWafer,fPickWafer, fWaitRobotMotion, _timeout)
                    .Run(SwapStep.PlaceWafer,fPlaceWafer, fWaitRobotMotion, _timeout)
                    .Delay(SwapStep.TimeDelay, delaytime)
                    .Run(SwapStep.CoolBufferMoveDown,fCoolBufferMoveDown, fWaitCoolBufferMoveDown, _timeout)
                    .End(SwapStep.CheckAfterSwap,NullFun,fCheckAfterSwap,_timeout);
            else
                Runner
                    .Run(SwapStep.CoolBufferMoveUP, fCoolBufferMoveUP, fWaitCoolBufferMoveUp, _timeout)
                    .Run(SwapStep.CheckBlade1WaferIsExist, fCheckBlade1WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBlade2WaferIsExist, fCheckBlade2WaferIsExist, fRobotReady, _timeout)
                    .Run(SwapStep.CheckBeforeSwap, NullFun, fCheckBeforeSwap, _timeout)
                    .Run(SwapStep.SwapWafer, fSwapWafer, fWaitRobotMotion, _timeout)
                    //.Run(SwapStep.PickWafer,fPickWafer, fWaitRobotMotion, _timeout)
                    //.Run(SwapStep.PlaceWafer,fPlaceWafer, fWaitRobotMotion, _timeout)
                    .Delay(SwapStep.TimeDelay, delaytime)
                    .Run(SwapStep.CoolBufferMoveDown, fCoolBufferMoveDown, fWaitCoolBufferMoveDown, _timeout)
                    .End(SwapStep.CheckAfterSwap, NullFun, fCheckAfterSwap, _timeout);
            return Runner.Status;
        }

        private bool fWaitCoolBufferMoveDown()
        {
            if (ModuleHelper.IsCoolingBuffer(this.Source))
            {
                IoCoolingBuffer device = GetCoolBuffer(Source);
                if (!device.Error)
                    return device.CheckMovedDown() && !device.Busy;
                return false;
            }
            if (ModuleHelper.IsAligner(Source))
            {
                //PreAligner device = DEVICE.GetDevice<PreAligner>(Source.ToString());
                //if (device.Busy)
                //{
                //    return false;
                //}
                return true;
            }
            return true;
        }

        private bool fCheckAfterSwap()
        {
            if (PlaceBlade == Hand.Blade1)
            {
                if (!this.CheckSensorNoWafer(ModuleName.Robot, 0))
                {
                    //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedAfterSend);
                    return false;
                }
                if (!this.CheckSeneorHasWafer(ModuleName.Robot, 1))
                {
                    EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                    return false;
                }
            }
            else
            {
                if (!this.CheckSensorNoWafer(ModuleName.Robot, 1))
                {
                    return false;
                }
                if (!this.CheckSeneorHasWafer(ModuleName.Robot, 0))
                {
                    return false;
                }
            }
            return true;
        }

        private bool fCoolBufferMoveDown()
        {
            if (ModuleHelper.IsCoolingBuffer(this.Source))
            {
                IoCoolingBuffer device = GetCoolBuffer(Source);
                if (device.CheckPinDown() || device.Move(Singleton<WaferManager>.Instance.GetWaferSize(this.Source, this.Slot), false, out _))
                    return true;
                return false;
            }
            if (ModuleHelper.IsAligner(Source))
            {
                //PreAligner device = DEVICE.GetDevice<PreAligner>(Source.ToString());
                //if (device.Busy)
                //{
                //    return false;
                //}
                return true;
            }
            return true;
        }

        private bool fPlaceWafer()
        {
            if (!ModuleHelper.IsLoadLock(Source))
            {
                if (!ModuleHelper.IsCoolingBuffer(Source))
                {
                    if (_offsetXPlace == 0 && _offsetYPlace == 0 && _offsetZPLace == 0 ? robot.Place((RobotArmEnum)PlaceBlade, Source.ToString(), Slot) : robot.PlaceEx((RobotArmEnum)_pickHand, Source.ToString(), Slot, _offsetXPlace, _offsetYPlace, _offsetZPLace))
                    {
                        LogObject.Info("System", "Swap place成功");
                        return true;
                    }
                    else
                    {
                        LogObject.Error("System", "Swap place失败");
                        return false;
                    }
                }
            }
            return true;
        }

        private bool fWaitRobotMotion()
        {
            if (robot.IsError)
            {
                LogObject.Info("Robot", "Robot进入Error");
                return false;
            }
            return robot.IsReady();
        }

        private bool fPickWafer()
        {
            if (!ModuleHelper.IsLoadLock(Source))
            {
                if (!ModuleHelper.IsCoolingBuffer(Source))
                {
                    if (_offsetXPick == 0 && _offsetYPick == 0 && _offsetZPick == 0 ? robot.Pick((RobotArmEnum)_pickHand, Source.ToString(), Slot) : robot.PickEx((RobotArmEnum)_pickHand, Source.ToString(), Slot, _offsetXPick, _offsetYPick, _offsetZPick))
                    {
                        LogObject.Info("System", "Swap pick成功");
                        return true;
                    }
                    else
                    {
                        LogObject.Error("System", "Swap pick失败");
                        return false;
                    }
                }
            }
            return true;
        }

        private bool fSwapWafer()
        {
            if (!ModuleHelper.IsLoadLock(Source))
            {
                if (ModuleHelper.IsCoolingBuffer(Source) || ModuleHelper.IsAligner(Source)||ModuleHelper.IsTurnOverStation(Source))
                {
                    if (OffsetX == 0 && OffsetY == 0 && OffsetZ == 0 ? robot.Swap((RobotArmEnum)_pickHand, Source.ToString(), Slot) : robot.SwapEx((RobotArmEnum)_pickHand, Source.ToString(), Slot, OffsetX, OffsetY, OffsetZ))
                    {
                        LogObject.Info("System", "Swap成功");
                        return true;
                    }
                    else
                    {
                        LogObject.Error("System", "Swap失败");
                        return false;
                    }
                }
            }
            else
            {
                if (OffsetX == 0 && OffsetY == 0 && OffsetZ == 0 ? robot.Swap((RobotArmEnum)_pickHand, Source.ToString(), Slot) : robot.SwapEx((RobotArmEnum)_pickHand, Source.ToString(), Slot, OffsetX, OffsetY, OffsetZ))
                {
                    LogObject.Info("System", "Swap成功");
                    return true;
                }
                else
                {
                    LogObject.Error("System", "Swap失败");
                    return false;
                }
            }
            return true;
                    //this.Source, this.Slot, this._pickHand, this.OffsetX, this.OffsetY, this.OffsetZ
            
        }

        private bool fCheckBeforeSwap()
        {
            //this.Source, this.Slot, this._pickHand
            if (!CheckRobotMotionInterlock(Source, Slot, out _))
            {
                return false;
            }
            if (_pickHand == Hand.Blade1)
            {
                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.Normal))
                {
                    //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedBeforeSend);
                    return false;
                }
                if (CheckSeneorHasWafer(ModuleName.Robot, 0))
                {
                    //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedBeforeSend);
                    return false;
                }
                if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, 1, WaferStatus.Normal))
                {
                    //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                    return false;
                }
                if (CheckSensorNoWafer(ModuleName.Robot, 1))
                {
                    //EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                    return false;
                }
            }
            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, 1, WaferStatus.Normal))
                {
                    return false;
                }
                if (this.CheckSeneorHasWafer(ModuleName.Robot, 1))
                {
                    return false;
                }
                if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, 0, WaferStatus.Normal))
                {
                    return false;
                }
                if (this.CheckSensorNoWafer(ModuleName.Robot, 0))
                {
                    return false;
                }
            }
            return true;
        }

        private bool fRobotReady() {
            if (robot.IsReady())
            {
                LogObject.Info("robot", "RobotReady");
                return true;
            }
            else
            {
                //LogObject.Error("robot", "RobotReady");
                return false;
            }
        }

        private bool fCheckBlade2WaferIsExist()
        {
            if (robot.ReadParameter(new object[3]
              {
                "CheckWaferIsPresence",
                1,//blade2
                _existInterval
              }))
            {
                //机械臂2存在wafer
                LogObject.Info("System","Blade2存在wafer");
                return true;
            }
            else
            {
                //机械臂2存在wafer
                LogObject.Error("System", "Blade2不存在wafer");
                return false;
            }
        }

        private bool fCheckBlade1WaferIsExist()
        {
            if (robot.ReadParameter(new object[3]
              {
                "CheckWaferIsPresence",
                0,//blade1
                _existInterval
              }))
            {
                //机械臂1存在wafer
                LogObject.Info("System", "Blade1存在wafer");
                return true;
            }
            else
            {
                //机械臂1存在wafer
                LogObject.Error("System", "Blade1不存在wafer");
                return false;
            }
        }
        
        private bool fWaitCoolBufferMoveUp()
        {
            if (ModuleHelper.IsCoolingBuffer(Source))
            {
                //this.robot, Hand.Blade1, this._timeout
                IoCoolingBuffer device = GetCoolBuffer(Source);
                if (!device.Error)
                {
                    //检查上升的情况 并等其空闲
                    return device.CheckMovedUp() && !device.Busy;
                }
                return false;
            }
            if (ModuleHelper.IsAligner(Source))
            {
                return true;
            }
            return true;
        }

        private bool fCoolBufferMoveUP()
        {
            if (ModuleHelper.IsCoolingBuffer(Source))
            {
                IoCoolingBuffer device = GetCoolBuffer(Source);
                //上升了 如果没有就命令上升
                if (device.CheckPinUp() || device.Move(Singleton<WaferManager>.Instance.GetWaferSize(Source, Slot), true, out _))
                    return true;
                return false;
            }
            if (ModuleHelper.IsAligner(Source))
            {
                return true;
            }
            return true;
        }

        private bool CheckFlipperIsIdle()
        {
            return Singleton<RouteManager1>.Instance.GetFlipper().IsIdle;
        }

        private bool NotifyFlipperOpen()
        {
            Singleton<RouteManager1>.Instance.GetFlipper().PostMsg(FlipperMSG.PrepareTransfer);
            return true;
        }

        private bool NotifyFlipperClose()
        {
            Singleton<RouteManager1>.Instance.GetFlipper().PostMsg(FlipperMSG.EndTransfer);
            return true;
        }

        private bool CheckFlipperOpen()
        {
            return Singleton<RouteManager1>.Instance.GetFlipper().IsTransfer;
        }

        public RState Start(params object[] objs)
        {
            Reset();
            _IsFlipper = false;
            _timeout =robot.RobotCommandTimeout;
            _alignerdelay = SC.GetValue<int>("System.AlignerPinDownDelay");
            _coolingdelay = SC.GetValue<int>("System.CoolingPinDownDelay");
            _timeout = robot.RobotCommandTimeout * 1000;
            if (ModuleHelper.IsTurnOverStation(Source))
                _IsFlipper = true;

            if (!Singleton<EfemEntity>.Instance.IsOnlineMode && (ModuleHelper.IsCoolingBuffer(Source) || ModuleHelper.IsAligner(Source)))
            {
                delaytime = !ModuleHelper.IsCoolingBuffer(Source) ? _alignerdelay * 1000 : _coolingdelay * 1000;
            }
            else
            {
                delaytime = 0;
            }
            _pickHand = Hand.Blade1;
            if (PlaceBlade == Hand.Blade1)
                _pickHand = Hand.Blade2;
            return Runner.Start(Module, Name);
        }


        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 || ModuleHelper.IsTurnOverStation(chamber))
            {
                //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))
            {
                //PreAligner device =  DEVICE.GetDevice<PreAligner>(chamber.ToString());
                //if (device.Busy)
                //{
                //    reason = "Aligner is not Idle";
                //    return false;
                //}
                return true;
            }
            if (ModuleHelper.IsBuffer(chamber))
            {
                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 ? !robot.IsWaferPresenceOnBlade1 : !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 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
        }
        public enum SwapStep
        {
            WaitForSwapIdle,
            CoolBufferMoveUP,
            WaitCoolBufferMoveUp,
            CheckBlade1WaferIsExist,
            CheckBlade2WaferIsExist,
            CheckBeforeSwap,
            SwapWafer,
            PickWafer,
            WaitRobotMotion,
            PlaceWafer,
            CoolBufferMoveDown,
            WaitCoolBufferMoveDown,
            CheckAfterSwap,
            TimeDelay,
            NotifyFlipperPrepare,
            NotifyFlipperOver,
        }

        public IoCoolingBuffer GetCoolBuffer(ModuleName coolbufferName)
        {
            switch (coolbufferName)
            {
                //case ModuleName.Aligner1:
                //    _ioCoolBuffer = aligner1;
                //    break;
                //case ModuleName.Aligner2:
                //    this._ioCoolBuffer = this.aligner2;
                //    break;
                case ModuleName.CoolingBuffer1:
                    this._ioCoolBuffer = this.buffer1;
                    break;
                case ModuleName.CoolingBuffer2:
                    this._ioCoolBuffer = this.buffer2;
                    break;
            }
            return _ioCoolBuffer;
        }
    }
}