using Aitex.Core.RT.Device.Unit;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.OcrReaders;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
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 athosRT.Devices;
using MECF.Framework.Common.Equipment;
using athosRT.FSM;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using Aitex.Core.Common;
using Aitex.Sorter.Common;
using System.Diagnostics;
using static MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase.RobotBaseDevice;
using athosRT.Devices.PA;

namespace athosRT.tool
{
    public class CommonRoutine
    {
        protected bool bUINotify;
        protected RobotBaseDevice robot = null;
        protected Aligner aligner = null;
        protected OcrReader widreader = 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 List<LoadPortBaseDevice> LPs;
        protected readonly bool HaveAligner = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("AlignerInstalled").GetValueOrDefault();
        protected readonly bool AlignerNeedMoveUpCommand = Singleton<DeviceDefineManager>.Instance.GetValue<bool>(nameof(AlignerNeedMoveUpCommand)).GetValueOrDefault();
        protected readonly int LoadPortQuantity = Singleton<DeviceDefineManager>.Instance.GetValue<int>(nameof(LoadPortQuantity)).GetValueOrDefault();
        protected readonly string RobotTypeDefine = Singleton<DeviceDefineManager>.Instance.GetValue("RobotType");
        protected readonly bool HaveMotionAxis = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("MotionAxisInstalled").GetValueOrDefault();
        protected static readonly bool LoadLockDoorControlByStation = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("LLDoorControlByStation").GetValueOrDefault();
        protected readonly string LoadPortTypeDefine = Singleton<DeviceDefineManager>.Instance.GetValue("LoadPortType");
        protected int OffsetX;
        protected int OffsetY;
        protected int OffsetZ;
        private DeviceTimer _timerQuery = new DeviceTimer();
        private int _queryPeriod = 500;
        protected bool NeedSetParameter;
        protected bool IsStopped;
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        protected bool NeedRobotGripAndUngrip = SC.GetValue<bool>("System.EnableRobotGripAndUngrip");
        private DeviceTimer _timerPerf = new DeviceTimer();

        public string Module { get; set; }

        public string Name { get; set; }

        public string Display { get; set; }

        public CommonRoutine()
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            aligner = DEVICE.GetDevice<Aligner>("Aligner");
            widreader = DEVICE.GetDevice<OcrReader>("WIDReader");
            buffer1 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer1");
            buffer2 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer2");
            aligner1 = DEVICE.GetDevice<PreAligner>("Aligner1");
            aligner2 = DEVICE.GetDevice<PreAligner>("Aligner2");
            ll1 = DEVICE.GetDevice<LoadLockDevice>("LL1");
            ll2 = DEVICE.GetDevice<LoadLockDevice>("LL2");
            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 class RobotReset : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "RobotReset";
        public RobotReset(ModuleName module,RobotBaseDevice _robot,int _timeout) : base(module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
        }

        public RState Monitor()
        {
            Runner.Run(RobotResetStep.RobotReset, fRobotReset, timeout).
                End(RobotResetStep.CheckRobotState,fCheckRobotState,timeout);
            return Runner.Status;
        }

        private bool fCheckRobotState()
        {
            robot.RobotReset();
            LogObject.Info(header, "机械臂开始重置");
            return true;
        }

        private bool fRobotReset()
        {
            if (!robot.IsBusy && robot.RobotState != RobotStateEnum.Error && robot.RobotState == RobotStateEnum.Idle)
            {
                LogObject.Info(header,"重置成功 机械臂处于空闲状态");
                return true;
            }
            else
            {
                LogObject.Error(header, "重置失败 请检查机械臂");
                return false;
            }
        }


        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(ModuleName.Robot,header);
        }

        public void Abort()
        {

        }
        public enum RobotResetStep
        {
            RobotReset,
            CheckRobotState
        }
    }

    public class RobotHome : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "RobotHome";
        public RobotHome(ModuleName module, RobotBaseDevice _robot, int _timeout) : base(module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
        }

        public void Abort()
        {

        }

        public RState Monitor()
        {
            Runner.End(RobotHomeStep.RobotHome, fRobotHome, timeout);
            return Runner.Status;
        }

        private bool fRobotHome()
        {
            LogObject.Info(header, "机械臂执行home操作");
            robot.Home(null);
            return true;
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(ModuleName.Robot,header);
        }
        public enum RobotHomeStep
        {
            RobotHome
        }
    }

    public class RobotArmHome : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "RobotArmHome";
        ModuleName module ;
        public RobotArmHome(ModuleName _module, RobotBaseDevice _robot, int _timeout) : base(_module)
        {
            module = _module;
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
        }

        public void Abort()
        {
        }

        public RState Monitor()
        {
            Runner.Run(RobotArmHomeStep.RobotArmHome, fRobotArmHome, timeout)
                .End(RobotArmHomeStep.End, fFindName,timeout);
            return Runner.Status;
        }

        private bool fFindName()
        {
            Trace.WriteLine("robot status:"+robot.RobotState);
            return true;
        }

        private bool fRobotArmHome()
        {
            Trace.WriteLine(header+" and  state is "+robot.RobotState);
            robot.PostMsg((int)RobotMsg.StartHome);
            if (robot.Home(null))
            {
                Trace.WriteLine("home成功");
                LogObject.Info(header,"home成功");
                robot.PostMsg((int)RobotMsg.HomeComplete);
                return true;
            }
            else
            {
                Trace.WriteLine("home失败");
                //LogObject.Error(header, "home失败");
                robot.PostMsg((int)RobotMsg.ERROR);
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module, header);
        }
        public enum RobotArmHomeStep {
            RobotArmHome,
            End
        }
    }

    public class WaitRobotMotion : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "WaitRobotMotion";
        ModuleName module;
        public WaitRobotMotion(ModuleName _module, RobotBaseDevice _robot, int _timeout) : base(_module)
        {
            module = _module;
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
        }

        public void Abort()
        {

        }

        public RState Monitor()
        {
            Runner.Run(WaitRobotMotionStep.WaitRobotMotion, fWaitRobotMotion , timeout)
                .End(WaitRobotMotionStep.End, NullFun,0);
            return Runner.Status;
        }

        private bool fWaitRobotMotion()
        {
            Trace.WriteLine("WaitRobotMotion");
            if (robot.IsReady())
            {
                LogObject.Info(header,  "状态切换为准备态");
                return true;
            }
            else
            {
                //LogObject.Error(header, "状态未能切换过来");
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module, header);
        }
        public enum WaitRobotMotionStep
        {
            WaitRobotMotion,
            End
        }
    }

    public class ReserveRobotToBusy : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "ReserveRobotToBusy";
        ModuleName module;
        public ReserveRobotToBusy(ModuleName _module, RobotBaseDevice _robot, int _timeout) : base(_module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
            module = _module;
        }

        public void Abort()
        {

        }

        public RState Monitor()
        {
            Runner.End(ReserveRobotToBusyStep.ReserveRobotToBusy, fReserveRobotToBusy, timeout);
            return Runner.Status;
        }

        private bool fReserveRobotToBusy()
        {
            LogObject.Info(header,"将设备置为Busy状态");
            robot.IsBusy = true;
            return true;
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module,header);
        }

        public enum ReserveRobotToBusyStep {
            ReserveRobotToBusy
        }
    }

    public class CoolBufferMoveUP : ModuleRoutineBase, FSM.IRoutine
    {
        IoCoolingBuffer device;
        WaferSize size;
        ModuleName module;
        string header = "CoolBufferMoveUP";
        int timeout = 0;
        public CoolBufferMoveUP(ModuleName _module, IoCoolingBuffer _device, WaferSize _size, int _timeout) : base(_module)
        {
            device = _device;
            size = _size;
            module = _module;
            timeout = _timeout;
        }

        public void Abort()
        {
        }

        public RState Monitor()
        {
            Runner.End(CoolBufferMoveUPStep.CoolBufferMoveUP, fCoolBufferMoveUP,timeout);
            return Runner.Status;
        }

        private bool fCoolBufferMoveUP()
        {
            if (device.CheckPinUp() || device.Move(size, true, out _))
            {
                LogObject.Info(header, "CoolBufferMoveUP成功");
                return true;
            }
            else
            {
                //LogObject.Error(header, "fCoolBufferMoveUP失败");
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module,header);
        }

        public enum CoolBufferMoveUPStep {
            CoolBufferMoveUP
        }
    }

    public class UnGripRobotBlade : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "UnGripRobotBlade";
        ModuleName module;
        Hand blade;
        //IRoutine WaitRobotMotioRoutine;
        public UnGripRobotBlade(ModuleName _module, Hand _blade,RobotBaseDevice _robot, int _timeout) : base(_module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            timeout = _timeout;
            module = _module;
            blade = _blade;
            //WaitRobotMotioRoutine = new WaitRobotMotion(module,robot,timeout);
        }

        public void Abort()
        {

        }

        public RState Monitor()
        {
            Runner.Run(UnGripRobotBladeStep.UnGripRobotBlade, fUnGripRobotBlade, fWaitRobot, timeout)
                .End(UnGripRobotBladeStep.WaitRobot, NullFun,timeout);
            return Runner.Status;
        }

        private bool fWaitRobot()
        {
            if (!robot.IsBusy)
            {
                LogObject.Info(header,"robot非忙");
                return true;
            }
            else
            {
                //LogObject.Error(header,"robot忙");
                return false;
            }
        }

        private bool fUnGripRobotBlade()
        {
            if (robot.Release((RobotArmEnum)blade))
            {
                LogObject.Info(header, "释放成功");
                return true;
            }
            else
            {
                LogObject.Error(header,"释放失败");
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module,header);
        }
        public enum UnGripRobotBladeStep {
            UnGripRobotBlade,
            WaitRobot
        }
    }

    public class GripRobotBlade : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "GripRobotBlade";
        ModuleName module;
        Hand blade;
        public GripRobotBlade(ModuleName _module, Hand _blade, RobotBaseDevice _robot, int _timeout) : base(_module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            module = _module;
            timeout = _timeout;
            blade = _blade;
        }

        public void Abort()
        {
            
        }

        public RState Monitor()
        {
            Runner.Run(GripRobotBladeStep.GripRobotBlade, fGripRobotBlade, timeout)
                .Wait(GripRobotBladeStep.WaitRobot, fWaitRobot, _delay_1s)
                .End(GripRobotBladeStep.End, NullFun,_delay_1s);
            return Runner.Status;
        }

        private bool fWaitRobot()
        {
            if (!robot.IsBusy)
            {
                LogObject.Info(header, "robot非忙");
                return true;
            }
            else
            {
                //LogObject.Error(header, "robot忙");
                return false;
            }
        }

        private bool fGripRobotBlade()
        {
            if (robot.Grip((RobotArmEnum)blade))
            {
                LogObject.Info(header, "释放成功");
                return true;
            }
            else
            {
                //LogObject.Error(header, "释放失败");
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module,header);
        }

        public enum GripRobotBladeStep
        {
            GripRobotBlade,
            WaitRobot,
            End
        }
    }

    public class RobotGoto : ModuleRoutineBase, FSM.IRoutine
    {
        RobotBaseDevice robot = null;
        int timeout = 0;
        string header = "RobotGoto";
        ModuleName module;
        Hand blade;
        int slot = 0;
        RobotPostionEnum postype;
        public RobotGoto(ModuleName _module,RobotBaseDevice _robot,int _timeout, Hand _blade, int _slot, RobotPostionEnum _postype) : base(_module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            module = _module;
            timeout = _timeout;
            blade = _blade;
            slot = _slot;
            postype = _postype;
        }

        public void Abort()
        {

        }

        public RState Monitor()
        {
            Runner
                .End(RobotGotoStep.RobotGoto,robotgoto,timeout);
            return Runner.Status;
        }

        private bool robotgoto()
        {
            bool flag = robot.GoTo(new object[4]
            {
              (RobotArmEnum) blade,
              module.ToString(),
              slot,
              postype
            });
            if (flag)
            {
                LogObject.Info(header,"RobotGoto成功");
                return true;
            }
            else
            {
                //LogObject.Error(header, "RobotGoto失败");
                return false;
            }
        }

        public RState Start(params object[] objs)
        {
            Reset();
            return Runner.Start(module,header);
        }

        public enum RobotGotoStep {
            RobotGoto
        }
    }

    //public class HomeGripAndUngripRobotBlade : ModuleRoutineBase, FSM.IRoutine
    //{
    //    public HomeGripAndUngripRobotBlade(ModuleName module) : base(module)
    //    {

    //    }

    //    public void Abort()
    //    {
    //    }

    //    public RState Monitor()
    //    {
    //        return Runner.Status;
    //    }

    //    public RState Start(params object[] objs)
    //    {
    //        return Runner.Start();
    //    }
    //}
}