using Aurora.Api;
using Aurora.Api.Devices;
using VisionAligner;
using VisionCalibrator;
using VisionCommon;
using Hermes;
using System;
using System.Collections.Generic;
using System.Threading;
using VisionSequence.Pattern;
using VisionCommonUI;
using VisionCommonUI.Resources;
using System.Linq;
using VisionDirector;

namespace VisionSequence.Setup {
    /// <summary>
    /// 센터 튜닝 셋업 시퀀스 요청 파라미터
    /// </summary>
    public class SetupSeqParams_CenterTuning {
        /// <summary>서브 시스템 정보</summary>
        public SubSysInfo SubSysInfo { get; set; }
        /// <summary>센터 튜닝 레시피</summary>
        public CenterTuningRecipe CenterTuningRecipe { get; set; }
        /// <summary>메인 화면 타입</summary>
        public eDisplayType DisplayType { get; set; }

        /// <summary>얼라인 델타를 로그 뷰에 출력하기 위한 콜백</summary>
        public Action<Position> CallBackAlignDelta;

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="subSysInfo">서브 시스템 정보</param>
        /// <param name="centerTuningRecipe">센터 튜닝 레시피</param>
        /// <param name="displayType">메인 화면 타입</param>
        public SetupSeqParams_CenterTuning(
            SubSysInfo subSysInfo,
            CenterTuningRecipe centerTuningRecipe,
            eDisplayType displayType = eDisplayType.CenterTuning) {
            this.SubSysInfo = subSysInfo;
            this.CenterTuningRecipe = centerTuningRecipe;
            this.DisplayType = displayType;
        }
    }

    /// <summary>
    /// 반복 얼라인을 통해 화면 중심에 패턴이 오도록 튜닝
    /// </summary>
    public class SetupSeq_CenterTuning {
        private readonly Logger LOG = new Logger(typeof(SetupSeq_CenterTuning).Name);
        private SetupSeqParams_CenterTuning _Params;
        private ICameraDisplay _CameraDisplay;
        private SubSysRecipe _SubSysRecipe;
        private FovInfo _FovInfo;
        private FovRecipe _FovRecipe;
        private PatternAgentBase _PatternAgent;
        private ImageManager _ImageManager = new ImageManager();

        private int _CurrentCnt = 0;

        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";
        private const string SEQ_ID_MOVE_READY_POS = "MOVE_READY_POS";
        private const string SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS = "SET_GRAB_FIND_PROC_SEQ_PARAMS";
        private const string SEQ_ID_GRAB_FIND_PROC_SEQ = "GRAB_FIND_PROC_SEQ";
        private const string SEQ_ID_SAVE_IMAGE = "SEQ_ID_SAVE_IMAGE";
        private const string SEQ_ID_CHECK_FOUND_STATUS = "CHECK_FOUND_STATUS";
        private const string SEQ_ID_ALIGN = "ALIGN";
        private const string SEQ_ID_MOVE_CENTER = "MOVE_CENTER";
        private const string SEQ_ID_CHECK_INRANGE = "CHECK_INRANGE";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        #endregion

        private void SetActions(SequenceBuilder sb) {
            SequenceBuilder grabFindProcSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindProcSeq(), "GrabFindProcSeq");

            sb.AddAction(SEQ_ID_INIT, Init);
            sb.AddAction(SEQ_ID_MOVE_READY_POS, MoveReadyPos);
            sb.AddAction(SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS, SetGrabFindProcSeqParams);
            sb.AddSubsequence(SEQ_ID_GRAB_FIND_PROC_SEQ, grabFindProcSeqBuilder);
            sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
            sb.AddDecision(SEQ_ID_CHECK_FOUND_STATUS, CheckFoundStatus);
            sb.AddAction(SEQ_ID_ALIGN, Align);
            sb.AddDecision(SEQ_ID_CHECK_INRANGE, CheckInrange);
            sb.AddAction(SEQ_ID_MOVE_CENTER, MoveCenter);
            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_READY_POS]
                > sb[SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS]
                > sb[SEQ_ID_GRAB_FIND_PROC_SEQ]
                > sb[SEQ_ID_SAVE_IMAGE]
                > sb[SEQ_ID_CHECK_FOUND_STATUS]
                > sb[SEQ_ID_ALIGN]
                > sb[SEQ_ID_CHECK_INRANGE]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_FOUND_STATUS]
                > sb[SEQ_ID_SEQ_COMPLETE]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_INRANGE]
                > sb[SEQ_ID_MOVE_CENTER]
                > sb[SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS]
                );

            sb.Flows(
                  sb[SEQ_ID_CHECK_INRANGE]
                > sb[SEQ_ID_SEQ_COMPLETE]
                > sb.Final
                );

#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_CenterTuning;
            }
            else {
                LOG.E($"Param is null.");
                return null;
            }

            _CurrentCnt = 0;
            _FovInfo = new FovInfo(_Params.SubSysInfo, eFovId.Fov1, eFovId.Fov1.ToString());
            _SubSysRecipe = VisionRecipeManager.GetSubSysRecipe(_FovInfo.UnitId, _FovInfo.SubSysId);
            _FovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfo);
            _PatternAgent = FovPatternAgentManager.GetAgent(_FovInfo.ToSubSysInfo());
            _CameraDisplay = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);

            GrabFindSeqUtil.Clear(_Params.SubSysInfo);

            return SequenceAction.Ok();
        }

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

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

            // 하나의 서브시스템에 Fov가 4개라면? 어떤 FovId를 넘겨야 하는가? (4점 얼라인)
            int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_FovInfo, ePositionType.Ready);
            if (res == E1.OK) {
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

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

            var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, "Inspect", eProcPatternId.ProcPattern1);
            if(!procPatternInfo.IsFovPattern) {
                _PatternAgent = CamPatternAgentManager.GetAgent(_FovRecipe.CamRecipe.CamId);
            }
            var patternInfo = _PatternAgent.GetPatternInfo(_FovInfo, procPatternInfo.PatternId);
            patternInfo.PatternName = procPatternInfo.PatternName;
            eSeqMode seqMode = eSeqMode.Manual;     // 그랩을 Skip하지 않기 위함.

            return SequenceAction.Ok(new GrabFindProcSeqParams(_FovInfo, patternInfo, _Params.DisplayType, seqMode));
        }

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

            if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
                var display = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);
                object image = display.GetImageObject();

                if (image != null) {
                    _ImageManager.SaveImageInternal(_FovInfo, eSaveDataType.CenterTuning, image);
                }
            }

            var patternResult = param.Param as PatternResult;

            return SequenceAction.Ok(patternResult);
        }

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

            var patternResult = param.Param as PatternResult;

            if (patternResult.Status == eFoundStatus.FoundOK) {
                return SEQ_ID_ALIGN;
            }
            else {
                var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
                context.PutValue<AlignResult>(
                    string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()), new AlignResult() {
                        AlignName = patternResult.PatternName,
                        Status = AlignStatus.NOT_FOUND,
                        AlignAlgorithm = _SubSysRecipe.AlignRecipes[Convert.ToInt32(alignInfo.AlignId)].ProcParams.AlignAlgorithm
                    });
                return SEQ_ID_SEQ_COMPLETE;
            }
        }

        // Align Params
        private SequenceActionResult Align(
            SequenceContext context,
            string id,
            SequenceActionResult param) {   // param : PatternResult

            var patternResult = param.Param as PatternResult;

            var refPoint = _FovRecipe.CalFovRecipe.FovCenter + _FovRecipe.RefPointRecipe.Offset;

            var meaPoint = patternResult.Results.ToList()[0].MaterialPosition.Clone();
            var meaAngle = patternResult.Results.ToList()[0].Angle.Clone();

            // Align
            var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
            var aligner = AlignAgentManager.GetAlignAgent(alignInfo);
            var calibrator = CalibratorManager.GetCalibrator(new FovInfo(_Params.SubSysInfo, eFovId.Fov1));
            var alignResult = aligner.Align(meaPoint, refPoint, (Angle)meaAngle, (int)calibrator.GetFlipY());

            if (alignResult == null) {
                MessageBoxUtil.ShowMessageBoxError(
                    string.Format(StrTokens.MSG_E_AlignResultIsNull));
                return SequenceAction.Error();
            }
            else {
                context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()), alignResult);
            }

            return SequenceAction.Ok(alignResult);
        }

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

            _CurrentCnt++;
            if (_CurrentCnt > _Params.CenterTuningRecipe.MaxCnt) {
                return SEQ_ID_SEQ_COMPLETE;
            }

            //var alignResult = param.Param as AlignResult;
            var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
            var alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()));
            var alignDelta = new Position() { X = alignResult.Delta.X, Y = alignResult.Delta.Y, T = alignResult.Delta.T.Degree };
            _Params.CallBackAlignDelta(alignDelta);

            if (_Params.CenterTuningRecipe.Spec.T == 0) {

                if (Math.Abs(_Params.CenterTuningRecipe.Spec.X) > Math.Abs(alignResult.Delta.X) &&
                    Math.Abs(_Params.CenterTuningRecipe.Spec.Y) > Math.Abs(alignResult.Delta.Y)) {
                    return SEQ_ID_SEQ_COMPLETE;
                }
                else {
                    return SEQ_ID_MOVE_CENTER;
                }
            }
            else {
                if (Math.Abs(_Params.CenterTuningRecipe.Spec.X) > Math.Abs(alignResult.Delta.X) &&
                    Math.Abs(_Params.CenterTuningRecipe.Spec.Y) > Math.Abs(alignResult.Delta.Y) &&
                    Math.Abs(_Params.CenterTuningRecipe.Spec.T) > Math.Abs(alignResult.Delta.T.Degree)) {
                    return SEQ_ID_SEQ_COMPLETE;
                }
                else {
                    return SEQ_ID_MOVE_CENTER;
                }
            }
        }

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

            var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
            var alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()));

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

            var delta = new Position() { X = alignResult.Delta.X, Y = alignResult.Delta.Y };
            if (_Params.CenterTuningRecipe.Spec.T != 0) {
                delta.T = alignResult.Delta.T.Degree;
            }

            var teachingDirector = TeachingDirectorManager.Instance.GetDirector(_FovInfo.ToSubSysInfo());
            var motionDelta = teachingDirector.ApplyDirection(delta);

            var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_FovInfo, motionDelta);
            if (res != E1.OK) {
                return SequenceAction.Error(E2.ABORTED);
            }

            return SequenceAction.Ok();
        }

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

            if (_CurrentCnt > _Params.CenterTuningRecipe.MaxCnt) {
                return SequenceAction.Ok(_Params.CenterTuningRecipe.MaxCnt);
            }
            else {
                return SequenceAction.Ok(_CurrentCnt);
            }
        }

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

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

        void HandleAbnormalFinish(
            SequenceContext context) {
        }
    }
}