using System;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.Equipment;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;

namespace JetMainframe.TMs
{
    public class TMHomeRoutine : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            ClearError,

            QueryArmPos,

            CheckArmPos,

            InitRobot,

            HomeRobot,
        }

        private int _timeout = 0;

        private TMModule _tmModule;

        public TMHomeRoutine(TMModule robotModule)
        {
            Module = ModuleName.TMRobot.ToString();

            Name = "Home";

            _tmModule = robotModule;
        }



        public Result Start(params object[] objs)
        {
            Reset();

            _timeout = SC.GetValue<int>("TM.TMRobot.HomeTimeout");

            Notify($"Start");

            return Result.RUN;
        }


        public Result Monitor()
        {
            try
            {
                ClearError((int)RoutineStep.ClearError, _timeout);

                QueryArmPos((int)RoutineStep.QueryArmPos, _timeout);

                CheckArmPos((int)RoutineStep.CheckArmPos, _timeout);

                //InitRobot((int)RoutineStep.InitRobot, _timeout);

                HomeRobot((int)RoutineStep.HomeRobot, _timeout);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException)
            {
                return Result.FAIL;
            }

            Notify("Finished");
            return Result.DONE;
        }

        public void ClearError(int id, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Start clear Robot error");
                _tmModule.RobotDevice.Reset();
                return true;
            }, () =>
            {
                if (_tmModule.RobotDevice.IsBusy)
                    return false;
                if (_tmModule.RobotDevice.IsError)
                    return false;

                return true;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop(string.Format("Home failed."));
                    throw new RoutineFaildException();
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(string.Format("Home timeout, can not clear error in {0} seconds", timeout));
                    throw new RoutineFaildException();
                }
                else
                    throw new RoutineBreakException();
            }
        }


        public void QueryArmPos(int id, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Start Query arm pos");
                _tmModule.RobotDevice.PostMsg(RobotBaseDevice.RobotMsg.ReadData, "CurrentPositionData");
                return true;
            }, () =>
            {
                if (_tmModule.RobotDevice.IsError)
                    return null;
                if (!_tmModule.RobotDevice.IsIdle)
                    return false;
                return true;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop(string.Format("Home failed , Query arm pos failed ."));
                    throw new RoutineFaildException();
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(string.Format("Home timeout, can not complete in {0} seconds", timeout));
                    throw new RoutineFaildException();
                }
                else
                    throw new RoutineBreakException();
            }
        }

        public void CheckArmPos(int id, int timeout)
        {
            Tuple<bool, Result> ret = Check(id, () =>
            {
                return _tmModule.RobotDevice.IsInSafePosition();
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop(string.Format("Home failed , arm in unsafe position ."));
                    throw new RoutineFaildException();
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(string.Format("Home timeout, can not complete in {0} seconds", timeout));
                    throw new RoutineFaildException();
                }
                else
                    throw new RoutineBreakException();
            }
        }

        //public void InitRobot(int id, int timeout)
        //{
        //    Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
        //    {
        //        Notify($"Start Retract robot arm");

        //        if (!_tmModule.RobotDevice.Retract(null))
        //        {
        //            return false;
        //        }
        //        return true;
        //    }, () =>
        //    {
        //        if (!_tmModule.RobotDevice.IsIdle)
        //            return false;

        //        return true;
        //    }, timeout * 1000);

        //    if (ret.Item1)
        //    {
        //        if (ret.Item2 == Result.FAIL)
        //        {
        //            Stop(string.Format("Home failed."));
        //            throw new RoutineFaildException();
        //        }
        //        else if (ret.Item2 == Result.TIMEOUT) //timeout
        //        {
        //            Stop(string.Format("Home timeout, can not complete in {0} seconds", timeout));
        //            throw new RoutineFaildException();
        //        }
        //        else
        //            throw new RoutineBreakException();
        //    }
        //}


        public void HomeRobot(int id, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Start home robot");

                if (!_tmModule.RobotDevice.Home(null))
                {
                    return false;
                }
                return true;
            }, () =>
            {
                if (_tmModule.RobotDevice.IsError)
                {
                    return null;
                }

                if (!_tmModule.RobotDevice.IsIdle)
                    return false;

                return true;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop(string.Format("Home failed."));
                    throw new RoutineFaildException();
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(string.Format("Home timeout, can not complete in {0} seconds", timeout));
                    throw new RoutineFaildException();
                }
                else
                    throw new RoutineBreakException();
            }
        }
        
        //public void CheckRobotWafer(int id, int timeout)
        //{
        //    Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
        //    {
        //        Notify($"Start check robot wafer present");
        //        //string reason;

        //        //if (!_efem.QueryRobotWaferPresence(out _, out reason))
        //        //{
        //        //    Stop(reason);
        //        //    return false;
        //        //}
        //        return true;
        //    }, () =>
        //    {
        //        //if (_efem.CheckIsBusy(ModuleName.System))
        //        //    return false;

        //        return true;
        //    }, timeout * 1000);

        //    if (ret.Item1)
        //    {
        //        if (ret.Item2 == Result.FAIL)
        //        {
        //            Stop(string.Format("Home failed."));
        //            throw new RoutineFaildException();
        //        }
        //        else if (ret.Item2 == Result.TIMEOUT) //timeout
        //        {
        //            Stop(string.Format("Home timeout, can not complete in {0} seconds", timeout));
        //            throw new RoutineFaildException();
        //        }
        //        else
        //            throw new RoutineBreakException();
        //    }
        //}


        public void Abort()
        {

        }

    }
}