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

namespace VisionAligner {
    /// <summary>
    /// 얼라인 모드(알고리즘)
    /// </summary>
    public abstract class AlignMode {
        /// <summary>로거</summary>
        protected Logger _Logger;
        /// <summary>얼라인 보정량 제한 체커</summary>
        protected AlignLimitChecker _AlignLimitChecker;
        /// <summary>얼라인 재시도 스펙 체커</summary>
        protected AlignRetryChecker _AlignRetryChecker;
        /// <summary>얼라인 재시도 횟수 체커</summary>
        protected AlignCountChecker _AlignCountChecker;

        /// <summary>얼라인 레시피</summary>
        protected AlignRecipe _AlignRecipe;
        /// <summary>서브 시스템 정보</summary>
        protected SubSysInfo _SubSysInfo;

        /// <summary>
        /// 생성자(UnitTest 전용)
        /// </summary>
        protected AlignMode() {
            // For UnitTest
        }

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="alignInfo">얼라인 정보</param>
        public AlignMode(AlignInfo alignInfo) {
            _Logger = new Logger($"AlignMode[{alignInfo.AlignName}]");
            _AlignRecipe = VisionRecipeManager.GetAlignRecipe(alignInfo);
            _SubSysInfo = alignInfo?.ToSubSysInfo();

            SetAlignCheckers();
        }

        private void SetAlignCheckers() {
            _AlignLimitChecker = new AlignLimitChecker(_AlignRecipe);
            _AlignRetryChecker = new AlignRetryChecker(_AlignRecipe);
            _AlignCountChecker = new AlignCountChecker(_AlignRecipe);
        }

        #region N-Point Align
        /// <summary>
        /// N-포인트 얼라인 보정량 XYT를 계산한다.
        /// </summary>
        /// <param name="meaPoints">패턴 검출 위치</param>
        /// <param name="refPoints">레퍼런스(기준) 위치</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <returns>얼라인 결과</returns>
        public AlignResult Align(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY) {
            StaticPoint thetaOrigin = GetThetaOrigin();
            eAlignAxis alignAxis = _AlignRecipe.ProcParams.AlignAxis;
            AlignTgtDist alignTgtDist = _AlignRecipe.AlignTgtDist;
            eAlignAlgorithm alignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;

            AlignDelta alignDelta = CalculateAlign(
                meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
            _Logger.I($"Align delta : {alignDelta}");

            AlignStatus status = GetAlignStatus(alignDelta);
            int alignCount = _AlignCountChecker.GetCurrentAlignCount(status);
            var alignResult = new AlignResult(alignCount, status, alignDelta, alignAlgorithm);

            return alignResult;
        }

        private AlignDelta CalculateAlign(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY,
            DoublePoint thetaOrigin,
            eAlignAxis alignAxis,
            AlignTgtDist alignTgtDist) {
            if(_AlignRecipe.ProcParams.AlignAlgorithm == eAlignAlgorithm.AVG) {
                return CalculateXYT(meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
            }
            else if(_AlignRecipe.ProcParams.AlignAlgorithm == eAlignAlgorithm.ICP) {
                return CalculateXYT_ICP(meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
            }
            return new AlignDelta();
        }

        private AlignDelta CalculateXYT(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY,
            DoublePoint thetaOrigin,
            eAlignAxis alignAxis,
            AlignTgtDist alignTgtDist) {
            AlignDelta alignDeltaT = CalculateT(meaPoints, refPoints, flipY);
            alignDeltaT.T = ApplyAlignAxisAndTargetDistT(alignDeltaT.T, alignAxis, alignTgtDist);

            AlignDelta alignDeltaXY = CalculateXY(meaPoints, refPoints, flipY, thetaOrigin, alignDeltaT.T);
            alignDeltaXY.X = ApplyAlignAxisAndTargetDistX(alignDeltaXY.X, alignAxis, alignTgtDist);
            alignDeltaXY.Y = ApplyAlignAxisAndTargetDistY(alignDeltaXY.Y, alignAxis, alignTgtDist);

            return new AlignDelta(
                alignDeltaXY.Translation,
                alignDeltaT.T);
        }

        private AlignDelta CalculateXYT_ICP(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY,
            DoublePoint thetaOrigin,
            eAlignAxis alignAxis,
            AlignTgtDist alignTgtDist) {
            AlignDelta alignDeltaT = CalculateT_ICP(meaPoints, refPoints, flipY);
            alignDeltaT.T = ApplyAlignAxisAndTargetDistT(alignDeltaT.T, alignAxis, alignTgtDist);

            AlignDelta alignDeltaXY = CalculateXY(meaPoints, refPoints, flipY, thetaOrigin, alignDeltaT.T);
            alignDeltaXY.X = ApplyAlignAxisAndTargetDistX(alignDeltaXY.X, alignAxis, alignTgtDist);
            alignDeltaXY.Y = ApplyAlignAxisAndTargetDistY(alignDeltaXY.Y, alignAxis, alignTgtDist);

            return new AlignDelta(
                alignDeltaXY.Translation,
                alignDeltaT.T);
        }

        /// <summary>
        /// N-포인트 얼라인 보정량 T를 계산한다.
        /// </summary>
        /// <param name="meaPoints">패턴 검출 위치</param>
        /// <param name="refPoints">레퍼런스(기준) 위치</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <returns>얼라인 보정량</returns>
        protected abstract AlignDelta CalculateT(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY);

        /// <summary>
        /// ICP 알고리즘을 이용하여 N-포인트 얼라인 보정량 T를 계산한다.
        /// </summary>
        /// <param name="meaPoints">패턴 검출 위치</param>
        /// <param name="refPoints">레퍼런스(기준) 위치</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <returns>얼라인 보정량</returns>
        protected abstract AlignDelta CalculateT_ICP(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY);

        /// <summary>
        /// N-포인트 얼라인 보정량 XY를 계산한다.
        /// </summary>
        /// <param name="meaPoints">패턴 검출 위치</param>
        /// <param name="refPoints">레퍼런스(기준) 위치</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <param name="thetaOrigin">회전 원점</param>
        /// <param name="alignT">얼라인 보정량 T</param>
        /// <returns>얼라인 보정량</returns>
        protected abstract AlignDelta CalculateXY(
            List<DoublePoint> meaPoints,
            List<DoublePoint> refPoints,
            int flipY,
            DoublePoint thetaOrigin,
            Angle alignT);
        #endregion

        #region 1-Point Align
        /// <summary>
        /// 1-포인트 얼라인 보정량 XYT를 계산한다.
        /// </summary>
        /// <param name="meaPoint">패턴 검출 위치</param>
        /// <param name="refPoint">레퍼런스(기준) 위치</param>
        /// <param name="theta">얼라인 보정량 T</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <returns>얼라인 결과</returns>
        public AlignResult Align(
            DoublePoint meaPoint,
            DoublePoint refPoint,
            Angle theta,
            int flipY) {
            StaticPoint thetaOrigin = GetThetaOrigin();
            eAlignAxis alignAxis = _AlignRecipe.ProcParams.AlignAxis;
            AlignTgtDist alignTgtDist = _AlignRecipe.AlignTgtDist;
            eAlignAlgorithm alignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;

            AlignDelta alignDelta = CalculateXYT(
                meaPoint, refPoint, flipY, thetaOrigin, alignAxis, alignTgtDist, theta);
            _Logger.I($"Align delta : {alignDelta}");

            AlignStatus status = GetAlignStatus(alignDelta);
            int alignCount = _AlignCountChecker.GetCurrentAlignCount(status);

            return new AlignResult(alignCount, status, alignDelta, alignAlgorithm);
        }

        private AlignDelta CalculateXYT(
            DoublePoint meaPoint,
            DoublePoint refPoint,
            int flipY,
            DoublePoint thetaOrigin,
            eAlignAxis alignAxis,
            AlignTgtDist alignTgtDist,
            Angle theta) {
            var alignT = ApplyAlignAxisAndTargetDistT(theta, alignAxis, alignTgtDist);

            var alignDelta = CalculateXYT(
                meaPoint, refPoint, flipY, thetaOrigin, alignT);

            alignDelta.X = ApplyAlignAxisAndTargetDistX(alignDelta.X, alignAxis, alignTgtDist);
            alignDelta.Y = ApplyAlignAxisAndTargetDistY(alignDelta.Y, alignAxis, alignTgtDist);

            return alignDelta;
        }

        /// <summary>
        /// 1-포인트 얼라인 보정량 XYT를 계산한다.
        /// </summary>
        /// <param name="meaPoint">패턴 검출 위치</param>
        /// <param name="refPoint">레퍼런스(기준) 위치</param>
        /// <param name="flipY">좌표계의 Y축 반전 여부</param>
        /// <param name="thetaOrigin">회전 원점</param>
        /// <param name="theta">얼라인 보정량 T</param>
        /// <returns>얼라인 보정량</returns>
        protected abstract AlignDelta CalculateXYT(
            DoublePoint meaPoint,
            DoublePoint refPoint,
            int flipY,
            DoublePoint thetaOrigin,
            Angle theta);
        #endregion

        /// <summary>
        /// 얼라인 상태를 체크한다.
        /// </summary>
        /// <param name="alignDelta">얼라인 보정량</param>
        /// <returns>얼라인 상태 코드</returns>
        protected AlignStatus GetAlignStatus(AlignDelta alignDelta) {
            AlignStatus status = AlignStatus.OK;

            if (_AlignLimitChecker.IsEnabled()) {
                status = _AlignLimitChecker.GetAlignLimitStatus(alignDelta);

                if (status == AlignStatus.ALIGN_LIMIT_OVER) {
                    ResetRetrySpecChecker();
                    return status;
                }
            }

            if (_AlignRetryChecker.IsEnabled()) {
                status = _AlignRetryChecker.GetRetryStatus(alignDelta);

                if (status == AlignStatus.RETRY_COUNT_OVER) {
                    ResetRetrySpecChecker();
                }
                return status;
            }

            return status;
        }

        /// <summary>
        /// 얼라인 재시도 스펙 체커를 초기화 한다.
        /// </summary>
        public void ResetRetrySpecChecker() {
            _AlignRetryChecker.Reset();
        }

        /// <summary>
        /// 얼라인 재시도 횟수를 초기화 한다.
        /// </summary>
        public void ResetAlignCountChecker() {
            _AlignCountChecker.Reset();
        }

        /// <summary>
        /// 얼라인 재시도 스펙 체커와 재시도 횟수를 초기화 한다.
        /// </summary>
        public void ResetAll() {
            ResetRetrySpecChecker();
            ResetAlignCountChecker();
        }

        /// <summary>
        /// 회전 원점을 얻어온다.
        /// </summary>
        /// <returns>회전 원점 좌표</returns>
        protected StaticPoint GetThetaOrigin() {
            var calRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
            return (calRecipe.ThetaOrigin + _AlignRecipe.AlignThetaOrigin.Offset) as StaticPoint;
        }

        /// <summary>
        /// 얼라인 보정량 T에 얼라인 타겟 회전량를 적용한다.
        /// </summary>
        /// <param name="alignT">얼라인 보정량 T</param>
        /// <param name="alignAxis">얼라인 사용 축</param>
        /// <param name="alignTgtDist">얼라인 타겟 회전량</param>
        /// <returns>최종 얼라인 보정량 T</returns>
        protected static Angle ApplyAlignAxisAndTargetDistT(
            Angle alignT, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
            if (IsRotationEnabled(alignAxis)) {
                if (alignTgtDist.Enabled) {
                    return Angle.CreateWithDegree(alignT.Degree - alignTgtDist.DistT);
                }
                return alignT;
            }

            return new Angle();
        }

        /// <summary>
        /// 얼라인 보정량 X에 얼라인 타겟 거리 X를 적용한다.
        /// </summary>
        /// <param name="alignX">얼라인 보정량 X</param>
        /// <param name="alignAxis">얼라인 사용 축</param>
        /// <param name="alignTgtDist">얼라인 타겟 거리 X</param>
        /// <returns>최종 얼라인 보정량 X</returns>
        protected static double ApplyAlignAxisAndTargetDistX(
            double alignX, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
            if (IsTranslationXEnabled(alignAxis)) {
                if (alignTgtDist.Enabled) {
                    return alignX - alignTgtDist.DistX;
                }
                return alignX;
            }

            return 0;
        }

        /// <summary>
        /// 얼라인 보정량 Y에 얼라인 타겟 거리 Y를 적용한다.
        /// </summary>
        /// <param name="alignY">얼라인 보정량 Y</param>
        /// <param name="alignAxis">얼라인 사용 축</param>
        /// <param name="alignTgtDist">얼라인 타겟 거리 Y</param>
        /// <returns>최종 얼라인 보정량 Y</returns>
        protected static double ApplyAlignAxisAndTargetDistY(
            double alignY, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
            if (IsTranslationYEnabled(alignAxis)) {
                if (alignTgtDist.Enabled) {
                    return alignY - alignTgtDist.DistY;
                }
                return alignY;
            }

            return 0;
        }

        /// <summary>
        /// 얼라인 T축 사용여부를 체크한다.
        /// </summary>
        /// <param name="alignAxis">얼라인 축 정보</param>
        /// <returns>체크 결과</returns>
        protected static bool IsRotationEnabled(eAlignAxis alignAxis) {
            return ((int)alignAxis & (int)eAlignAxis.T) != 0;
        }

        /// <summary>
        /// 얼라인 XY축 사용여부를 체크한다.
        /// </summary>
        /// <param name="alignAxis">얼라인 축 정보</param>
        /// <returns>체크 결과</returns>
        protected static bool IsTranslationEnabled(eAlignAxis alignAxis) {
            return ((int)alignAxis & (int)eAlignAxis.XY) != 0;
        }

        /// <summary>
        /// 얼라인 X축 사용여부를 체크한다.
        /// </summary>
        /// <param name="alignAxis">얼라인 축 정보</param>
        /// <returns>체크 결과</returns>
        protected static bool IsTranslationXEnabled(eAlignAxis alignAxis) {
            return ((int)alignAxis & (int)eAlignAxis.X) != 0;
        }

        /// <summary>
        /// 얼라인 Y축 사용여부를 체크한다.
        /// </summary>
        /// <param name="alignAxis">얼라인 축 정보</param>
        /// <returns>체크 결과</returns>
        protected static bool IsTranslationYEnabled(eAlignAxis alignAxis) {
            return ((int)alignAxis & (int)eAlignAxis.Y) != 0;
        }
    }
}