using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using SicModules.LLs.Routines.Base;

namespace SicModules.LLs.Routines
{
    public class LoadRotationHomeRoutine : LoadLockBaseRoutine
    {
        enum RoutineStep
        {
            TimeDelay1,
            TrayClamp,
            TrayUnClamp,
            ResetRotationServo,
            RotationServoOn,
            MoveRelativeHome,
            WaitMoveDone,
            MoveHomeOffset,
            TimeDelay2,
            WaitMoveDone2,
        }
        private float _homeOffset;

        private LoadLockTrayClawRoutine _trayClamp = new LoadLockTrayClawRoutine();
        private LoadLockTrayClawRoutine _trayUnClamp = new LoadLockTrayClawRoutine();

        public LoadRotationHomeRoutine()
        {
            Name = "Home";
        }

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

            _homeOffset = (float)SC.GetConfigItem($"LoadLock.LoadRotation.HomeOffset").DoubleValue;

            _trayClamp.Init(true);
            _trayUnClamp.Init(false);

            Notify("Start");

            return Result.RUN;
        }

        public override Result Monitor()
        {
            try
            {
                if (SC.GetValue<bool>($"System.IsSimulatorMode"))
                {
                    return Result.DONE;
                }

                //对中
                ExecuteRoutine((int)RoutineStep.TrayClamp, _trayClamp);        //夹爪关闭
                TimeDelay((int)RoutineStep.TimeDelay1, 1);//延迟1s
                ExecuteRoutine((int)RoutineStep.TrayUnClamp, _trayUnClamp);      //夹爪打开

                ResetRotationServo((int)RoutineStep.ResetRotationServo, 3);

                RotationServoOn((int)RoutineStep.RotationServoOn, 3);

                MoveRelativeHome((int)RoutineStep.MoveRelativeHome, 10);
                //TimeDelay((int)RoutineStep.TimeDelay2, 3);
                //WaitLoadRotationDone((int)RoutineStep.WaitMoveDone, 60);

                //取消Offset
                //MoveLoadRotationHomeOffset((int)RoutineStep.MoveHomeOffset, _homeOffset, 10);
                //TimeDelay((int)RoutineStep.TimeDelay2, 1);
                //WaitLoadRotationDone((int)RoutineStep.WaitMoveDone2, 60);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException ex)
            {
                LOG.Error(ex.ToString());
                return Result.FAIL;
            }

            Notify("Finished");

            return Result.DONE;
        }

        public override void Abort()
        {
            base.Abort();
        }

    }
}