Kyungshin-Welder/AuroraVision/VisionSequence/Pattern/GrabFindWithFixtureSeq.cs

/*
  * Copyright (c) 2025 LG Electronics Inc.
   * SPDX-License-Identifier: LicenseRef-LGE-Proprietary
   */
using Aurora.Api;
using VisionCalibrator;
using VisionCommon;
using System.Collections.Generic;
using VisionSequence.Camera;
using System.Linq;
using System;

namespace VisionSequence.Pattern {
    /// <summary>
    /// 카메라 그랩, 픽스처와 메인 패턴 검출 시퀀스 파라미터
    /// </summary>
    public class GrabFindWithFixtureSeqParams : GrabFindProcSeqParams {
        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="fovInfo">FOV 키 정보</param>
        /// <param name="patternInfo">패턴 정보</param>
        /// <param name="displayType">메인 화면 타입</param>
        /// <param name="seqMode">시퀀스 모드</param>
        public GrabFindWithFixtureSeqParams(
            FovInfo fovInfo,
            PatternInfo patternInfo,
            eDisplayType displayType = eDisplayType.Main_1,
            eSeqMode seqMode = eSeqMode.Auto) : base(fovInfo, patternInfo, displayType, seqMode) {
        }
    }

    /// <summary>
    /// 카메라 그랩, 픽스처와 메인 패턴 검출 시퀀스
    /// </summary>
    public class GrabFindWithFixtureSeq : SubSeqBase {
        private readonly Logger _Logger;
        private TactTimeChecker _TactTimeChecker;
        private GrabFindWithFixtureSeqParams _Params;
        private FovRecipe _FovRecipe;
        private PatternAgentBase _PatternAgent;
        private PatternInfo _FixturePatternInfo;
        private Calibrator _Calibrator;
        private CameraAgentBase _CameraAgent;
        private ICameraDisplay _CameraDisplay;
        private ImageManager _ImageManager = new ImageManager();

        private string _SeqName;

        // Get Context Key Values - Align에서 사용 할 때만 적용
        private ProcParams _ProcParams;
        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";

        private const string SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS = "SET_GRAB_FIXTURE_SEQ_PARAMS";
        private const string SEQ_ID_GRAB_FIXTURE_SEQ = "GRAB_FIXTURE_SEQ";
        private const string SEQ_ID_FIND_FIXTURE = "FIND_FIXTURE";
        private const string SEQ_ID_CHECK_FIXTURE_FOUND = "CHECK_FIXTURE_FOUND";
        private const string SEQ_ID_FIXTURE_FORCE_OK = "FIXTURE_FORCE_OK";

        private const string SEQ_ID_CHECK_SAME_LIGHT = "CHECK_SAME_LIGHT";

        private const string SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS = "SET_GRAB_MAIN_SEQ_PARAMS";
        private const string SEQ_ID_GRAB_MAIN_SEQ = "GRAB_MAIN_SEQ";
        private const string SEQ_ID_FIND_MAIN_WITH_FIXTURE = "FIND_MAIN_WITH_FIXTURE";
        private const string SEQ_ID_CHECK_MAIN_FOUND = "CHECK_MAIN_FOUND";
        private const string SEQ_ID_LIGHT_RETRY = "LIGHT_RETRY";
        private const string SEQ_ID_MAIN_FORCE_OK = "MAIN_FORCE_OK";

        private const string SEQ_ID_UPDATE_UI = "UPDATE_UI";
        private const string SEQ_ID_SAVE_IMAGE = "SAVE_IMAGE";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        #endregion

        /// <summary>
        /// 생성자
        /// </summary>
        public GrabFindWithFixtureSeq() {
            _Logger = new Logger($"{GetType().Name}");
        }

        private void SetActions(SequenceBuilder sb) {
            var grabSeqBuilder_Fixture = SubSeqManager.CreateSubSeqBuilder(new GrabSeq(), "GrabSeq_Fixture");
            var grabSeqBuilder_Main = SubSeqManager.CreateSubSeqBuilder(new GrabSeq(), "GrabSeq_Main");

            sb.AddAction(SEQ_ID_INIT, Init);

            sb.AddAction(SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS, SetGrabFixtureSeqParams);
            sb.AddSubsequence(SEQ_ID_GRAB_FIXTURE_SEQ, grabSeqBuilder_Fixture);
            sb.AddAction(SEQ_ID_FIND_FIXTURE, FindFixture);
            sb.AddDecision(SEQ_ID_CHECK_FIXTURE_FOUND, CheckFixtureFound);
            sb.AddAction(SEQ_ID_FIXTURE_FORCE_OK, ForceOK);

            sb.AddDecision(SEQ_ID_CHECK_SAME_LIGHT, CheckSameLight);
            sb.AddAction(SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS, SetGrabMainSeqParams);
            sb.AddSubsequence(SEQ_ID_GRAB_MAIN_SEQ, grabSeqBuilder_Main);

            sb.AddAction(SEQ_ID_FIND_MAIN_WITH_FIXTURE, FindMainWithFixture);
            sb.AddDecision(SEQ_ID_CHECK_MAIN_FOUND, CheckMainFound);
            sb.AddDecision(SEQ_ID_LIGHT_RETRY, LightRetry);
            sb.AddAction(SEQ_ID_MAIN_FORCE_OK, ForceOK);

            sb.AddAction(SEQ_ID_UPDATE_UI, UpdateUI);
            sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
            sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
        }

        public override SequenceBuilder Builder(string seqName) {
            _SeqName = seqName;
            _TactTimeChecker = new TactTimeChecker(_SeqName);

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

            sb.Flows(
                  sb.Init
                > sb[SEQ_ID_INIT]
                > sb[SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS]
                > sb[SEQ_ID_GRAB_FIXTURE_SEQ]
                > sb[SEQ_ID_FIND_FIXTURE]
                > sb[SEQ_ID_CHECK_FIXTURE_FOUND]
                > sb[SEQ_ID_CHECK_SAME_LIGHT]

                > sb[SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS]
                > sb[SEQ_ID_GRAB_MAIN_SEQ]
                > sb[SEQ_ID_FIND_MAIN_WITH_FIXTURE]
                > sb[SEQ_ID_CHECK_MAIN_FOUND]

                > sb[SEQ_ID_UPDATE_UI]
                > sb[SEQ_ID_SAVE_IMAGE]
                > sb[SEQ_ID_SEQ_COMPLETE]
                > sb.Final
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_FIXTURE_FOUND] // Fixture Not Found
                > sb[SEQ_ID_FIXTURE_FORCE_OK]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_SAME_LIGHT] // Same Light : Skip Grab Main
                > sb[SEQ_ID_FIND_MAIN_WITH_FIXTURE]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_MAIN_FOUND]   // Light Retry
                > sb[SEQ_ID_LIGHT_RETRY]
                > sb[SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS]
                );
            sb.Flows(
                  sb[SEQ_ID_LIGHT_RETRY]        // Light Retry 모두 실패
                > sb[SEQ_ID_MAIN_FORCE_OK]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_MAIN_FOUND] // Main Not Found
                > sb[SEQ_ID_MAIN_FORCE_OK]
                );
            sb.Flows(
                  sb[SEQ_ID_FIXTURE_FORCE_OK]
                > sb[SEQ_ID_UPDATE_UI]
                );
            sb.Flows(
                  sb[SEQ_ID_MAIN_FORCE_OK]
                > sb[SEQ_ID_UPDATE_UI]
                );

#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 GrabFindWithFixtureSeqParams;
                context.TryGetValue<ProcParams>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_PROC_PARAMS, _Params.FovInfo.ToSubSysInfo()), out _ProcParams);
            }
            else {
                _Logger.E($"_Params is null.");
                return null;
            }

            _FovRecipe = VisionRecipeManager.GetFovRecipe(_Params.FovInfo);

            SetLightRetry();
            SetPatternAgent();
            SetCalibrator();
            SetCameraAgent();
            SetDisplay();

            return SequenceAction.Ok();
        }

        private void SetPatternAgent() {
            var patternIndex = _Params.PatternInfo.GetPatternIndex();
            _PatternAgent = PatternAgentManager.GetAgent(_Params.FovInfo);

            var fixturePatternIndex = FixtureSetting.GetFixturePatternIndex(
                _Params.PatternInfo.GetPatternIndex(), _Params.PatternInfo.FixtureSetting.Pattern1);

            _FixturePatternInfo = _PatternAgent.GetPatternInfo(fixturePatternIndex);
        }

        private void SetCalibrator() {
            _Calibrator = CalibratorManager.GetCalibrator(_Params.FovInfo);
        }

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

        private void SetDisplay() {
            _CameraDisplay = CameraDisplayManager.GetDisplay(_Params.FovInfo, _Params.DisplayType);
        }

        private SequenceActionResult SetGrabFixtureSeqParams(SequenceContext context, string id, SequenceActionResult param) {
            return SequenceAction.Ok(
                new GrabSeqParams(_Params.FovInfo, _FixturePatternInfo, _Params.DisplayType, _Params.SeqMode));
        }

        private SequenceActionResult FindFixture(SequenceContext context, string id, SequenceActionResult param) {
            var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
            var fixturePatternResult = _PatternAgent.Find(_FixturePatternInfo, grabImage, _CameraDisplay);
            _PatternAgent.Draw(_FixturePatternInfo, fixturePatternResult, _CameraDisplay, isFixturePattern: true);
            GrabFindSeqUtil.SetFixturePatternResult(_Params.FovInfo, fixturePatternResult);

            return SequenceAction.Ok();
        }

        private string CheckFixtureFound(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            var fixturePatternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);

            if (fixturePatternResult.Status == eFoundStatus.FoundOK) {
                return SEQ_ID_CHECK_SAME_LIGHT;
            }
            else {
                return SEQ_ID_FIXTURE_FORCE_OK;
            }
        }

        private string CheckSameLight(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            if (GrabFindSeqUtil.IsPatternAndLastLightSame(_Params.FovInfo, _Params.PatternInfo)) {
                return SEQ_ID_FIND_MAIN_WITH_FIXTURE;
            }
            else {
                return SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS;
            }
        }

        private SequenceActionResult SetGrabMainSeqParams(SequenceContext context, string id, SequenceActionResult param) {
            return SequenceAction.Ok(
                new GrabSeqParams(_Params.FovInfo, _Params.PatternInfo, _Params.DisplayType, _Params.SeqMode));
        }

        private SequenceActionResult FindMainWithFixture(SequenceContext context, string id, SequenceActionResult param) {
            var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
            var fixturePatternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);
            var fixtureResult = fixturePatternResult.MoreInfo as FixtureResult;

            _CameraDisplay.SetImage(grabImage);     // Fixture Enabled && Same Light 조건에서 grabImage를 새로 Set해준다.

            PatternResult patternResult = null;             // Single Pattern
            List<PatternResult> patternResults = null;      // Multi Pattern

            // Multi Pattern
            if (PatternConstants.IsMultiPatternType(_Params.PatternInfo.PatternType)) {
                patternResults = GrabFindSeqUtil.FindMulti(
                    _Params.FovInfo, _Params.PatternInfo, _PatternAgent, _Calibrator, _CameraAgent, _CameraDisplay, grabImage, fixtureResult);
                patternResult = patternResults[0];  // Master Pattern 결과

                context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULTS, _Params.FovInfo.GetKey()), patternResults);
            }
            // Single Pattern
            else {
                patternResult = _PatternAgent.Find(_Params.PatternInfo, grabImage, _CameraDisplay, fixtureResult);
                GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
                _PatternAgent.Draw(_Params.PatternInfo, patternResult, _CameraDisplay);
                //_CameraDisplay.DrawRefCross();
            }

            patternResult.PatternName = _Params.PatternInfo.PatternName;
            patternResult.PatternType = _Params.PatternInfo.PatternType;
            patternResult.FovId = _Params.FovInfo.FovId;

            context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()), patternResult);

            return SequenceAction.Ok();
        }

        private string CheckMainFound(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));

            if (mainPatternResult.Status == eFoundStatus.FoundOK) {
                return SEQ_ID_UPDATE_UI;
            }
            else {
                if (_ProcParams != null && _ProcParams.LightRetryParams.LightRetryEnabled)
                {
                    return SEQ_ID_LIGHT_RETRY;
                }
                else {
                    return SEQ_ID_MAIN_FORCE_OK;
                }
            }
        }

        private string LightRetry(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            if (_OriginalLightInfo == null) {
                _OriginalLightInfo = _Params.PatternInfo.LightInfo.Clone();
            }

            _CurLightRetryCnt++;
            if (_CurLightRetryCnt <= _ProcParams.LightRetryParams.LightRetryCount)
            {
                var lightParams = _Params.PatternInfo.LightInfo.LightParams;
                var lightLevelMsg = string.Empty;
                for (int i = 0; i < lightParams.Count; i++)
                {
                    var lightParam = lightParams[i];
                    lightParam.Level = IsLightRetryStepMode(_ProcParams.LightRetryParams.LightRetryMode) ?
                        Convert.ToInt16(GetLightRetryValueStepMode(_CurLightRetryCnt, lightParam.Level, _ProcParams.LightRetryParams)) :
                        Convert.ToInt16(GetLightRetryValueRatioMode(_CurLightRetryCnt, lightParam.Level, _OriginalLightInfo.LightParams[i].Level, _ProcParams.LightRetryParams));
                    lightLevelMsg += $"[{i} | {lightParam.Level}] ";
                }
                PrintLogAndShowToast(_Logger,
                    $"[{_Params.FovInfo.SubSysName}][{_Params.FovInfo.FovName}] " +
                    $"Grab retried at light step {_ProcParams.LightRetryParams.LightRetryStep}." +
                    $"Mode is {_ProcParams.LightRetryParams.LightRetryMode.ToString()}. " +
                    $"{lightLevelMsg} [{_CurLightRetryCnt}/{_ProcParams.LightRetryParams.LightRetryCount}]");

                return SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS;     // 조명 Retry 시도
            }

            _Params.PatternInfo.LightInfo = _OriginalLightInfo.Clone();     // 기존 조명 원복

            return SEQ_ID_MAIN_FORCE_OK;     // 조명 Retry 모두 실패시, 강제 OK 시도
        }

        private SequenceActionResult ForceOK(SequenceContext context, string id, SequenceActionResult param) {
            if (_ProcParams != null && _ProcParams.ForceOkEnabled &&
                PatternConstants.IsForceOkSupport(_Params.PatternInfo.PatternType)) {

                bool isMainPattern = true;
                PatternResult patternResult;

                if (id == SEQ_ID_FIXTURE_FORCE_OK) {
                    isMainPattern = false;
                }

                if (isMainPattern) {
                    patternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
                }
                else {
                    patternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);
                }

                patternResult = GrabFindSeqUtil.DoForceOk(new ForceOkReqParam(
                    _Params.FovInfo,
                    patternResult.PatternIndex,
                    context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey())),
                    patternResult));

                if (patternResult.Status == eFoundStatus.FoundOK || patternResult.Status == eFoundStatus.ForceOK) {
                    _CameraDisplay.ClearMeasureGraphics();
                    GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
                    DrawForceOkResult(patternResult);

                    if (isMainPattern) {
                        context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()), patternResult);
                    }
                    else {
                        GrabFindSeqUtil.SetFixturePatternResult(_Params.FovInfo, patternResult);
                    }
                }
            }

            return SequenceAction.Ok();
        }

        private void DrawForceOkResult(PatternResult patternResult) {
            _PatternAgent.Draw(_Params.PatternInfo, patternResult, _CameraDisplay, isFixturePattern: false);
            var result = patternResult.Results.ToList()[0];
            eColor color = eColor.DarkRed;
            _CameraDisplay.AddCross(new Cross(result.ImagePosition, 80), color);
        }

        private SequenceActionResult UpdateUI(SequenceContext context, string id, SequenceActionResult param) {
            var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
            VisionEnv.AuroraEnv.EventPublisher.Publish(new EventMessage(
                _Params.FovInfo.GetKey(), VisionEventId.UPDATE_PATTERN_RESULT, mainPatternResult));

            return SequenceAction.Ok();
        }

        private SequenceActionResult SaveImage(SequenceContext context, string id, SequenceActionResult param) {
            var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
            var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
            var materialInfo = context.GetValue<MaterialInfo>(VisionContextKeys.CNTXT_KEY_MATERIAL_INFO);

            if (mainPatternResult.Status == eFoundStatus.FoundOK) {
                if (VisionSettingManager.VisionSetting.SaveOkImage == false) {
                    return SequenceAction.Ok();
                }
            }
            _ImageManager.SaveImage(_Params.FovInfo, grabImage, mainPatternResult.Status.ToString(), materialInfo);

            return SequenceAction.Ok();
        }

        private SequenceActionResult SeqComplete(SequenceContext context, string id, SequenceActionResult param) {
            var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
            _TactTimeChecker.Stop(TactTimeType.ProcSeq, "Light/Grab/Find");

            return SequenceAction.Ok(mainPatternResult);
        }

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

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

        void HandleAbnormalFinish(
            SequenceContext context) {
            _TactTimeChecker.Stop(TactTimeType.ProcSeq, "Light/Grab/Find");
            if(_Params!=null && _OriginalLightInfo !=null) _Params.PatternInfo.LightInfo = _OriginalLightInfo.Clone();     // 기존 조명 원복 ( 방어코드 )
        }
    }
}