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

namespace VisionSequence.Aligner {
    /// <summary>
    /// C1_FN_MV 얼라인 시퀀스 파라미터
    /// </summary>
    public class AlignSeqParams_C1_FN_MV : SeqParamsBase {
        /// <summary>얼라인 키 정보</summary>
        public AlignInfo AlignInfo { get; set; }
        /// <summary>FOV별로 이동시킬 절대위치</summary>
        public Dictionary<eFovId, Position> FovPositions { get; set; }
        /// <summary>메인 화면 타입</summary>
        public eDisplayType DisplayType { get; set; }

        /// <summary>
        /// C1_FN_MV 얼라인 시퀀스 파라미터를 얻어온다.
        /// </summary>
        /// <param name="alignInfo">얼라인 키 정보</param>
        /// <param name="fovPositions">FOV별로 이동시킬 절대위치. null일 경우, 자동으로 Adapter에 정의된 티칭위치로 이동</param>
        /// <param name="displayType">메인 화면 타입</param>
        /// <param name="seqMode">시퀀스 모드</param>
        public AlignSeqParams_C1_FN_MV(
            AlignInfo alignInfo,
            Dictionary<eFovId, Position> fovPositions = null,
            eDisplayType displayType = eDisplayType.Main_1,
            eSeqMode seqMode = eSeqMode.Auto) : base(seqMode) {

            this.AlignInfo = alignInfo;
            this.FovPositions = fovPositions;
            this.DisplayType = displayType;
        }
    }

    /// <summary>
    /// CAM 1대 / FOV N개 (2 <= N <= 4) / 포지션 이동 (O) / 2 Point Align / 2 MeasurePatterns to 2 ReferencePatterns
    /// </summary>
    public class AlignSeq_C1_FN_MV : SubSeqBase {
        private const int MEASURE_MAX_CNT = 2;      // 얼라인을 수행하기 위해 찾아야 하는 패턴의 수

        private readonly Logger LOG = new Logger(typeof(AlignSeq_C1_FN_MV).Name);

        private AlignSeqParams_C1_FN_MV _Params;
        private AlignRecipe _AlignRecipe;
        private SubSysInfo _SubSysInfo;
        private VisionAligner.Aligner _Aligner;
        private Calibrator _Calibrator;
        private PatternAgentBase _PatternAgent;

        private Dictionary<eFovId, FovInfo> _FovInfos = new Dictionary<eFovId, FovInfo>();
        private Dictionary<eFovId, BaseResult> _MeasureResults = new Dictionary<eFovId, BaseResult>();
        private Dictionary<eFovId, BaseResult> _ReferenceResults = new Dictionary<eFovId, BaseResult>();

        private eFovId _CurFovId = eFovId.Fov1;
        private string _SeqName;
        private int _FovNum = 0;
        private bool _IsForceOk = false;

        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";
        private const string SEQ_ID_CLEAR = "CLEAR";

        private const string SEQ_ID_FORK = "FORK";
        private const string SEQ_ID_MOVE_FOV_POS = "MOVE_FOV_POS";
        private const string SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS = "SET_FIND_MEASURE_SEQ_PARAMS";
        private const string SEQ_ID_FIND_MEASURE_SEQ = "FIND_MEASURE_SEQ";
        private const string SEQ_ID_CHECK_MEASURE_FOUND = "CHECK_MEASURE_FOUND";
        private const string SEQ_ID_MARK_NG = "MARK_NG";
        private const string SEQ_ID_SET_MEASURE = "SET_MEASURE";
        private const string SEQ_ID_CHECK_MEASURE_FINISHED = "CHECK_MEASURE_FINISHED";

        private const string SEQ_ID_SET_REFERENCE = "SET_REFERENCE";
        private const string SEQ_ID_JOIN = "JOIN";

        private const string SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED = "CHECK_PATTERN_DISTANCE_INSP_ENABLED";
        private const string SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS = "SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS";
        private const string SEQ_ID_PATTERN_DISTANCE_INSP_SEQ = "PATTERN_DISTANCE_INSP_SEQ";
        private const string SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS = "CHECK_PATTERN_DISTANCE_INSP";
        private const string SEQ_ID_DISTANCE_NG = "DISTANCE_NG";

        private const string SEQ_ID_ALIGN = "ALIGN";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        #endregion

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

        private void SetActions(SequenceBuilder sb) {
            SequenceBuilder grabFindProcSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindProcSeq(), typeof(GrabFindProcSeq).Name);
            SequenceBuilder patternDistanceInspSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new InspSeq_PatternDistance(), typeof(InspSeq_PatternDistance).Name);

            sb.AddAction(SEQ_ID_INIT, Init);
            sb.AddAction(SEQ_ID_CLEAR, Clear);

            sb.AddFork(SEQ_ID_FORK);
            sb.AddAction(SEQ_ID_MOVE_FOV_POS, MoveFovPos);
            sb.AddAction(SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS, SetFindMeasureSeqParams);
            sb.AddSubsequence(SEQ_ID_FIND_MEASURE_SEQ, grabFindProcSeqBuilder);
            sb.AddDecision(SEQ_ID_CHECK_MEASURE_FOUND, CheckFound);
            sb.AddAction(SEQ_ID_MARK_NG, MarkNG);
            sb.AddAction(SEQ_ID_SET_MEASURE, SetMeasure);
            sb.AddDecision(SEQ_ID_CHECK_MEASURE_FINISHED, CheckMeasureFinished);

            sb.AddAction(SEQ_ID_SET_REFERENCE, SetReference);
            sb.AddJoin(SEQ_ID_JOIN);

            // 패턴 거리 검사
            sb.AddDecision(SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED, CheckPatternDistanceInspEnabled);
            sb.AddAction(SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS, SetPatternDistanceInspSeqParams);
            sb.AddSubsequence(SEQ_ID_PATTERN_DISTANCE_INSP_SEQ, patternDistanceInspSeqBuilder);
            sb.AddDecision(SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS, CheckPatternDistanceInspStatus);
            sb.AddAction(SEQ_ID_DISTANCE_NG, DistanceNG);

            sb.AddAction(SEQ_ID_ALIGN, Align);
            sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
        }

        public override SequenceBuilder Builder(string seqName) {
            _SeqName = 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_CLEAR]

                > sb[SEQ_ID_FORK]
                > sb[SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS]
                > sb[SEQ_ID_FIND_MEASURE_SEQ]
                > sb[SEQ_ID_CHECK_MEASURE_FOUND]
                > sb[SEQ_ID_SET_MEASURE]
                > sb[SEQ_ID_CHECK_MEASURE_FINISHED]
                > sb[SEQ_ID_JOIN]

                > sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED]    // 패턴 거리 검사
                > sb[SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS]
                > sb[SEQ_ID_PATTERN_DISTANCE_INSP_SEQ]
                > sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS]

                > sb[SEQ_ID_ALIGN]
                > sb[SEQ_ID_SEQ_COMPLETE]
                );
            sb.Flows(
                  sb[SEQ_ID_FORK]
                > sb[SEQ_ID_SET_REFERENCE]
                > sb[SEQ_ID_JOIN]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_MEASURE_FINISHED]
                > sb[SEQ_ID_MOVE_FOV_POS]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_MEASURE_FINISHED]     // Measure Mark NG -> Finish Seq
                > sb[SEQ_ID_MARK_NG]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_MEASURE_FOUND]        // Measure Mark NG -> Check Measure Finish
                > sb[SEQ_ID_CHECK_MEASURE_FINISHED]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED]    // Pattern Distance OK
                > sb[SEQ_ID_ALIGN]
                );
            sb.Flows(
                  sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS]     // Pattern Distance NG
                > sb[SEQ_ID_DISTANCE_NG]
                );
            sb.Flows(
                  sb[SEQ_ID_MARK_NG]
                > sb[SEQ_ID_SEQ_COMPLETE]
                );
            sb.Flows(
                  sb[SEQ_ID_DISTANCE_NG]
                > sb[SEQ_ID_SEQ_COMPLETE]
                );
            sb.Flows(
                  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 AlignSeqParams_C1_FN_MV;
            }
            else {
                LOG.E($"Param is null.");
                return null;
            }

            _IsForceOk = false;
            _AlignRecipe = VisionRecipeManager.GetAlignRecipe(_Params.AlignInfo);
            _SubSysInfo = _Params.AlignInfo.ToSubSysInfo();
            _PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);
            _Aligner = AlignAgentManager.GetAlignAgent(_Params.AlignInfo);

            SetFovInfos();
            _Calibrator = CalibratorManager.GetCalibrator(_FovInfos[eFovId.Fov1]);

            return SequenceAction.Ok();
        }

        private void SetFovInfos() {
            foreach (FovInfo fovInfo in VisionRecipeManager.GetFovInfos(_SubSysInfo)) {
                if (!_FovInfos.ContainsKey(fovInfo.FovId)) {
                    _FovInfos.Add(fovInfo.FovId, fovInfo);
                }
            }

            _FovNum = ProcParams.GetFovNum(_AlignRecipe.ProcParams.ProcFovType);
        }

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

            _CurFovId = eFovId.Fov1;
            _MeasureResults.Clear();
            _ReferenceResults.Clear();

            return SequenceAction.Ok();
        }

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

            var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfos[_CurFovId], _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
            var fovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfos[_CurFovId]);
            PatternIndex patternIndex = null;
            if (procPatternInfo.IsFovPattern) {
                _PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);
                patternIndex = new FovPatternIndex(_FovInfos[_CurFovId], fovRecipe.FovName, procPatternInfo.PatternId);
            }
            else {
                _PatternAgent = CamPatternAgentManager.GetAgent(fovRecipe.CamRecipe.CamId);
                patternIndex = new CamPatternIndex(fovRecipe.CamRecipe.CamId, procPatternInfo.PatternId);
            }
            PatternInfo patternInfo = _PatternAgent.GetPatternInfo(patternIndex);
            patternInfo.PatternName = procPatternInfo.PatternName;

            return SequenceAction.Ok(new GrabFindProcSeqParams(_FovInfos[_CurFovId], patternInfo, _Params.DisplayType, _Params.SeqMode));
        }

        private SequenceActionResult SetMeasure(
            SequenceContext context,
            string id,
            SequenceActionResult param) {   // Param : PatternResult

            var patternResult = param.Param as PatternResult;
            var baseResults = patternResult.Results.ToList();
            _MeasureResults.Add(_CurFovId, baseResults[0]);

            return SequenceAction.Ok();
        }

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

            if (_MeasureResults.Count == MEASURE_MAX_CNT) {
                return SEQ_ID_JOIN;
            }

            if (HasNextFov()) {
                if (IsPossibleToFindAll()) {
                    IncreaseCurFovId();
                    return SEQ_ID_MOVE_FOV_POS;
                }
            }

            return SEQ_ID_MARK_NG;
        }

        private bool HasNextFov() {
            return _CurFovId < _FovInfos.Keys.Max();
        }

        private bool IsPossibleToFindAll() {
            int lastFovIdx = (int)_FovInfos.Keys.Max();
            int curFovIdx = (int)_CurFovId;

            int possibleFoundCount = lastFovIdx - curFovIdx;
            int curFoundCount = _MeasureResults.Count;

            return MEASURE_MAX_CNT <= (possibleFoundCount + curFoundCount);
        }

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

            foreach (var fovInfoPair in _FovInfos) {
                eFovId fovId = fovInfoPair.Key;
                var refMaterialPos = VisionRecipeManager.GetRefPoint(_SubSysInfo, fovId);
                _ReferenceResults.Add(
                    fovId,
                    new BaseResult() { MaterialPosition = refMaterialPos.Clone() as StaticPoint });
            }

            return SequenceAction.Ok();
        }

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

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

            if(_CurFovId == eFovId.Fov1) {
                return SequenceAction.Ok();
            }

            int res = E1.GENERIC;
            if (_Params.FovPositions != null && _Params.FovPositions.ContainsKey(_CurFovId)) {
                // 파라미터로 넘겨받은 절대위치로 이동
                res = EquipAdapter.Instance.TeachingMotionManager.AbsMove(_FovInfos[_CurFovId], _Params.FovPositions[_CurFovId]);
            }
            else {
                // Adapter에 정의된 티칭위치로 이동
                res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_FovInfos[_CurFovId], ePositionType.Teaching);
            }

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

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

            var patternResult = param.Param as PatternResult;

            if (patternResult.Status != eFoundStatus.NotFound) {
                if (patternResult.Status == eFoundStatus.ForceOK) {
                    _IsForceOk = true;
                }
                return SEQ_ID_SET_MEASURE;
            }

            return SEQ_ID_CHECK_MEASURE_FINISHED;
        }

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

            context.PutValue<AlignResult>(
                string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()),
                new AlignResult() {
                    AlignName = _AlignRecipe.AlignName,
                    Status = AlignStatus.NOT_FOUND,
                    AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
                });

            return SequenceAction.Ok();
        }

        #region 패턴 간에 거리 검사
        private string CheckPatternDistanceInspEnabled(
            SequenceContext context,
            string id,
            ICollection<string> branches,
            SequenceActionResult param) {

            if (_AlignRecipe.InspParams.PatternDistanceParam.Enabled) {
                return SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS;
            }
            else {
                return SEQ_ID_ALIGN;
            }
        }

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

            return SequenceAction.Ok(
                new InspSeqParams_PatternDistance(_Params.AlignInfo, _MeasureResults));
        }

        private string CheckPatternDistanceInspStatus(
            SequenceContext context,
            string id,
            ICollection<string> branches,
            SequenceActionResult param) {   // Param : List<PatternDistanceResult>

            var patternDistanceResults = param.Param as List<PatternDistanceResult>;

            foreach (PatternDistanceResult result in patternDistanceResults) {
                if (result.InspStatus == InspStatus.NG) {
                    return SEQ_ID_DISTANCE_NG;
                }
            }
            return SEQ_ID_ALIGN;
        }

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

            context.PutValue<AlignResult>(
                string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()),
                new AlignResult() {
                    AlignName = _AlignRecipe.AlignName,
                    Status = AlignStatus.DISTANCE_NG,
                    AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
                });

            return SequenceAction.Ok();
        }
        #endregion

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

            var meaPoints = new List<DoublePoint>();
            var refPoints = new List<DoublePoint>();
            _MeasureResults = _MeasureResults.OrderBy(obj => obj.Key).ToDictionary(obj => obj.Key, obj => obj.Value);

            foreach (var meaResultPair in _MeasureResults) {
                meaPoints.Add((meaResultPair.Value as BaseResult).MaterialPosition);

                foreach (var refResultPair in _ReferenceResults) {
                    if (refResultPair.Key == meaResultPair.Key) {
                        refPoints.Add((refResultPair.Value as BaseResult).MaterialPosition);
                        break;
                    }
                }
            }

            var flipY = (int)_Calibrator.GetFlipY();
            var alignResult = _Aligner.Align(meaPoints, refPoints, flipY);

            if (alignResult == null) {
                return SequenceAction.Error();
            }
            else {
                context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()), alignResult);
            }

            return SequenceAction.Ok(alignResult);
        }

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

            AlignResult alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()));
            if (alignResult == null) {
                return SequenceAction.Error();
            }
            alignResult.AlignName = _AlignRecipe.AlignName;
            if (_IsForceOk && alignResult.Status == AlignStatus.OK) {
                alignResult.Status = AlignStatus.F_OK;
            }

            if (_MeasureResults.Keys.Count > 0) {
                context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_LAST_MEASURE_FOV_ID, _Params.AlignInfo.GetKey()), _MeasureResults.Keys.ToList().Max());
            }

            return SequenceAction.Ok(alignResult);
        }

        private void IncreaseCurFovId() {
            _CurFovId++;
        }

        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) {
        }
    }
}