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

namespace VisionSequence.Setup {
    /// <summary>
    /// 캘리브레이션 시퀀스 요청 파라미터
    /// </summary>
    public class CalSeqParams {
        /// <summary>서브 시스템 정보</summary>
        public SubSysInfo SubSysInfo { get; set; }
        /// <summary>메인 화면 타입</summary>
        public eDisplayType DisplayType { get; set; }
        /// <summary>캘리브레이션 플랜아이템 리스트</summary>
        public List<PlanItem> PlanItems { get; set; }
        /// <summary>캘리브레이션 셋업 레시피</summary>
        public CalSetupRecipe CalSetupRecipe { get; set; }
    }

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

        /// <summary>X축으로 1회 이동 거리</summary>
        public double LengthX;
        /// <summary>Y축으로 1회 이동 거리</summary>
        public double LengthY;
        /// <summary>T축으로 1회 회전 각도(Degree)</summary>
        public double LengthTheta;
    }

    /// <summary>
    /// 캘리브레이션 시퀀스 수행 결과 파라미터
    /// </summary>
    public class CalSeqResultParams {
        /// <summary>캘리브레이션 시퀀스 수행 결과 리스트</summary>
        public List<CalSeqResult> Results { get; set; }
    }

    /// <summary>
    /// 캘리브레이션 계산 결과
    /// </summary>
    public class CalCalculateResult {
        /// <summary>카메라 회전 오차</summary>
        public Angle ErrorRot;
        /// <summary>이미지 픽셀 크기 X</summary>
        public double PixelSizeX;
        /// <summary>이미지 픽셀 크기 Y</summary>
        public double PixelSizeY;
        /// <summary>모터 회전 중심</summary>
        public StaticPoint ThetaOrigin;
    }

    /// <summary>
    /// 캘리브레이션 계산 결과 파라미터
    /// </summary>
    public class CalCalculateResultParams {
        /// <summary>캘리브레이션 계산 결과 리스트</summary>
        public List<CalCalculateResult> Results { get; set; }
    }

    /// <summary>
    /// 캘리브레이션 셋업 시퀀스
    /// </summary>
    public class SetupSeq_Calibration : SubSeqBase {
        private readonly Logger LOG = new Logger(typeof(SetupSeq_Calibration).Name);

        private CalSeqParams _Params;
        private List<CalSubSeqResultParams> _CalResult;
        private List<FovInfo> _FovInfos;
        private List<FovRecipe> _FovRecipes;
        private SubSysInfo _SubSysInfo;
        private List<eCamId> _CamIds;
        private int _CamCnt;
        private string _SeqName;
        private int _CurrentFov;
        private bool _IsPatternNotFound;
        private CalSetupRecipe _CalSetupRecipe;


        #region Sequence Id
        private const string SEQ_ID_INIT = "INIT";
        private const string SEQ_ID_CHECK_PATTERN = "CHECK_PATTERN";
        private const string SEQ_ID_UNDEFINED_PATTERN_NG = "SEQ_ID_UNDEFINED_PATTERN_NG";
        private const string SEQ_ID_PREPARE = "PREPARE";
        private const string SEQ_ID_RUN_CALIBRATION_SEQ = "RUN_CALIBRATION_SEQ";
        private const string SEQ_ID_APPLY_DATA = "ApplyData";
        private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
        private const string SEQ_ID_CALCULATE = "CALCULATE";
        #endregion

        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_CHECK_PATTERN]
                > sb[SEQ_ID_PREPARE]
                > sb[SEQ_ID_RUN_CALIBRATION_SEQ]
                > sb[SEQ_ID_APPLY_DATA]
                );

            sb.Flows(
                sb[SEQ_ID_CHECK_PATTERN]
                > sb[SEQ_ID_UNDEFINED_PATTERN_NG]
                > sb[SEQ_ID_SEQ_COMPLETE]
                );

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

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

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

            return sb;
        }
        private void SetActions(SequenceBuilder sb) {
            var CalSubSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new SetupSeq_CalSub(), "SetupSeq_CalSub");

            sb.AddAction(SEQ_ID_INIT, Init);
            sb.AddDecision(SEQ_ID_CHECK_PATTERN, CheckPattern);
            sb.AddAction(SEQ_ID_UNDEFINED_PATTERN_NG, UndefinedPatternNG);
            sb.AddAction(SEQ_ID_PREPARE, Prepare);
            sb.AddSubsequence(SEQ_ID_RUN_CALIBRATION_SEQ, CalSubSeqBuilder);
            sb.AddDecision(SEQ_ID_APPLY_DATA, ApplyData);
            sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
            sb.AddAction(SEQ_ID_CALCULATE, Calculate);
        }

        private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
            if (param.Param != null) {
                _Params = param.Param as CalSeqParams;
            }
            _CalResult = new List<CalSubSeqResultParams>();
            //_FovInfo = _Params.FovInfo;
            _SubSysInfo = _Params.SubSysInfo;
            _FovInfos = VisionRecipeManager.GetFovInfos(_SubSysInfo);
            _FovRecipes = VisionRecipeManager.GetFovRecipes(_FovInfos);
            _CamIds = VisionRecipeManager.GetCamIds(_FovRecipes);
            _CamCnt = _CamIds.Count();
            _CurrentFov = 0;
            _IsPatternNotFound = false;
            _CalSetupRecipe = _Params.CalSetupRecipe;

            if (_SubSysInfo != null)
                return SequenceAction.Ok();
            else
                return SequenceAction.Error();
        }

        private string CheckPattern(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            var subsysRecipe = VisionRecipeManager.GetSubSysRecipe(_SubSysInfo);
            foreach (var fovRecipe in subsysRecipe.FovRecipes) {
                if (fovRecipe.CalFovRecipe.PatternId == -1) {
                    _IsPatternNotFound = true;
                    return SEQ_ID_UNDEFINED_PATTERN_NG;
                }
            }

            return SEQ_ID_PREPARE;
        }

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

            string errMsg = "Undefined PatternID Available";
            PrintErrorLogAndShowErrorToast(LOG, errMsg);

            _CalResult = new List<CalSubSeqResultParams>();

            return SequenceAction.Ok();
        }

        private SequenceActionResult Prepare(SequenceContext context, string id, SequenceActionResult param) {
            List<FovInfo> fovInfos = new List<FovInfo>();
            fovInfos.Clear();
            for (int i = 0; i < _CamCnt; i++) {
                fovInfos.Add(_FovInfos[_CurrentFov + i]);
            }
            List<PlanItem> planItems = new List<PlanItem>();
            planItems.Clear();
            planItems = _Params.PlanItems.ConvertAll(item => new PlanItem(item.Step, item.Purpose, item.Target, item.Action, item.X, item.Y, item.T));

            return SequenceAction.Ok(
                new CalSubSeqParams() {
                    FovInfos = fovInfos,
                    PlanItems = planItems,
                    DisplayType = _Params.DisplayType,
                    CamCnt = _CamCnt
                }); ;
        }

        private string ApplyData(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
            var calResult = param.Param as CalSubSeqResultParams;

            if (calResult.IsPatternNotFound) {
                LOG.E($"FOV {_CurrentFov} Calibration : Pattern Not Found");
                _IsPatternNotFound = true;
                return SEQ_ID_SEQ_COMPLETE;
            }

            _CalResult.Add(calResult);

            _CurrentFov = _CamCnt + _CurrentFov;

            if (_CurrentFov == _FovInfos.Count)
                return SEQ_ID_SEQ_COMPLETE;

            return SEQ_ID_PREPARE;
        }

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

            if (_IsPatternNotFound) {
                return SequenceAction.Error();
            }

            CalSeqResultParams returnParam = new CalSeqResultParams();
            if (returnParam.Results == null) {
                returnParam.Results = new List<CalSeqResult>();
            }

            foreach (var calSubSeqResult in _CalResult) {
                for (int i = 0; i < _CamCnt; i++) {
                    CalSeqResult calFovResult = new CalSeqResult();

                    if (_CalSetupRecipe.isX()) {
                        calFovResult.XPoints = calSubSeqResult.XPoints[i];
                        calFovResult.LengthX = calSubSeqResult.LengthX[i];
                    }
                    if (_CalSetupRecipe.isY()) {
                        calFovResult.YPoints = calSubSeqResult.YPoints[i];
                        calFovResult.LengthY = calSubSeqResult.LengthY[i];
                    }

                    if (_CalSetupRecipe.isT()) {
                        calFovResult.ThetaPoints = calSubSeqResult.ThetaPoints[i];
                        calFovResult.LengthTheta = calSubSeqResult.LengthTheta[i];
                    }

                    returnParam.Results.Add(calFovResult);
                }
            }

            return SequenceAction.Ok(returnParam);
        }

        private SequenceActionResult Calculate(
           SequenceContext context,
           string id,
           SequenceActionResult param) {
            CalCalculateResultParams returnParam = new CalCalculateResultParams();
            returnParam.Results = new List<CalCalculateResult>();
            List<CalCalculateResult> fovCalResults = new List<CalCalculateResult>();
            var calSeqResult = param.Param as CalSeqResultParams;

            if (_IsPatternNotFound) {
                return SequenceAction.Error();
            }
            var calResults = calSeqResult.Results;

            for (int fovId = 0; fovId < calResults.Count(); fovId++) {
                CalCalculateResult fovCalResult = new CalCalculateResult();
                var calibrator = CalibratorManager.GetCalibrator(_FovInfos[fovId]);
                var cameraAgent = CameraAgentManager.GetCameraAgent(_FovInfos[fovId]);
                var imageSize = cameraAgent.GetImageSize();
                Angle errorRotX = Angle.CreateWithDegree(0);
                Angle errorRotY = Angle.CreateWithDegree(0);
                double imgPixelX = 0;
                double imgPixelY = 0;

                if (_CalSetupRecipe.isX() && _CalSetupRecipe.isY()) {
                    if (calResults[fovId].XPoints != null) {
                        errorRotX = calibrator.FindStaticErrorRotation(calResults[fovId].XPoints, 'x');
                        imgPixelX = Math.Abs(calibrator.GetPixelSize(calResults[fovId].XPoints, calResults[fovId].LengthX, errorRotX.Radian, 'x'));
                    }
                    if (calResults[fovId].YPoints != null) {
                        errorRotY = calibrator.FindStaticErrorRotation(calResults[fovId].YPoints, 'y');
                        imgPixelY = Math.Abs(calibrator.GetPixelSize(calResults[fovId].YPoints, calResults[fovId].LengthY, errorRotY.Radian, 'y'));
                    }
                    fovCalResult.ErrorRot = Angle.CreateWithDegree((errorRotX.Degree + errorRotY.Degree) / 2);
                    fovCalResult.PixelSizeX = imgPixelX;
                    fovCalResult.PixelSizeY = imgPixelY;
                    // 3. theta origin
                    if (_CalSetupRecipe.isT()) {
                        StaticPoint thetaOrigin;
                        if (calResults[fovId].ThetaPoints != null) {
                            var thetaStaticPoints = new List<StaticPoint>();
                            foreach (var point in calResults[fovId].ThetaPoints) {
                                thetaStaticPoints.Add(calibrator.TransformToCalibratedSpace(point, imageSize, fovCalResult.ErrorRot, new StaticPoint(imgPixelX, imgPixelY)));
                            }
                            thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
                        }
                        else {
                            thetaOrigin = new StaticPoint(0, 0);
                        }
                        fovCalResult.ThetaOrigin = thetaOrigin;
                    }
                }

                else if (_CalSetupRecipe.isX()) {
                    if (calResults[fovId].XPoints != null) {
                        errorRotX = calibrator.FindStaticErrorRotation(calResults[fovId].XPoints, 'x');
                        imgPixelX = Math.Abs(calibrator.GetPixelSize(calResults[fovId].XPoints, calResults[fovId].LengthX, errorRotX.Radian, 'x'));
                    }
                    fovCalResult.ErrorRot = errorRotX;
                    fovCalResult.PixelSizeX = imgPixelX;

                    if (_CalSetupRecipe.isT()) {
                        StaticPoint thetaOrigin;
                        if (calResults[fovId].ThetaPoints != null) {
                            var thetaStaticPoints = new List<StaticPoint>();
                            foreach (var point in calResults[fovId].ThetaPoints) {
                                thetaStaticPoints.Add(calibrator.TransformToCalibratedSpaceWithPixelX(point, imageSize, fovCalResult.ErrorRot, fovCalResult.PixelSizeX));
                            }
                            thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
                        }
                        else {
                            thetaOrigin = new StaticPoint(0, 0);
                        }
                        fovCalResult.ThetaOrigin = thetaOrigin;
                    }
                }
                else if (_CalSetupRecipe.isY()) {
                    if (calResults[fovId].YPoints != null) {
                        errorRotY = calibrator.FindStaticErrorRotation(calResults[fovId].YPoints, 'y');
                        imgPixelY = Math.Abs(calibrator.GetPixelSize(calResults[fovId].YPoints, calResults[fovId].LengthY, errorRotY.Radian, 'y'));
                    }
                    fovCalResult.ErrorRot = errorRotY;
                    fovCalResult.PixelSizeY = imgPixelY;

                    if (_CalSetupRecipe.isT()) {
                        StaticPoint thetaOrigin;
                        if (calResults[fovId].ThetaPoints != null) {
                            var thetaStaticPoints = new List<StaticPoint>();
                            foreach (var point in calResults[fovId].ThetaPoints) {
                                thetaStaticPoints.Add(calibrator.TransformToCalibratedSpaceWithPixelY(point, imageSize, fovCalResult.ErrorRot, fovCalResult.PixelSizeY));
                            }
                            thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
                        }
                        else {
                            thetaOrigin = new StaticPoint(0, 0);
                        }
                        fovCalResult.ThetaOrigin = thetaOrigin;
                    }
                }

                else if (_CalSetupRecipe.isT()) {
                    // 3. theta origin
                    StaticPoint thetaOrigin;
                    if (calResults[fovId].ThetaPoints != null) {
                        var thetaStaticPoints = new List<StaticPoint>();
                        foreach (var point in calResults[fovId].ThetaPoints) {
                            thetaStaticPoints.Add(calibrator.TransformToCalibratedSpace(point, imageSize));
                        }
                        thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
                    }
                    else {
                        thetaOrigin = new StaticPoint(0, 0);
                    }
                    fovCalResult.ThetaOrigin = thetaOrigin;
                }
                fovCalResults.Add(fovCalResult);
            }

            List<StaticPoint> thetaOrigins = new List<StaticPoint>();
            StaticPoint averageThetaOrigin;
            if (_CalSetupRecipe.isT()) {
                for (int fovId = 0; fovId < calResults.Count(); fovId++) {
                    thetaOrigins.Add(fovCalResults[fovId].ThetaOrigin);
                }
                averageThetaOrigin = new StaticPoint(thetaOrigins.Average(item => item.X), thetaOrigins.Average(item => item.Y));
            }
            else {
                averageThetaOrigin = new StaticPoint(0, 0);
            }

            foreach (var camId in _CamIds) {
                CalCalculateResult camCalResult = new CalCalculateResult();
                List<Angle> errorRots = new List<Angle>();
                List<double> pixelSizesX = new List<double>();
                List<double> pixelSizesY = new List<double>();

                if (_CalSetupRecipe.isX() && _CalSetupRecipe.isY()) {
                    for (int fovId = 0; fovId < calResults.Count(); fovId++) {
                        if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
                            errorRots.Add(fovCalResults[fovId].ErrorRot);
                            pixelSizesX.Add(fovCalResults[fovId].PixelSizeX);
                            pixelSizesY.Add(fovCalResults[fovId].PixelSizeY);
                        }
                    }
                    camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
                    camCalResult.PixelSizeX = pixelSizesX.Average();
                    camCalResult.PixelSizeY = pixelSizesY.Average();
                }
                else if (_CalSetupRecipe.isX()) {
                    for (int fovId = 0; fovId < calResults.Count(); fovId++) {
                        if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
                            errorRots.Add(fovCalResults[fovId].ErrorRot);
                            pixelSizesX.Add(fovCalResults[fovId].PixelSizeX);
                        }
                    }
                    camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
                    camCalResult.PixelSizeX = pixelSizesX.Average();
                }
                else if (_CalSetupRecipe.isY()) {
                    for (int fovId = 0; fovId < calResults.Count(); fovId++) {
                        if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
                            errorRots.Add(fovCalResults[fovId].ErrorRot);
                            pixelSizesY.Add(fovCalResults[fovId].PixelSizeY);
                        }
                    }
                    camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
                    camCalResult.PixelSizeY = pixelSizesY.Average();
                }
                if (_CalSetupRecipe.isT()) {
                    camCalResult.ThetaOrigin = averageThetaOrigin;
                }
                returnParam.Results.Add(camCalResult);
            }

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