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

namespace VisionCommon {
    /// <summary>
    /// FOV 간에 상관관계를 이용하여 라인을 교정하기 위한 매니저
    /// </summary>
    public class LineFittingManager {
        private static readonly Logger _Logger = new Logger(typeof(LineFittingManager).Name);
        private static readonly object LockThis = new object();

        // 라인 중점 보관
        private static Dictionary<SubSysInfo, Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>> _LineCenterPoints =
            new Dictionary<SubSysInfo, Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>>();

        /// <summary>
        /// 라인 중점 보관 (자재 좌표)
        /// </summary>
        /// <param name="fovInfo">FOV 정보</param>
        /// <param name="lineType">수평/수직 라인 타입</param>
        /// <param name="calibratedPoint">라인 중점 (자재 좌표)</param>
        public static void SetLineCenterPoint(FovInfo fovInfo, eLineDirection lineType, StaticPoint calibratedPoint) {
            lock (LockThis) {
                var subSysInfo = fovInfo.ToSubSysInfo();
                if (!_LineCenterPoints.ContainsKey(subSysInfo)) {
                    _LineCenterPoints.Add(subSysInfo, new Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>());
                }
                if (!_LineCenterPoints[subSysInfo].ContainsKey(fovInfo)) {
                    _LineCenterPoints[subSysInfo].Add(fovInfo, new Dictionary<eLineDirection, StaticPoint>());
                }
                if (!_LineCenterPoints[subSysInfo][fovInfo].ContainsKey(lineType)) {
                    _LineCenterPoints[subSysInfo][fovInfo].Add(lineType, null);
                }
                _LineCenterPoints[subSysInfo][fovInfo][lineType] = calibratedPoint.Clone() as StaticPoint;
            }
        }

        /// <summary>
        /// 라인 중점 가져오기 (자재 좌표)
        /// </summary>
        /// <param name="fovInfo">FOV 정보</param>
        /// <param name="lineType">수평/수직 라인 타입</param>
        /// <returns>라인 중점 (자재 좌표)</returns>
        public static StaticPoint GetLineCenterPoint(FovInfo fovInfo, eLineDirection lineType) {
            var subSysInfo = fovInfo.ToSubSysInfo();
            if (_LineCenterPoints.ContainsKey(subSysInfo)) {
                if (_LineCenterPoints[subSysInfo].ContainsKey(fovInfo)) {
                    if (_LineCenterPoints[subSysInfo][fovInfo].ContainsKey(lineType)) {
                        return _LineCenterPoints[subSysInfo][fovInfo][lineType];
                    }
                    else {
                        _Logger.FuncE($"_LineCenterPoints of [{fovInfo}] of [{subSysInfo}] contains not key [lineType : {lineType}]");
                    }
                }
                else {
                    _Logger.FuncE($"_LineCenterPoints of [{subSysInfo}] contains not key [fovInfo : {fovInfo}]");
                }
            }
            else {
                _Logger.FuncE($"_LineCenterPoints contains not key [subSysInfo : {subSysInfo}]");
            }
            return null;
        }

        /// <summary>
        /// 라인 중점 초기화
        /// </summary>
        public static void ClearLineCenterPoints(SubSysInfo subSysInfo) {
            _LineCenterPoints[subSysInfo].Clear();
        }

        /// <summary>
        /// 각 FOV에서 찾은 라인의 중점 기반으로 새로 그려진 라인의 교점으로 meaPoints를 새롭게 정의
        /// </summary>
        /// <returns>보상된 교점 위치</returns>
        public static List<DoublePoint> GetFixedMeasurePoints(SubSysInfo subSysInfo) {
            try {
                // 라인 중점 (자재 좌표)
                StaticPoint cpFov1_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1), eLineDirection.H);
                StaticPoint cpFov1_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1), eLineDirection.V);
                StaticPoint cpFov2_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2), eLineDirection.H);
                StaticPoint cpFov2_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2), eLineDirection.V);
                StaticPoint cpFov3_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3), eLineDirection.H);
                StaticPoint cpFov3_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3), eLineDirection.V);
                StaticPoint cpFov4_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4), eLineDirection.H);
                StaticPoint cpFov4_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4), eLineDirection.V);

                // 라인 방정식 (자재 좌표)
                var lineFov1_2 = new LineEquation(cpFov2_H.X, cpFov2_H.Y, cpFov1_H.X, cpFov1_H.Y);
                var lineFov1_3 = new LineEquation(cpFov1_V.X, cpFov1_V.Y, cpFov3_V.X, cpFov3_V.Y);
                var lineFov2_4 = new LineEquation(cpFov4_V.X, cpFov4_V.Y, cpFov2_V.X, cpFov2_V.Y);
                var lineFov3_4 = new LineEquation(cpFov3_H.X, cpFov3_H.Y, cpFov4_H.X, cpFov4_H.Y);

                // 라인 교점 (자재 좌표)
                StaticPoint intersectFov1 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_2, lineFov1_3);
                StaticPoint intersectFov2 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov2_4, lineFov1_2);
                StaticPoint intersectFov3 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_3, lineFov3_4);
                StaticPoint intersectFov4 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov3_4, lineFov2_4);

                // 얼라인 포인트 재정의
                var fixedMeaPoints = new List<DoublePoint>();
                fixedMeaPoints.Add(intersectFov1);     // 반드시 refPoints의 Fov쌍과 맞아야 함. (Fov1,2,3,4 순서대로 넣어야 함)
                fixedMeaPoints.Add(intersectFov2);
                fixedMeaPoints.Add(intersectFov3);
                fixedMeaPoints.Add(intersectFov4);

                return fixedMeaPoints;
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
            return null;
        }

        /// <summary>
        /// 라인 교점들로 라인 그리기
        /// </summary>
        /// <param name="fixedMeaPoints">라인 교점들</param>
        public static void DrawFixedLinesAndIntersects(SubSysInfo subSysInfo, eDisplayType displayType) {
            try {
                Application.Current.Dispatcher.BeginInvoke(
                    new System.Action(() => {
                        var fovInfoFov1 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1);
                        var fovInfoFov2 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2);
                        var fovInfoFov3 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3);
                        var fovInfoFov4 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4);
                        var fovRecipeFov1 = VisionRecipeManager.GetFovRecipe(fovInfoFov1);
                        var fovRecipeFov2 = VisionRecipeManager.GetFovRecipe(fovInfoFov2);
                        var fovRecipeFov3 = VisionRecipeManager.GetFovRecipe(fovInfoFov3);
                        var fovRecipeFov4 = VisionRecipeManager.GetFovRecipe(fovInfoFov4);
                        var camConfigFov1 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov1.CamRecipe.CamId.ToString()) as CamConfig;
                        var camConfigFov2 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov2.CamRecipe.CamId.ToString()) as CamConfig;
                        var camConfigFov3 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov3.CamRecipe.CamId.ToString()) as CamConfig;
                        var camConfigFov4 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov4.CamRecipe.CamId.ToString()) as CamConfig;
                        var cameraDisplayFov1 = CameraDisplayManager.GetDisplay(fovInfoFov1, displayType);
                        var cameraDisplayFov2 = CameraDisplayManager.GetDisplay(fovInfoFov2, displayType);
                        var cameraDisplayFov3 = CameraDisplayManager.GetDisplay(fovInfoFov3, displayType);
                        var cameraDisplayFov4 = CameraDisplayManager.GetDisplay(fovInfoFov4, displayType);

                        // 라인 중점 (자재 좌표)
                        StaticPoint cpFov1_H = GetLineCenterPoint(fovInfoFov1, eLineDirection.H);
                        StaticPoint cpFov1_V = GetLineCenterPoint(fovInfoFov1, eLineDirection.V);
                        StaticPoint cpFov2_H = GetLineCenterPoint(fovInfoFov2, eLineDirection.H);
                        StaticPoint cpFov2_V = GetLineCenterPoint(fovInfoFov2, eLineDirection.V);
                        StaticPoint cpFov3_H = GetLineCenterPoint(fovInfoFov3, eLineDirection.H);
                        StaticPoint cpFov3_V = GetLineCenterPoint(fovInfoFov3, eLineDirection.V);
                        StaticPoint cpFov4_H = GetLineCenterPoint(fovInfoFov4, eLineDirection.H);
                        StaticPoint cpFov4_V = GetLineCenterPoint(fovInfoFov4, eLineDirection.V);

                        // 라인 중점 (픽셀 좌표)
                        ImagePoint cpFov1_I_H = GetPixelPos(fovInfoFov1, cpFov1_H, camConfigFov1.ImageSize);
                        ImagePoint cpFov1_I_V = GetPixelPos(fovInfoFov1, cpFov1_V, camConfigFov1.ImageSize);
                        ImagePoint cpFov2_I_H = GetPixelPos(fovInfoFov2, cpFov2_H, camConfigFov1.ImageSize);
                        ImagePoint cpFov2_I_V = GetPixelPos(fovInfoFov2, cpFov2_V, camConfigFov1.ImageSize);
                        ImagePoint cpFov3_I_H = GetPixelPos(fovInfoFov3, cpFov3_H, camConfigFov1.ImageSize);
                        ImagePoint cpFov3_I_V = GetPixelPos(fovInfoFov3, cpFov3_V, camConfigFov1.ImageSize);
                        ImagePoint cpFov4_I_H = GetPixelPos(fovInfoFov4, cpFov4_H, camConfigFov1.ImageSize);
                        ImagePoint cpFov4_I_V = GetPixelPos(fovInfoFov4, cpFov4_V, camConfigFov1.ImageSize);

                        // 라인 방정식 (자재 좌표)
                        var lineFov1_2 = new LineEquation(cpFov2_H.X, cpFov2_H.Y, cpFov1_H.X, cpFov1_H.Y);
                        var lineFov1_3 = new LineEquation(cpFov1_V.X, cpFov1_V.Y, cpFov3_V.X, cpFov3_V.Y);
                        var lineFov2_4 = new LineEquation(cpFov4_V.X, cpFov4_V.Y, cpFov2_V.X, cpFov2_V.Y);
                        var lineFov3_4 = new LineEquation(cpFov3_H.X, cpFov3_H.Y, cpFov4_H.X, cpFov4_H.Y);

                        // 라인 교점 (자재 좌표)
                        StaticPoint intersectFov1 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_2, lineFov1_3);
                        StaticPoint intersectFov2 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov2_4, lineFov1_2);
                        StaticPoint intersectFov3 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_3, lineFov3_4);
                        StaticPoint intersectFov4 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov3_4, lineFov2_4);

                        // 라인 교점 (픽셀 좌표)
                        ImagePoint intersectFov1_I = GetPixelPos(fovInfoFov1, intersectFov1, camConfigFov1.ImageSize);
                        ImagePoint intersectFov2_I = GetPixelPos(fovInfoFov2, intersectFov2, camConfigFov2.ImageSize);
                        ImagePoint intersectFov3_I = GetPixelPos(fovInfoFov3, intersectFov3, camConfigFov3.ImageSize);
                        ImagePoint intersectFov4_I = GetPixelPos(fovInfoFov4, intersectFov4, camConfigFov4.ImageSize);

                        // 교정된 교점 십자가 그리기
                        cameraDisplayFov1.AddCross(new Cross(intersectFov1_I), eColor.Magenta);
                        cameraDisplayFov2.AddCross(new Cross(intersectFov2_I), eColor.Magenta);
                        cameraDisplayFov3.AddCross(new Cross(intersectFov3_I), eColor.Magenta);
                        cameraDisplayFov4.AddCross(new Cross(intersectFov4_I), eColor.Magenta);

                        // 교정된 교점 위치 테스트 뿌리기
                        var txtPosOffsetY = -150;

                        var msgsFov1 = new List<string>();
                        msgsFov1.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov1_I.X, intersectFov1_I.Y));
                        msgsFov1.Add(ConvertMaterialPosition(intersectFov1));
                        var txtPos_Fov1 = new ImagePoint(intersectFov1_I.X, intersectFov1_I.Y + txtPosOffsetY);
                        cameraDisplayFov1.AddTextList(eColor.Magenta, txtPos_Fov1, msgsFov1);

                        var msgsFov2 = new List<string>();
                        msgsFov2.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov2_I.X, intersectFov2_I.Y));
                        msgsFov2.Add(ConvertMaterialPosition(intersectFov2));
                        var txtPos_Fov2 = new ImagePoint(intersectFov2_I.X, intersectFov2_I.Y + txtPosOffsetY);
                        cameraDisplayFov2.AddTextList(eColor.Magenta, txtPos_Fov2, msgsFov2);

                        var msgsFov3 = new List<string>();
                        msgsFov3.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov3_I.X, intersectFov3_I.Y));
                        msgsFov3.Add(ConvertMaterialPosition(intersectFov3));
                        var txtPos_Fov3 = new ImagePoint(intersectFov3_I.X, intersectFov3_I.Y + txtPosOffsetY);
                        cameraDisplayFov3.AddTextList(eColor.Magenta, txtPos_Fov3, msgsFov3);

                        var msgsFov4 = new List<string>();
                        msgsFov4.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov4_I.X, intersectFov4_I.Y));
                        msgsFov4.Add(ConvertMaterialPosition(intersectFov4));
                        var txtPos_Fov4 = new ImagePoint(intersectFov4_I.X, intersectFov4_I.Y + txtPosOffsetY);
                        cameraDisplayFov4.AddTextList(eColor.Magenta, txtPos_Fov4, msgsFov4);

                        #region 라인 그리기 불필요하여 주석처리
                        // 교정된 라인 방정식 (픽셀 좌표)
                        //var lineFov1_I_H = new LineEquation(intersectFov1_I.X, intersectFov1_I.Y, cpFov1_I_H.X, cpFov1_I_H.Y);
                        //var lineFov1_I_V = new LineEquation(intersectFov1_I.X, intersectFov1_I.Y, cpFov1_I_V.X, cpFov1_I_V.Y);
                        //var lineFov2_I_H = new LineEquation(intersectFov2_I.X, intersectFov2_I.Y, cpFov2_I_H.X, cpFov2_I_H.Y);
                        //var lineFov2_I_V = new LineEquation(intersectFov2_I.X, intersectFov2_I.Y, cpFov2_I_V.X, cpFov2_I_V.Y);
                        //var lineFov3_I_H = new LineEquation(intersectFov3_I.X, intersectFov3_I.Y, cpFov3_I_H.X, cpFov3_I_H.Y);
                        //var lineFov3_I_V = new LineEquation(intersectFov3_I.X, intersectFov3_I.Y, cpFov3_I_V.X, cpFov3_I_V.Y);
                        //var lineFov4_I_H = new LineEquation(intersectFov4_I.X, intersectFov4_I.Y, cpFov4_I_H.X, cpFov4_I_H.Y);
                        //var lineFov4_I_V = new LineEquation(intersectFov4_I.X, intersectFov4_I.Y, cpFov4_I_V.X, cpFov4_I_V.Y);

                        // 교정된 연장 라인 얻어오기 (이미지의 끝단까지 그림)
                        //var extLineFov1_I_H = GetMinLengthLine(lineFov1_I_H, camConfigFov1.ImageSize);
                        //var extLineFov1_I_V = GetMinLengthLine(lineFov1_I_V, camConfigFov1.ImageSize);
                        //var extLineFov2_I_H = GetMinLengthLine(lineFov2_I_H, camConfigFov2.ImageSize);
                        //var extLineFov2_I_V = GetMinLengthLine(lineFov2_I_V, camConfigFov2.ImageSize);
                        //var extLineFov3_I_H = GetMinLengthLine(lineFov3_I_H, camConfigFov3.ImageSize);
                        //var extLineFov3_I_V = GetMinLengthLine(lineFov3_I_V, camConfigFov3.ImageSize);
                        //var extLineFov4_I_H = GetMinLengthLine(lineFov4_I_H, camConfigFov4.ImageSize);
                        //var extLineFov4_I_V = GetMinLengthLine(lineFov4_I_V, camConfigFov4.ImageSize);

                        // 교정된 라인 그리기
                        //cameraDisplayFov1.AddLine(eColor.Orange, extLineFov1_I_H.StartPoint, extLineFov1_I_H.EndPoint);
                        //cameraDisplayFov1.AddLine(eColor.Orange, extLineFov1_I_V.StartPoint, extLineFov1_I_V.EndPoint);
                        //cameraDisplayFov2.AddLine(eColor.Orange, extLineFov2_I_H.StartPoint, extLineFov2_I_H.EndPoint);
                        //cameraDisplayFov2.AddLine(eColor.Orange, extLineFov2_I_V.StartPoint, extLineFov2_I_V.EndPoint);
                        //cameraDisplayFov3.AddLine(eColor.Orange, extLineFov3_I_H.StartPoint, extLineFov3_I_H.EndPoint);
                        //cameraDisplayFov3.AddLine(eColor.Orange, extLineFov3_I_V.StartPoint, extLineFov3_I_V.EndPoint);
                        //cameraDisplayFov4.AddLine(eColor.Orange, extLineFov4_I_H.StartPoint, extLineFov4_I_H.EndPoint);
                        //cameraDisplayFov4.AddLine(eColor.Orange, extLineFov4_I_V.StartPoint, extLineFov4_I_V.EndPoint);
                        #endregion

                        ClearLineCenterPoints(subSysInfo);
                    }));
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
        }

        private static ImagePoint GetPixelPos(FovInfo fovInfo, StaticPoint staticPoint, ImageSize imageSize) {
            var imagePoint = CalAdapter.Instance.CalManager.TransformToImagePoint(fovInfo, staticPoint, imageSize);

            return new ImagePoint(imagePoint.X, imagePoint.Y);
        }

        public static string ConvertMaterialPosition(StaticPoint intersect) {
            if (VisionSettingManager.VisionSetting.CoordUnit == eCoordUnit.UM) {
                return String.Format(
                    "S({0:0.0}, {1:0.0})",
                    MathUtil.ConvertUnitMilliToMicro(intersect.X),
                    MathUtil.ConvertUnitMilliToMicro(intersect.Y));
            }

            return String.Format(
                    "S({0:0.00}, {1:0.00})",
                    intersect.X,
                    intersect.Y);
        }

        private static Line<ImagePoint> GetMinLengthLine(
            LineEquation lineEquation, ImageSize imageSize) {
            var line1 = new Line<ImagePoint>(
                new ImagePoint(0, lineEquation.GetY(0)),
                new ImagePoint(imageSize.Width, lineEquation.GetY(imageSize.Width)));

            var line2 = new Line<ImagePoint>(
                new ImagePoint(lineEquation.GetX(0), 0),
                new ImagePoint(lineEquation.GetX(imageSize.Height), imageSize.Height));

            return line1.Length < line2.Length ? line1 : line2;
        }
    }
}