/*
  * Copyright (c) 2025 LG Electronics Inc.
   * SPDX-License-Identifier: LicenseRef-LGE-Proprietary
   */
using Aurora.Api;
using Aurora.Api.DataType;
using Aurora.Api.Devices;
using VisionCommon;
using Hermes;
using System;
using System.Collections.Generic;
using System.Reactive.Linq;
using System.Threading;
using System.Windows;
using VisionCommonUI;
using VisionCommonUI.Resources;

namespace VisionSequence.Setup {
    /// <summary>
    /// 자동 초점(축) 모드
    /// </summary>
    public enum eFocusMode {
        /// <summary>대략적으로</summary>
        Roughly,
        /// <summary>섬세하게</summary>
        Delicately,
        /// <summary>종료</summary>
        Finished
    }

    /// <summary>
    /// 자동 초점(축) 로그
    /// </summary>
    public class AutoFocusLog : ICloneable {
        /// <summary>공정 시간</summary>
        public string Time { get; set; }
        /// <summary>스텝 번호</summary>
        public int Step { get; set; }
        /// <summary>자동 초점(축) 모드</summary>
        public string Mode { get; set; }
        /// <summary>Sharpness 측정 값</summary>
        public int Sharpness { get; set; }
        /// <summary>Z축 위치</summary>
        public double PosZ { get; set; }

        /// <summary>
        /// Clone (Copy)
        /// </summary>
        /// <returns>자동 초점(축) 로그</returns>
        public object Clone() {
            return new AutoFocusLog() {
                Time = Time,
                Step = this.Step,
                Mode = this.Mode,
                Sharpness = this.Sharpness,
                PosZ = this.PosZ
            };
        }

        /// <summary>
        /// 생성자
        /// </summary>
        public AutoFocusLog() { }

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="step">스텝 번호</param>
        /// <param name="mode">자동 초점(축) 모드</param>
        /// <param name="sharpness">Sharpness 측정 값</param>
        /// <param name="posZ">Z축 위치</param>
        public AutoFocusLog(int step, eFocusMode mode, int sharpness, double posZ) {
            Step = step;
            Mode = mode.ToString();
            Sharpness = sharpness;
            PosZ = posZ;
        }
    }

    /// <summary>
    /// 자동 초첨(축) 로그 매니저
    /// </summary>
    public class AutoFocusLogManager : ItemManagerBase {
        private static readonly object _Lock = new object();

        /// <summary>
        /// 자동 초첨(축) 로그에 공정시간을 더해 리턴한다.
        /// </summary>
        /// <param name="subSysInfo">서브 시스템 키 정보</param>
        /// <param name="log">자동 초첨(축) 로그</param>
        /// <returns>자동 초첨(축) 로그</returns>
        public static AutoFocusLog GetAutoFocusLog(SubSysInfo subSysInfo, AutoFocusLog log) {
            lock (_Lock) {
                return new AutoFocusLog() {
                    Time = GetTimeStamp(subSysInfo, log),
                    Step = log.Step,
                    Mode = log.Mode,
                    Sharpness = log.Sharpness,
                    PosZ = log.PosZ
                };
            }
        }
    }

    /// <summary>
    /// 자동 초점(축) 셋업 시퀀스 요청 파라미터
    /// </summary>
    public class SetupSeqParams_AutoFocus {
        /// <summary>FOV 키 정보</summary>
        public FovInfo FovInfo { get; set; }
        /// <summary>자동 초점(축) 레시피</summary>
        public AutoFocusRecipe AutoFocusRecipe { get; set; }
        /// <summary>메인 화면 타입</summary>
        public eDisplayType DisplayType { get; set; }

        /// <summary>
        /// 자동 초점(축) 셋업 시퀀스 요청 파라미터
        /// </summary>
        /// <param name="fovInfo">FOV 키 정보</param>
        /// <param name="autoFocusRecipe">자동 초점(축) 레시피</param>
        /// <param name="displayType">메인 화면 타입</param>
        public SetupSeqParams_AutoFocus(
            FovInfo fovInfo,
            AutoFocusRecipe autoFocusRecipe,
            eDisplayType displayType = eDisplayType.AutoFocus) {
            this.FovInfo = fovInfo;
            this.AutoFocusRecipe = autoFocusRecipe;
            this.DisplayType = displayType;
        }
    }

    /// <summary>
    /// 자동 초점(축) 셋업 시퀀스 결과 파라미터
    /// </summary>
    public class SetupSeqResultParams_AutoFocus {
        /// <summary>최적의 스텝 번호</summary>
        public int BestStep { get; set; }
        /// <summary>최적의 Sharpness 측정 값</summary>
        public int BestSharpness { get; set; }
        /// <summary>최적의 Z축 위치</summary>
        public double BestPosZ { get; set; }

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="bestStep">최적의 스텝 번호</param>
        /// <param name="bestSharpness">최적의 Sharpness 측정 값</param>
        /// <param name="bestPosZ">최적의 Z축 위치</param>
        public SetupSeqResultParams_AutoFocus(int bestStep, int bestSharpness, double bestPosZ) {
            this.BestStep = bestStep;
            this.BestSharpness = bestSharpness;
            this.BestPosZ = bestPosZ;
        }
    }

    /// <summary>
    /// 자동 초점(축) 조절
    /// </summary>
    public class SetupSeq_AutoFocus {
        private readonly Logger LOG = new Logger(typeof(SetupSeq_AutoFocus).Name);
        private SetupSeqParams_AutoFocus _Params;
        private FovRecipe _FovRecipe;
        private CamConfig _CamConfig;
        private CameraAgentBase _CameraAgent;
        private ICameraDisplay _CameraDisplay;
        private CsvManager _CsvManager;
        private ImageManager _ImageManager = new ImageManager();

        private List<AutoFocusLog> AutoFocusLogList = new List<AutoFocusLog>();
        private eFocusMode _FocusMode = eFocusMode.Finished;

        private double _MoveBackStepValue = 1.5;
        private double _MoveStepValue = 0.1;
        private double _MoveStepMaxCnt = 30;

        private bool _IsObservableSet = false;
        private int _CurrentStep;
        private int _CurrentSharpness;
        private double _CurrentPositionZ;

        private object _GrabImage = null;

        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";
        private const string SEQ_ID_MOVE_TEACHING_POSITION = "MOVE_TEACHING_POSITION";
        private const string SEQ_ID_CHECK_FOCUS_MODE = "CHECK_FOCUS_MODE";
        private const string SEQ_ID_CLEAR = "CLEAR";
        private const string SEQ_ID_MOVE_BACK_STEP = "MOVE_BACK_STEP";
        private const string SEQ_ID_MOVE_BEST_POS = "MOVE_BEST_POS";
        private const string SEQ_ID_SAVE_BEST_POS = "SAVE_BEST_POS";
        private const string SEQ_ID_MOVE_STEP = "MOVE_STEP";
        private const string SEQ_ID_GRAB = "GRAB";
        private const string SEQ_ID_SAVE_IMAGE = "SAVE_IMAGE";
        private const string SEQ_ID_FINAL_GRAB = "FINAL_GRAB";
        private const string SEQ_ID_MEASURE_SHARPNESS = "MEASURE_SHARPNESS";
        private const string SEQ_ID_MEASURE_NG = "MEASURE_NG";
        private const string SEQ_ID_MOVE_FINAL_POSITION = "MOVE_FINAL_POSITION";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        #endregion

        /// <summary>
        /// 생성자
        /// </summary>
        public SetupSeq_AutoFocus() {
        }

        private void SetActions(SequenceBuilder sb) {
            sb.AddAction(SEQ_ID_INIT, Init);
            sb.AddAction(SEQ_ID_MOVE_TEACHING_POSITION, MoveTeachingPos);
            sb.AddDecision(SEQ_ID_CHECK_FOCUS_MODE, CheckFocusMode);
            sb.AddAction(SEQ_ID_CLEAR, Clear);
            sb.AddAction(SEQ_ID_MOVE_BACK_STEP, MoveBackStep);
            sb.AddAction(SEQ_ID_MOVE_STEP, MoveStep);
            sb.AddAction(SEQ_ID_GRAB, Grab);
            sb.AddDecision(SEQ_ID_MEASURE_SHARPNESS, MeasureSharpness);
            sb.AddAction(SEQ_ID_MEASURE_NG, MeasureNG);
            sb.AddAction(SEQ_ID_MOVE_BEST_POS, MoveBestPos);
            sb.AddAction(SEQ_ID_FINAL_GRAB, Grab);
            sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
            sb.AddAction(SEQ_ID_SAVE_BEST_POS, SaveBestPos);
            sb.AddAction(SEQ_ID_MOVE_FINAL_POSITION, MoveFinalPos);
            sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
        }

        public SequenceBuilder Builder() {
            SequenceBuilder sb = new SequenceBuilder(1000);
            SetActions(sb);
            sb.OnStopped(OnStopped);
            sb.OnForceError(OnForceError);

            sb.Flows(
                  sb.Init
                > sb[SEQ_ID_INIT]
                > sb[SEQ_ID_MOVE_TEACHING_POSITION]
                > sb[SEQ_ID_CHECK_FOCUS_MODE]
                > sb[SEQ_ID_CLEAR]
                > sb[SEQ_ID_MOVE_BACK_STEP]
                > sb[SEQ_ID_MOVE_STEP]
                > sb[SEQ_ID_GRAB]
                > sb[SEQ_ID_SAVE_IMAGE]
                > sb[SEQ_ID_MEASURE_SHARPNESS]
                );

            sb.Flows(
                  sb[SEQ_ID_CHECK_FOCUS_MODE]
                > sb[SEQ_ID_FINAL_GRAB]
                > sb[SEQ_ID_SAVE_BEST_POS]
                > sb[SEQ_ID_MOVE_FINAL_POSITION]
                > sb[SEQ_ID_SEQ_COMPLETE]
                > sb.Final
                );

            sb.Flows(
                  sb[SEQ_ID_MEASURE_SHARPNESS]
                > sb[SEQ_ID_MEASURE_NG]
                > sb[SEQ_ID_MOVE_FINAL_POSITION]
                > sb[SEQ_ID_SEQ_COMPLETE]
                > sb.Final
                );

            sb.Flows(
                  sb[SEQ_ID_MEASURE_SHARPNESS]
                > sb[SEQ_ID_MOVE_STEP]
                );

            sb.Flows(
                  sb[SEQ_ID_MEASURE_SHARPNESS]
                > sb[SEQ_ID_MOVE_BEST_POS]
                );

            sb.Flows(
                  sb[SEQ_ID_MOVE_BEST_POS]
                > sb[SEQ_ID_CHECK_FOCUS_MODE]
                );

#if SEQ_DRAW
            sb.Draw($"{GetType().Name}.pdf");
#endif

            return sb;
        }

        private SequenceActionResult Init(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            if (param.Param != null) {
                _Params = param.Param as SetupSeqParams_AutoFocus;
            }
            else {
                LOG.E($"Param is null.");
                return null;
            }

            _FocusMode = eFocusMode.Roughly;
            _FovRecipe = VisionRecipeManager.GetFovRecipe(_Params.FovInfo);
            _CamConfig = VisionEnv.AuroraEnv.DeviceManager.
                GetDeviceConfig(_FovRecipe.CamRecipe.CamId.ToString()) as CamConfig;
            _CameraDisplay = CameraDisplayManager.GetDisplay(_Params.FovInfo, _Params.DisplayType);
            _CsvManager = new CsvManager();

            SetCameraAgent();
            SetObservable();
            InitGrabImageBuffer();

            return SequenceAction.Ok();
        }

        private void SetCameraAgent() {
            _CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
        }

        private void SetObservable() {
            if (_IsObservableSet) {
                return;
            }

            if (_FovRecipe != null) {
                VisionEnv.AuroraEnv.EventObservable.AsObservable(VisionEventId.UPDATE_GRAB_IMAGE_BUFFER).
                    Where(x => x.From == _FovRecipe.CamRecipe.CamId.ToString()).Subscribe(_ => UpdateGrabImageBuffer());
                _IsObservableSet = true;
            }
        }

        // 사용하는 곳에서 그랩 버퍼 할당, 디바이스에서는 복사본 전달
        private void InitGrabImageBuffer() {
            if (_GrabImage == null) {
                _CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
                _GrabImage = _CameraAgent.CloneImageBuffer();
            }
            else {
                ImageSize camImageSize = _CameraAgent.GetImageSize();
                ImageSize grabImageSize = IpAdapter.Instance.IpEnvironment.GetImageSize(_GrabImage);

                bool isSameSize = IpAdapter.Instance.IpEnvironment.ImageSizeEquals(camImageSize, grabImageSize);
                if (!isSameSize) {
                    UpdateGrabImageBuffer();
                }
            }
        }

        // 디바이스 연결시, 이미지 회전이 발생했다면 이미지버퍼 재할당
        private void UpdateGrabImageBuffer() {
            if (_FovRecipe != null) {
                IpAdapter.Instance.IpEnvironment.FreeImage(_GrabImage);
                _GrabImage = null;
                _CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
                _GrabImage = _CameraAgent.CloneImageBuffer();
            }
        }

        private SequenceActionResult MoveTeachingPos(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Teaching);

            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

        private string CheckFocusMode(
            SequenceContext context,
            string id,
            ICollection<string> branches,
            SequenceActionResult param) {

            switch (_FocusMode) {
                case eFocusMode.Roughly:
                    _MoveBackStepValue = _Params.AutoFocusRecipe.BackStep_1;
                    _MoveStepValue = _Params.AutoFocusRecipe.MoveStep_1;
                    _MoveStepMaxCnt = _Params.AutoFocusRecipe.MaxStepCnt_1;
                    return SEQ_ID_CLEAR;
                case eFocusMode.Delicately:
                    _MoveBackStepValue = _Params.AutoFocusRecipe.BackStep_2;
                    _MoveStepValue = _Params.AutoFocusRecipe.MoveStep_2;
                    _MoveStepMaxCnt = _Params.AutoFocusRecipe.MaxStepCnt_2;
                    return SEQ_ID_CLEAR;
            }

            return SEQ_ID_FINAL_GRAB;
        }

        private SequenceActionResult Clear(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            InitParams(context);
            AutoFocusLogList.Clear();

            return SequenceAction.Ok();
        }

        private SequenceActionResult MoveBackStep(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            // Auto Focus 시작 위치로 이동. 현재 위치에서 1.5mm 뒤로 이동
            var moveDelta = new Position();
            switch (_Params.AutoFocusRecipe.Axis) {
                case eAxis.X:
                    moveDelta.X = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
                    break;
                case eAxis.Y:
                    moveDelta.Y = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
                    break;
                case eAxis.Z:
                    moveDelta.Z = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
                    break;
            }

            var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

        private SequenceActionResult MoveStep(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            UpdateParams(context);

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            var moveDelta = new Position();
            switch (_Params.AutoFocusRecipe.Axis) {
                case eAxis.X:
                    moveDelta.X = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
                    break;
                case eAxis.Y:
                    moveDelta.Y = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
                    break;
                case eAxis.Z:
                    moveDelta.Z = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
                    break;
            }

            var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

        private SequenceActionResult Grab(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            Thread.Sleep(150);  // 그랩 딜레이

            int result = E1.GENERIC;

            if (_GrabImage != null) {
                result = _CameraAgent.GrabSync(_CameraDisplay, ref _GrabImage);
            }
            else {
                LOG.FuncE("_GrabImage is null.");
            }

            if (result.Equals(E1.OK)) {
                return SequenceAction.Ok(_GrabImage);
            }
            else {
                return SequenceAction.Error();
            }
        }

        private SequenceActionResult SaveImage(
            SequenceContext context,
            string id,
            SequenceActionResult param) {   // Param : GrabImage

            object grabImage = param.Param;

            if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
                _ImageManager.SaveImageInternal(_Params.FovInfo, eSaveDataType.AutoFocus, grabImage);
            }

            return SequenceAction.Ok(grabImage);
        }

        private string MeasureSharpness(
            SequenceContext context,
            string id,
            ICollection<string> branches,
            SequenceActionResult param) {   // Param : GrabImage

            if (param.Error != E1.OK || param.Param == null) {
                return SEQ_ID_MEASURE_NG;
            }

            int currentStep = context.GetValue<int>("CurrentStep");
            if (currentStep > _MoveStepMaxCnt) {
                return SEQ_ID_MOVE_BEST_POS;
            }
            else {
                object image = param.Param;
                _CurrentSharpness = IpAdapter.Instance.IpEnvironment.GetSharpnessWithCropRoi(image, _Params.AutoFocusRecipe.SharpnessRoi);
                var autoFocusLog = new AutoFocusLog(_CurrentStep, _FocusMode, _CurrentSharpness, _CurrentPositionZ);
                var log = AutoFocusLogManager.GetAutoFocusLog(_Params.FovInfo.ToSubSysInfo(), autoFocusLog);
                AutoFocusLogList.Add(log);

                VisionEnv.AuroraEnv.EventPublisher.Publish(
                    new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.UPDATE_AUTO_FOCUS_LOG, log));

                _CsvManager.SaveCsv(_Params.FovInfo.ToSubSysInfo(), log, eSaveDataType.AutoFocus);

                LOG.FuncI(string.Format(
                    $"[{_FocusMode}][{_CurrentStep}/{_MoveStepMaxCnt}] _CurrentSharpness : {_CurrentSharpness}, _CurrentPositionZ : {_CurrentPositionZ}"));

                return SEQ_ID_MOVE_STEP;
            }
        }

        private SequenceActionResult MoveBestPos(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            // 최적의 Z축 위치 산출
            double maxSharpness = 0;
            int maxIdx = 0;
            for (int i = 0; i < AutoFocusLogList.Count; i++) {
                if (maxSharpness < AutoFocusLogList[i].Sharpness) {
                    maxSharpness = AutoFocusLogList[i].Sharpness;
                    maxIdx = i;
                }
            }

            var bestStep = AutoFocusLogList[maxIdx].Step;
            var bestSharpness = AutoFocusLogList[maxIdx].Sharpness;
            var bestPos = AutoFocusLogList[maxIdx].PosZ;
            var autoFocusResult = new SetupSeqResultParams_AutoFocus(bestStep, bestSharpness, bestPos);
            context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()), autoFocusResult);
            var autoFocusLog = new AutoFocusLog(bestStep, _FocusMode,bestSharpness, bestPos);

            if (_FocusMode == eFocusMode.Roughly) {
                //VisionEnv.AuroraEnv.EventPublisher.Publish(
                //    new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.CLEAR_AUTO_FOCUS_LOG));
                _FocusMode = eFocusMode.Delicately;
            }
            else if (_FocusMode == eFocusMode.Delicately) {
                VisionEnv.AuroraEnv.EventPublisher.Publish(
                    new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.UPDATE_AUTO_FOCUS_BEST_POS, autoFocusLog));
                _FocusMode = eFocusMode.Finished;
            }

            LOG.FuncI(string.Format($"[{_FocusMode}] BestStep : {bestStep}, BestPosZ : {bestPos}, BestSharpness : {bestSharpness}"));

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            // 최적의 Z축 위치로 돌아갈 이동량 계산
            double curPos = GetCurrentPosition();
            double backStepValue = curPos - bestPos;

            // 이동량이 4mm 이상이면 리밋 NG 처리
            if (Math.Abs(backStepValue) >= 4) {
                MessageBoxUtil.ShowMessageBoxError(
                    string.Format(StrTokens.MSG_E_BackStepValueLimitOver, backStepValue));
                return null;
            }

            var moveDelta = new Position();
            switch (_Params.AutoFocusRecipe.Axis) {
                case eAxis.X:
                    moveDelta.X = backStepValue;
                    break;
                case eAxis.Y:
                    moveDelta.Y = backStepValue;
                    break;
                case eAxis.Z:
                    moveDelta.Z = backStepValue;
                    break;
            }

            var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

        private double GetCurrentPosition() {
            Position position = EquipAdapter.Instance.TeachingMotionManager.GetPosition(_Params.FovInfo);

            switch (_Params.AutoFocusRecipe.Axis) {
                case eAxis.X:
                    return position.X;
                case eAxis.Y:
                    return position.Y;
                case eAxis.Z:
                    return position.Z;
            }

            return position.Z;
        }

        private SequenceActionResult SaveBestPos(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            var autoFocusResult = context.GetValue<SetupSeqResultParams_AutoFocus>(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()));
            if (MessageBoxUtil.ShowMessageBoxQuestion(
                string.Format(
                    StrTokens.MSG_Q_SyncPatternOriginFormat, autoFocusResult.BestPosZ)) == MessageBoxResult.Yes) {
                EquipAdapter.Instance.VisionSetup.SaveBestPos(autoFocusResult.BestPosZ);
            }

            return SequenceAction.Ok();
        }

        private SequenceActionResult MeasureNG(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            LOG.E("Fail to grab image.");

            return SequenceAction.Ok();
        }

        private SequenceActionResult MoveFinalPos(
            SequenceContext context,
            string id,
            SequenceActionResult param) {

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            // 시퀀스 종료시, 다시 안전위치로 이동
            int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Ready);
            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

        private SequenceActionResult SeqComplete(
            SequenceContext context,
            string id,
            SequenceActionResult param) {   // Param : AutoFocusResult

            var autoFocusResult = context.GetValue<SetupSeqResultParams_AutoFocus>(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()));

            return SequenceAction.Ok(autoFocusResult);
        }

        void OnStopped(
            SequenceContext context) {
            LOG.I($"{context.SequenceId} - STOP Handler");
            HandleAbnormalFinish(context);
        }

        void OnForceError(
            SequenceContext context, string id) {
            LOG.E($"{context.SequenceId} - FORCE ERROR Handler");
            HandleAbnormalFinish(context);
        }

        void HandleAbnormalFinish(
            SequenceContext context) {
        }

        private void InitParams(SequenceContext context) {
            _CurrentStep = 0;
            _CurrentPositionZ = 0;
            _CurrentSharpness = 0;

            context.PutValue("CurrentStep", _CurrentStep);
        }

        private void UpdateParams(SequenceContext context) {
            _CurrentStep++;

            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                _CurrentPositionZ = Math.Round(123.456789, 4);
            }
            else {
                // 모터 현재 위치 가져오기
                double curPos = GetCurrentPosition();
                _CurrentPositionZ = Math.Round(curPos, 4);

            }
            context.PutValue("CurrentStep", _CurrentStep);
        }
    }
}