using Aitex.Core.Common;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
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.Modules.FLP;
using athosRT.Modules.Robot;
using athosRT.tool;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
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.Diagnostics;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Linq;
using static athosRT.Modules.FLP.FlipperEntity;

namespace athosRT.Modules.EFEMs.Routine
{
    public class InitRoutine : ModuleRoutineBase, FSM.IRoutine
    {
        protected IoCoolingBuffer buffer1 = null;
        protected IoCoolingBuffer buffer2 = null;
        protected PreAligner aligner1 ;
        protected PreAligner aligner2 ;
        private SetSpeedRoutine RbSetRoutine = null;
        private int DeviceResetTimeout = 0;
        private bool _scanBuffer = true;
        private SCConfigItem _scRobotHomeTimeout => SC.GetConfigItem("Robot.TimeLimitRobotHome");
        private int _scFfuSpeed;
        private int _timeoutRobot = 0;
        private int _timeoutBuffer = 0;
        private RobotBaseDevice robot;
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        protected static readonly bool LoadLockDoorControlByStation = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("LLDoorControlByStation").GetValueOrDefault();
        private List<LoadPortBaseDevice> LPs;
        public InitRoutine(ModuleName module) : base(module)
        {
            Module = module;
            Name = "Init Routine";
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            RbSetRoutine = new SetSpeedRoutine(module);
            buffer1 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer1");
            buffer2 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer2");
            aligner1 = DEVICE.GetDevice<PreAligner>("Aligner1");
            aligner2 = DEVICE.GetDevice<PreAligner>("Aligner2");
            DeviceResetTimeout = 1000;//1s
            List <ModuleName> moduleNameList = new List<ModuleName>(Singleton<DeviceManager>.Instance.LpNames);
            LPs = new List<LoadPortBaseDevice>();
            
            moduleNameList.ForEach((lp =>
            { LPs.Add(DEVICE.GetDevice<LoadPortBaseDevice>(lp.ToString())); }));
        }

        

        public RState Monitor()
        {
            Runner
                .Run(InitStep.DeviceReset, fDeviceReset, DeviceResetTimeout)
                //.Run(InitStep.FfuSpeedSet1, fFfuSpeedSet1, fCheckFfuStatus1, _timeoutRobot)
                //.Run(InitStep.FfuSpeedSet2, fFfuSpeedSet2, fCheckFfuStatus2, _timeoutRobot)
                .Run(InitStep.RobotReset, fRobotReset, fcheckRobotState, _timeoutRobot)
                .Run(InitStep.RobotHome, fRobotHome, fWaitRobotMotion, _timeoutRobot)
                .Run(InitStep.CheckBlade1WaferIsExist, fCheckBlade1WaferIsExist, fWaitRobotMotion, _timeoutRobot)
                .Run(InitStep.CheckBlade2WaferIsExist, fCheckBlade2WaferIsExist, fWaitRobotMotion, _timeoutRobot)
                .Run(InitStep.ConfirmRobotBlade1Wafer, NullFun, fConfirmRobotBlade1Wafer, _timeoutRobot)
                .Run(InitStep.ConfirmRobotBlade2Wafer, NullFun, fConfirmRobotBlade2Wafer, _timeoutRobot)
                .Run(InitStep.HomeGripAndUngripRobotBlade1, fHomeGripAndUngripRobotBlade1, fRobotNoBusy, _timeoutRobot)
                .Run(InitStep.HomeGripAndUngripRobotBlade2, fHomeGripAndUngripRobotBlade2, fRobotNoBusy, _timeoutRobot)
                //.Run(InitStep.FlipperHome, fFlipperHome, fFlipperIdle, _timeoutRobot)
                //.Run(InitStep.CooingbufferHome1, fCooingbufferHome1, fCheckCooingbufferHome1, _timeoutBuffer)-
                //.Run(InitStep.CooingbufferHome2, fCooingbufferHome2, fCheckCooingbufferHome2, _timeoutBuffer)-
                //.Run(InitStep.CooingbufferHome3, fAligner1Home, fCheckAligner1Home, _timeoutBuffer)-
                //.Run(InitStep.CooingbufferHome4, fAligner2Home, fCheckAligner2Home, _timeoutBuffer)-
                //.Run(InitStep.LoadLockCloseAtmDoor, fLoadLockCloseAtmDoor, fCheckLoadLockCloseAtmDoor, _timeoutRobot)-
                //.Run(InitStep.LoadLockCloseVtmDoor, fLoadLockCloseVtmDoor, fCheckLoadLockCloseVtmDoor, _timeoutRobot)-
                .Run(InitStep.LoadportAllReset, fLoadportAllReset, fCheckLoadportAllReset, _timeoutRobot)
                .Run(InitStep.LoadportAllInit, fLoadportAllInit, fCheckLoadportAllInit, _timeoutRobot)
                .End(InitStep.End, NullFun);
            return Runner.Status;
        }

        private bool fCheckLoadportAllInit()
        {
            bool flag1 = LPs.Any(lp=>lp.CurrentState == LoadPortStateEnum.Error);
            bool flag2 = LPs.Any(lp => { return !lp.IsHomed; });
            if (flag1)
            {
                LogObject.Error("LPs","LP中存在错误");
                return false;
            }
            if (flag2)
            {
                //LogObject.Error("LPs", "LP中非home状态");
                return false;
            }
            LogObject.Info("LPs", "LP已完成init");
            return true;
        }

        private bool fLoadportAllInit()
        {
            LPs.ForEach(lp => lp.Home(out _));
            LogObject.Info("LPs", "全部LP开始初始化");
            return true;
        }

        private bool fCheckLoadportAllReset()
        {
            bool flag = LPs.Any(lp => lp.IsBusy);
            if (flag)
            {
                LogObject.Error("LPs", "尚有LP未脱离busy状态");
                return false;
            }
            else
            {
                LogObject.Info("LPs", "全部LP脱离busy状态");
                return true;
            }
        }

        private bool fLoadportAllReset()
        {
            LPs.ForEach(LP => LP.ClearError(out _));
            LogObject.Info("LPs", "开始重置所有LP");
            return true;
        }

        private bool fCheckLoadLockCloseVtmDoor() 
        {
            if (LoadLockDoorControlByStation)
            {
                bool flag = LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Closed;
                if (flag)
                {
                    LogObject.Info("LoadLockDevice", "VtmDoor Close Success");
                    return true;
                }
                else
                {
                    LogObject.Error("LoadLockDevice", "VtmDoor Close Error");
                    return false;
                }
            }
            return true;
        }
        private bool fLoadLockCloseVtmDoor()
        {
            if (LoadLockDoorControlByStation)
            {
                //检查LoadLockAtmDoor的状态 如果没有的话就执行关闭AtmDoor
                bool flag = LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Closed || LoadLockDevice.CloseVtmDoor(out string _);
                if (flag)
                {
                    LogObject.Info("LoadLockDevice", "VtmDoor Close Start");
                    return true;
                }
                else
                {
                    LogObject.Error("LoadLockDevice", "VtmDoor Close can not excute!");
                    return false;
                }
            }
            return true;
        }

        private bool fCheckLoadLockCloseAtmDoor() 
        {
            if (LoadLockDoorControlByStation)
            {
                bool flag = LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Closed;
                if (flag)
                {
                    LogObject.Info("LoadLockDevice", "AtmDoor Close Success");
                    return true;
                }
                else
                {
                    LogObject.Error("LoadLockDevice", "AtmDoor Close Error");
                    return false;
                }
            }
            return true;
        }

        private bool fLoadLockCloseAtmDoor()
        {
            if (LoadLockDoorControlByStation)
            {
                //检查LoadLockAtmDoor的状态 如果没有的话就执行关闭AtmDoor
                bool flag = LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Closed || LoadLockDevice.CloseAtmDoor(out string _);
                if (flag)
                {
                    LogObject.Info("LoadLockDevice", "AtmDoor Close Start");
                    return true;
                }
                else
                {
                    LogObject.Error("LoadLockDevice", "AtmDoor Close can not excute!");
                    return false;
                }
            }
            return true;
        }

        private bool fCheckAligner2Home()
        {
            //if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            //{
            //    if (aligner2.Error)
            //    {
            //        LogObject.Error("aligner2", "home Error");
            //        return false;
            //    }
            //    else if (aligner2.CheckMovedDown() && !aligner1.Busy)
            //    {
            //        LogObject.Info("aligner2", "Moved Down Over");
            //        return true;
            //    }
            //    else
            //    {
            //        //LogObject.Warning("aligner2", "Moved Down Error Or Busy");
            //        return false;
            //    }
            //}
            if (aligner2 != null)
            {
                string reason = "";
                if (aligner2.Error)
                {
                    LogObject.Error("aligner2", "Home failed!");
                    return true;
                }
                else if (!aligner2.Busy)
                {
                    LogObject.Info("aligner2", "Home Succeed!");
                    return true;
                }
                else
                {
                    return false;
                }
            }
            return true;
        }
        private bool fAligner2Home()
        {
            //if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            //{
            //    aligner2.Home(out _);
            //}
            if (aligner2 != null)
            {
                string reason = "";
                return aligner2.Home(out reason);
            }
            return true;
        }
        private bool fCheckAligner1Home()
        {
            //2023/9/6 删除相关IO 调整相关代码
            //if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBuffer1Installed").GetValueOrDefault())
            //{
            //    if (aligner1.Error)
            //    {
            //        LogObject.Error("aligner1", "home Error");
            //        return false;
            //    }
            //    else if (aligner1.CheckMovedDown() && !aligner1.Busy)
            //    {
            //        LogObject.Info("aligner1", "Moved Down Over");
            //        return true;
            //    }
            //    else
            //    {
            //        //LogObject.Warning("aligner1", "Moved Down Error Or Busy");
            //        return false;
            //    }
            //}
            if (aligner1 != null)
            {
                string reason = "";
                if (aligner1.Error)
                {
                    LogObject.Error("aligner1", "Home failed!");
                    return true;
                }
                else if (!aligner1.Busy)
                {
                    LogObject.Info("aligner1", "Home Succeed!");
                    return true;
                }
                else
                {
                    return false;
                }
            }
            return true;
        }
        private bool fAligner1Home()
        {
            //if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBuffer1Installed").GetValueOrDefault())
            //{
            //    aligner1.Home(out _);
            //}
            if (aligner1 != null)
            {
                string reason = "";
                return aligner1.Home(out reason);
            }
            return true;
        }

        private bool fCheckCooingbufferHome2()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            {
                if (buffer2.Error)
                {
                    LogObject.Error("Coolingbuffer2", "home Error");
                    return false;
                }
                else if (buffer2.CheckMovedDown() && !buffer2.Busy)
                {
                    LogObject.Info("Coolingbuffer2", "Moved Down Over");
                    return true;
                }
                else
                {
                    //LogObject.Warning("Coolingbuffer2", "Moved Down Error Or Busy");
                    return false;
                }
            }
            return true;
        }

        private bool fCooingbufferHome2()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            {
                buffer2.Home(out _);
            }
            return true;
        }

        private bool fCheckCooingbufferHome1() 
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            { 
                if (buffer1.Error)
                {
                    LogObject.Error("Coolingbuffer1","home Error");
                    return false;
                }
                else if (buffer1.CheckMovedDown() && !buffer1.Busy)
                {
                    LogObject.Info("Coolingbuffer1", "Moved Down Over");
                    return true;
                }
                else 
                {
                    //LogObject.Warning("Coolingbuffer1", "Moved Down Error Or Busy");
                    return false; 
                }
            }
            return true;
        }

        private bool fCooingbufferHome1()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("CoolingBufferInstalled").GetValueOrDefault())
            {
                Trace.WriteLine("buffer1 Home");
                buffer1.Home(out _);
            }
            return true;
        }

        private bool fRobotNoBusy() {
            if (robot.IsBusy)
            {
                //LogObject.Error("robot", "检查robot状态忙");
                return false;
            }
            else
            {
                LogObject.Info("robot", "检查robot状态不忙");
                return true;
            }
        }

        private bool fFlipperIdle()
        {
            return Singleton<RouteManager1>.Instance.GetFlipper().State == Devices.FLP.FlipperState.Idle;
        }

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

        private bool fHomeGripAndUngripRobotBlade2()
        {
            if (Singleton<WaferManager>.Instance.CheckHasWafer(ModuleName.Robot, (int)Hand.Blade2))
            {
                //有wafer 吸附上去
                if (!robot.Grip((RobotArmEnum)Hand.Blade2))
                {
                    LogObject.Error("robot", "Grip失败");
                    return false;
                }
            }
                //没wafer 释放掉
            else if (!robot.Release((RobotArmEnum)Hand.Blade2))
            {
                LogObject.Error("robot", "Release失败");
                return false;
            }
            return true;
        }

        private bool fHomeGripAndUngripRobotBlade1()
        {
            //Hand.Blade1, this.robot, this._timeoutRobot

            if (Singleton<WaferManager>.Instance.CheckHasWafer(ModuleName.Robot, (int)Hand.Blade1))
            {
                // "抓的动作"
                if (!robot.Grip((RobotArmEnum)Hand.Blade1))
                {
                    LogObject.Error("robot", "Grip失败");
                    return false;
                }
            }
            else if (!robot.Release((RobotArmEnum)Hand.Blade1))
            {
                LogObject.Error("robot", "Release失败");
                return false;
            }
            return true;
        }

        private bool fConfirmRobotBlade2Wafer()
        {
            //WaferManager是持久层 存储断电重启时wafer的状态
            bool flag = robot.IsWaferPresenceOnBlade2;
            int num = 1;
            if (flag)
            {
                //实际有wafer 界面没wafer
                if (Singleton<WaferManager>.Instance.CheckNoWafer(ModuleName.Robot, num))
                {
                    //手臂上加个wafer
                    Singleton<WaferManager>.Instance.CreateWafer(ModuleName.Robot, num, WaferStatus.Normal);
                }
            }
            //手臂没wafer 手上有wafer
            else if (Singleton<WaferManager>.Instance.CheckHasWafer(ModuleName.Robot, num))
            {
                //把手臂上的wafer去掉
                Singleton<WaferManager>.Instance.DeleteWafer(ModuleName.Robot, num);
            }
            return true;
        }

        private bool fConfirmRobotBlade1Wafer()
        {
            //这里是单对WaferManager的处理 不需check
            bool flag = robot.IsWaferPresenceOnBlade1;
            int num = 0;
            if (flag)
            {
                if (Singleton<WaferManager>.Instance.CheckNoWafer(ModuleName.Robot, num))
                {
                    Singleton<WaferManager>.Instance.CreateWafer(ModuleName.Robot, num, WaferStatus.Normal);
                }
            }
            else if (Singleton<WaferManager>.Instance.CheckHasWafer(ModuleName.Robot, num))
            {
                Singleton<WaferManager>.Instance.DeleteWafer(ModuleName.Robot, num);
            }
            return true;
        }

        private bool fCheckBlade2WaferIsExist()
        {
            bool flag = robot.ReadParameter(new object[3] { "CheckWaferIsPresence", 1, _existInterval });
            if (flag)
            {
                LogObject.Info("Robot", "Check Blade2 Wafer Is Exist");
                return true;
            }
            else
            {
                LogObject.Error("Robot", "Check Blade2 Wafer Is not Exist");
                return false;
            }
        }

        private bool fCheckBlade1WaferIsExist()
        {
            bool flag = robot.ReadParameter(new object[3] { "CheckWaferIsPresence", 0, _existInterval });
            if (flag)
            {
                LogObject.Info("Robot", "Check Blade1 Wafer Is Exist");
                return true;
            }
            else
            {
                LogObject.Error("Robot", "Check Blade1 Wafer Is not Exist");
                return false;
            }
        }

        private bool fWaitRobotMotion()
        {
            if (robot.IsReady()) 
            {
                LogObject.Info("Robot","等待Robot准备完毕");
                return true;
            }
            return false;
        }
        private bool fRobotHome()
        {
            robot.Home(null);
            LogObject.Info("Robot", "RobotHome start");
            return true;
        }

        private bool fRobotReset()
        {
            robot.RobotReset();
            LogObject.Info("Robot", "RobotReset start");
            return true;
        }

        private bool fcheckRobotState() {
            RobotBaseDevice device = robot;
            //LogObject.Info("Robot State", $"Busy:{device.IsBusy},Robot State:{device.RobotState}");
            if (!device.IsBusy && device.RobotState != RobotStateEnum.Error && device.RobotState == RobotStateEnum.Idle)
            {
                LogObject.Info("Robot", "CheckRobotState end");
                return true;
            }
            else
            {
                //LogObject.Error("Robot", "CheckRobotState error");
                return false;
            }
        }

        private bool fFfuSpeedSet1()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("FfuMemoBusControl").GetValueOrDefault())
            {
                string name = "FFU";
                Ffu device = DEVICE.GetDevice<Ffu>(name);
                device?.StartAndSetSpeed(_scFfuSpeed);
                LogObject.Info(name, "fFfuSpeedSet start");
            }
            return true;
        }
        private bool fFfuSpeedSet2()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("FfuMemoBusControl").GetValueOrDefault())
            {
                string name = "FFU2";
                Ffu device = DEVICE.GetDevice<Ffu>(name);
                device.StartAndSetSpeed(_scFfuSpeed);
                LogObject.Info(name, "FfuSpeedSet start");
            }
            return true;
        }

        private bool fCheckFfuStatus1() {
            //return true;
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("FfuMemoBusControl").GetValueOrDefault())
            {
                if (Ffu.SpeedSet1)
                {
                    LogObject.Info("FFU", "FfuSpeedSet end");
                    return true;
                }
                //LogObject.Error("FFU", "FfuSpeedSet error");
                return true;
            }
            return true;
        }

        private bool fCheckFfuStatus2()
        {
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("FfuMemoBusControl").GetValueOrDefault())
            {
                if (Ffu.SpeedSet2)
                {
                    LogObject.Info("FFU2", "FfuSpeedSet end");
                    return true;
                }
                LogObject.Error("FFU2", "FfuSpeedSet error");
                return false;
            }
            return true;
        }

        private bool fDeviceReset()
        {
            //设备状态机重置
            Singleton<DeviceEntity>.Instance.PostMsg(DeviceEntityT<DeviceManager>.MSG.RESET);
            LogObject.Info("Device","设备重置");
            return true;
        }

        public RState Start(params object[] objs)
        {
            Reset();
            Trace.WriteLine($"检查是否更新timeout参数：{_scRobotHomeTimeout.IntValue}");
            _timeoutRobot = _scRobotHomeTimeout.IntValue * 1000;
            _timeoutBuffer = SC.GetValue<int>("Coolbuffer.DownTimeout") * 1000;
            RbSetRoutine.Speed = SC.GetValue<int>("Robot.Robot.SpeedLevel");
           // RbSetRoutine.SetRobotSpeed( RbSetRoutine.Speed);
            if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("FfuMemoBusControl").GetValueOrDefault())
                _scFfuSpeed = SC.GetValue<int>("System.FFUSetSpeed");
            _scanBuffer = true;
            WaferSensorResetCreate();
            return Runner.Start(ModuleName.System,"initRoutine");
        }


        public void Abort()
        {

        }

        private void WaferSensorResetCreate()
        {
            if (!this._scanBuffer)
                return;
            if (DeviceModel.SensorPreAlignerWaferOn != null && !DeviceModel.SensorPreAlignerWaferOn.Value && Singleton<WaferManager>.Instance.CheckNoWafer(ModuleName.Aligner, 0))
                Singleton<WaferManager>.Instance.CreateWafer(ModuleName.Aligner, 0, WaferStatus.Normal);
            else if (Singleton<DeviceDefineManager>.Instance.GetValue<bool>("AlignerInstalled").GetValueOrDefault())
                Singleton<WaferManager>.Instance.DeleteWafer(ModuleName.Aligner, 0);
            this._scanBuffer = false;
        }

        public enum InitStep
        {
            DeviceReset,
            FfuSpeedSet1,
            FfuSpeedSet2,
            RobotReset,
            RobotHome,
            CheckBlade1WaferIsExist,
            CheckBlade2WaferIsExist,
            ConfirmRobotBlade1Wafer,
            ConfirmRobotBlade2Wafer,
            HomeGripAndUngripRobotBlade1,
            HomeGripAndUngripRobotBlade2,
            FlipperHome,
            CooingbufferHome1,
            CooingbufferHome2,
            CooingbufferHome3,
            CooingbufferHome4,
            LoadLockCloseAtmDoor,
            LoadLockCloseVtmDoor,
            LoadportAllReset,
            LoadportAllInit,
            End
        }
    }
}