using Aurora.Api;
using VisionCommon;
using System.Collections.Generic;
using System.Threading;
using Aurora.Api.Devices;
using Hermes;
using VisionSequence.Pattern;
using System.Linq;

namespace VisionSequence.Setup {
    /// <summary>
    /// 캘리브레이션 서브 시퀀스 요청 파라미터
    /// </summary>
    public class CalSubSeqParams {
        /// <summary>FOV 키 정보 리스트</summary>
        public List<FovInfo> FovInfos { get; set; }
        /// <summary>메인 화면 타입</summary>
        public eDisplayType DisplayType { get; set; }
        /// <summary>캘리브레이션 플랜아이템 리스트</summary>
        public List<PlanItem> PlanItems { get; set; }
        /// <summary>카메라 대수</summary>
        public int CamCnt { get; set; }
        //public Action<PlanItem> CallBack { get; set; }
    }

    /// <summary>
    /// 캘리브레이션 서브 시퀀스 결과 파라미터
    /// </summary>
    public class CalSubSeqResultParams {
        /// <summary>X축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
        public List<List<ImagePoint>> XPoints;
        /// <summary>Y축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
        public List<List<ImagePoint>> YPoints;
        /// <summary>회전 원점 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
        public List<List<ImagePoint>> ThetaPoints;

        /// <summary>X축으로 1회 이동 거리</summary>
        public List<double> LengthX;
        /// <summary>Y축으로 1회 이동 거리</summary>
        public List<double> LengthY;
        /// <summary>T축으로 1회 회전 각도(Degree)</summary>
        public List<double> LengthTheta;
        /// <summary>패턴 검출 NG 발생 플래그</summary>
        public bool IsPatternNotFound;
    }

    /// <summary>
    /// 픽셀 사이즈 및 회전 원점 계산
    /// </summary>
    public class SetupSeq_CalSub : SubSeqBase {
        private readonly Logger LOG = new Logger(typeof(SetupSeq_CalSub).Name);

        private CalSubSeqParams _Params;
        private PatternAgentBase _PatternAgent;
        private ImageManager _ImageManager = new ImageManager();

        private SubSysInfo _SubSysInfo;
        private List<FovInfo> _FovInfos;
        private List<FovRecipe> _FovRecipes;

        private int _CamCnt;
        private string _SeqName;

        private int _PlanCurrentStep;
        private int _SubseqRepeatCnt;
        private bool _IsPatternFound;

        private List<List<ImagePoint>> _XPoints;
        private List<List<ImagePoint>> _YPoints;
        private List<List<ImagePoint>> _ThetaPoints;

        private List<double> _LengthX;
        private List<double> _LengthY;
        private List<double> _LengthTheta;

        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";
        private const string SEQ_ID_PREPARE = "PREPARE";
        private const string SEQ_ID_MOVE_TEACH_POS = "MOVE_TEACH_POS";
        private const string SEQ_ID_MOVE = "MOVE";
        private const string SEQ_ID_CLEAR = "CLEAR";
        private const string SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS = "SET_GRAB_FIND_SEQ_PARAMS";
        private const string SEQ_ID_GRAB_FIND_SEQ = "GRAB_FIND_SEQ";
        private const string SEQ_ID_SAVE_IMAGE = "SEQ_ID_SAVE_IMAGE";
        private const string SEQ_ID_APPLY_DATA = "ApplyData";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        #endregion

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

        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_PREPARE]
                > sb[SEQ_ID_MOVE_TEACH_POS]
                > sb[SEQ_ID_MOVE]
                > sb[SEQ_ID_CLEAR]
                > sb[SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS]
                > sb[SEQ_ID_GRAB_FIND_SEQ]
                > sb[SEQ_ID_SAVE_IMAGE]
                > sb[SEQ_ID_APPLY_DATA]
                );

            sb.Flows(
                  sb[SEQ_ID_APPLY_DATA]
                > sb[SEQ_ID_MOVE]
                );

            sb.Flows(
                  sb[SEQ_ID_APPLY_DATA]
                > sb[SEQ_ID_CLEAR]
                );

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

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

            return sb;
        }

        private void SetActions(SequenceBuilder sb) {
            var grabFindSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindSeq(), "GrabFindSeq");

            sb.AddAction(SEQ_ID_INIT, Init);
            sb.AddAction(SEQ_ID_PREPARE, Prepare);
            sb.AddAction(SEQ_ID_MOVE_TEACH_POS, MoveTeachPos);
            sb.AddAction(SEQ_ID_MOVE, Move);
            sb.AddAction(SEQ_ID_CLEAR, Clear);
            sb.AddAction(SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS, SetGrabFindSeqParams);
            sb.AddSubsequence(SEQ_ID_GRAB_FIND_SEQ, grabFindSeqBuilder);
            sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
            sb.AddDecision(SEQ_ID_APPLY_DATA, ApplyData);
            sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
        }

        private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
            if (param.Param != null) {
                _Params = param.Param as CalSubSeqParams;
            }

            _SubSysInfo = _Params.FovInfos[0].ToSubSysInfo();
            _FovInfos = _Params.FovInfos;
            _CamCnt = _Params.CamCnt;

            _FovRecipes = new List<FovRecipe>();
            _PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);

            _PlanCurrentStep = 0;

            _XPoints = new List<List<ImagePoint>>();
            _YPoints = new List<List<ImagePoint>>();
            _ThetaPoints = new List<List<ImagePoint>>();

            _LengthX = new List<double>();
            _LengthY = new List<double>();
            _LengthTheta = new List<double>();

            for (int i = 0; i < _CamCnt; i++) {
                _FovRecipes.Add(VisionRecipeManager.GetFovRecipe(_FovInfos[i]));
                _XPoints.Add(new List<ImagePoint>());
                _YPoints.Add(new List<ImagePoint>());
                _ThetaPoints.Add(new List<ImagePoint>());
            }

            return SequenceAction.Ok();
        }

        private SequenceActionResult Prepare(SequenceContext context, string id, SequenceActionResult param) {
            if (_FovRecipes != null)
                return SequenceAction.Ok();
            else
                return SequenceAction.Error();
        }

        private SequenceActionResult MoveTeachPos(SequenceContext context, string id, SequenceActionResult param) {
            if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
                return SequenceAction.Ok();
            }

            int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfos[0], ePositionType.Teaching);
            if (res == E1.OK) {
                Thread.Sleep(100);
                return SequenceAction.Ok();
            }
            else {
                return SequenceAction.Error(E2.ABORTED);
            }
        }

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

            GrabFindSeqUtil.Clear(_SubSysInfo);

            return SequenceAction.Ok();
        }

        private SequenceActionResult Move(SequenceContext context, string id, SequenceActionResult param) {
            var item = _Params.PlanItems[_PlanCurrentStep].Clone();
            _SubseqRepeatCnt = 0;

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

            // Image -> Motion Direction
            var csSource = new CoordSystem(); // Image CS
            var alignDirectorRecipe = VisionRecipeManager.GetAlignDirectorRecipe(_SubSysInfo);
            var csTarget = alignDirectorRecipe.CoordSystem;
            var moveXY = CoordSystem.ApplyDirection(
                new MotionPoint(double.Parse(item.X), double.Parse(item.Y)),
               csSource, csTarget);
            var moveT = CoordSystem.ApplyDirectionT(double.Parse(item.T), csTarget);
            var motionDelta = new Position() {
                X = moveXY.X,
                Y = moveXY.Y,
                T = moveT
            };

            // Move
            EquipAdapter.Instance.AlignMotionManager.RelMove(_SubSysInfo, motionDelta);
            Thread.Sleep(500);

            return SequenceAction.Ok(item);
        }

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

            FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];

            var fovRecipe = VisionRecipeManager.GetFovRecipe(fovInfo);
            PatternIndex patternIndex = null;

            if (fovRecipe.CalFovRecipe.IsFovPattern) {
                _PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);
                patternIndex = new FovPatternIndex(fovInfo, fovRecipe.FovName, fovRecipe.CalFovRecipe.PatternId);
            }
            else {
                _PatternAgent = CamPatternAgentManager.GetAgent(fovRecipe.CamRecipe.CamId);
                patternIndex = new CamPatternIndex(fovRecipe.CamRecipe.CamId, fovRecipe.CalFovRecipe.PatternId);
            }

            var patternInfo = _PatternAgent.GetPatternInfo(patternIndex);
            eSeqMode seqMode = eSeqMode.Manual;     // 그랩을 Skip하지 않기 위함.

            return SequenceAction.Ok(new GrabFindSeqParams(fovInfo, patternInfo, _Params.DisplayType, seqMode));
        }

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

            if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
                FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];

                var display = CameraDisplayManager.GetDisplay(fovInfo, _Params.DisplayType);
                object image = display.GetImageObject();

                if (image != null) {
                    _ImageManager.SaveImageInternal(fovInfo, eSaveDataType.Calibration, image);
                }
            }

            var patternResult = param.Param as PatternResult;

            return SequenceAction.Ok(patternResult);
        }

        private string ApplyData(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            List<ImagePoint> patternResultList = new List<ImagePoint>();
            List<StaticPoint> staticResultList = new List<StaticPoint>();
            List<Angle> resultAngleList = new List<Angle>();
            FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];

            var patternResult = param.Param as PatternResult;
            if (patternResult.Status == eFoundStatus.FoundOK) //하나 이상일 경우 OK
            {
                _IsPatternFound = true;
                foreach (var result in patternResult.Results) {
                    var position = result.ImagePosition.Clone() as ImagePoint;
                    patternResultList.Add(position);

                    var staticPosition = result.MaterialPosition.Clone() as StaticPoint;
                    staticResultList.Add(staticPosition);

                    var angle = result.Angle.Clone() as Angle;
                    resultAngleList.Add(angle);
                }
            }
            else if (patternResult.Status != eFoundStatus.FoundOK) {
                _IsPatternFound = false;
                return SEQ_ID_SEQ_COMPLETE;
            }

            _Params.PlanItems[_PlanCurrentStep].FovId = (int)fovInfo.FovId;
            var item = _Params.PlanItems[_PlanCurrentStep].Clone();

            item.ImagePosX = patternResultList[0].X.ToString("F4");
            item.ImagePosY = patternResultList[0].Y.ToString("F4");

            item.StaticPosX = staticResultList[0].X.ToString("F4");
            item.StaticPosY = staticResultList[0].Y.ToString("F4");

            item.ResultT = resultAngleList[0].Degree.ToString("F4");

            //Log
            context.EventPublisher.Publish(new EventMessage("SetupSeq_Cal", VisionEventId.CALIBRATION_SUB_SEQ_LOG_UPDATE, item));

            if (_PlanCurrentStep + 1 == _Params.PlanItems.Count) {
                if (_SubseqRepeatCnt + 1 == _Params.CamCnt) {
                    return SEQ_ID_SEQ_COMPLETE;
                }
            }
            else if (item.Purpose == Constants.X_CAL) {
                SaveCalDataForPurpose(item, context, "X");
            }
            else if (item.Purpose == Constants.Y_CAL) {
                SaveCalDataForPurpose(item, context, "Y");
            }
            else if (item.Purpose == Constants.THETA_CAL) {
                SaveCalDataForPurpose(item, context, "T");
            }

            _SubseqRepeatCnt++;
            if (_SubseqRepeatCnt == _Params.CamCnt) {
                _PlanCurrentStep++;
                return SEQ_ID_MOVE;
            }
            return SEQ_ID_CLEAR;
        }

        private void SaveCalDataForPurpose(PlanItem item, SequenceContext context, string purpose) {
            if (purpose == "X") {
                if (item.Action == Constants.MOVE) {
                    _LengthX.Add(double.Parse(item.X));
                    _XPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
                }
            }
            else if (purpose == "Y") {
                if (item.Action == Constants.MOVE) {
                    _LengthY.Add(double.Parse(item.Y));
                    _YPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
                }
            }
            else {
                if (item.Action == Constants.MOVE) {
                    _LengthTheta.Add(double.Parse(item.T));
                    _ThetaPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
                }
            }
        }

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

            CalSubSeqResultParams returnParam = new CalSubSeqResultParams();
            if (_IsPatternFound) {
                returnParam.XPoints = _XPoints;
                returnParam.YPoints = _YPoints;
                returnParam.ThetaPoints = _ThetaPoints;

                returnParam.LengthX = _LengthX;
                returnParam.LengthY = _LengthY;
                returnParam.LengthTheta = _LengthTheta;
                returnParam.IsPatternNotFound = false;
            }
            else {
                returnParam.IsPatternNotFound = true;
            }

            return SequenceAction.Ok(returnParam);
        }

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