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

namespace CogUI {
    internal enum eGridMode {
        None, Narrow, Wide
    }

    public class GridGuideLine {
        private const string KEY_INTERACTIVE_GRID_WIDE = "INTERACTIVE_GRID_WIDE";
        private const string KEY_INTERACTIVE_GRID_NARROW = "INTERACTIVE_GRID_NARROW";
        private const string KEY_INTERACTIVE_REF_LINE = "INTERACTIVE_REF_LINE";
        private const string KEY_INTERACTIVE_COORD_AXES = "KEY_INTERACTIVE_COORD_AXES";
        private const string KEY_INTERACTIVE_LABEL_X = "KEY_INTERACTIVE_LABEL_X";
        private const string KEY_INTERACTIVE_LABEL_Y = "KEY_INTERACTIVE_LABEL_Y";

        private Logger _Logger = new Logger("GridGuideLine");
        private CogDisplay _cogDisplay;
        private FovInfo _fovInfo;
        private eGridMode _gridMode = eGridMode.None;

        public GridGuideLine(CogDisplay cogDisplay, FovInfo fovInfo) {
            _cogDisplay = cogDisplay;
            _fovInfo = fovInfo;
        }

        private CoordSystem GetCameraSetupDirection(FovInfo fovInfo) {
            var camRecipe = VisionRecipeManager.GetCamRecipe(fovInfo);
            var camConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(camRecipe.CamId.ToString()) as CamConfig;
            if (camConfig != null) {
                return camConfig.SetupDirection.ToCoordSystem();
            }
            return null;
        }

        private CoordSystem GetStaticCoordSystem(SubSysInfo subSysInfo) {
            var calRecipe = VisionRecipeManager.GetCalRecipe(subSysInfo);
            return calRecipe?.CoordSystem;
        }

        public void Update(ImageSize imageSize) {
            switch (_gridMode) {
                case eGridMode.Narrow:
                    NarrowOn(imageSize);
                    return;
                case eGridMode.Wide:
                    WideOn(imageSize);
                    return;
                case eGridMode.None:
                    return;
            }
        }

        public void NarrowOn(ImageSize imageSize, bool errRotation = false) {
            Clear();
            var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);

            if (errRotation) {
                DrawGridLines(eGridMode.Narrow, imageSize, calFovRecipe.ErrRotationX);
            }
            else {
                DrawGridLines(eGridMode.Narrow, imageSize);
            }

            DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
            DrawCoordDirection(imageSize);
            _gridMode = eGridMode.Narrow;
        }

        public void WideOn(ImageSize imageSize, bool errRotation = false) {
            Clear();
            var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);

            if (errRotation) {
                DrawGridLines(eGridMode.Wide, imageSize, calFovRecipe.ErrRotationX);
            }
            else {
                DrawGridLines(eGridMode.Wide, imageSize);
            }

            DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
            DrawCoordDirection(imageSize);
            _gridMode = eGridMode.Wide;
        }

        private void DrawGridLines(eGridMode gridMode, ImageSize imageSize, Angle errRotation = null) {
            var centerPos = new ImagePoint(imageSize.Width / 2, imageSize.Height / 2);
            var lineStyle = CogGraphicLineStyleConstants.Solid;

            if (gridMode == eGridMode.Narrow) {
                DrawGuideLines(imageSize, centerPos, 11, lineStyle, CogColorConstants.LightGrey, KEY_INTERACTIVE_GRID_NARROW, errRotation);
                DrawGuideLines(imageSize, centerPos, 1, lineStyle, CogColorConstants.Orange, KEY_INTERACTIVE_GRID_NARROW, errRotation);
            }
            else {
                DrawGuideLines(imageSize, centerPos, 5, lineStyle, CogColorConstants.LightGrey, KEY_INTERACTIVE_GRID_WIDE, errRotation);
                DrawGuideLines(imageSize, centerPos, 1, lineStyle, CogColorConstants.Orange, KEY_INTERACTIVE_GRID_WIDE, errRotation);
            }
        }

        public void DrawRefLine(ImageSize imageSize, Angle errRotation) {
            var refPointRecipe = VisionRecipeManager.GetRefPointRecipe(_fovInfo);
            if (refPointRecipe.Offset.X == 0 && refPointRecipe.Offset.Y == 0) {
                // 레퍼런스 위치가 센터이므로 또 그릴 필요가 없음.
                return;
            }

            var refPos = GetRefPos(imageSize);
            var lineStyle = CogGraphicLineStyleConstants.Dot;
            DrawGuideLines(imageSize, refPos, 1, lineStyle, CogColorConstants.Yellow, KEY_INTERACTIVE_REF_LINE, errRotation);
        }

        private ImagePoint GetRefPos(ImageSize imageSize) {
            var calibrator = CalibratorManager.GetCalibrator(_fovInfo);
            var staticRefPoint = VisionRecipeManager.GetRefPoint(_fovInfo);
            var imageRefPoint = calibrator.TransformToImagePoint(staticRefPoint, imageSize);

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

        public void DrawGuideLines(
            ImageSize imageSize,
            ImagePoint crossPos,
            int lineCount,
            CogGraphicLineStyleConstants lineStyle,
            CogColorConstants color,
            string groupName,
            Angle errRotation = null) {

            try {
                int lineCnt = ConvertToOddNumber(lineCount);

                double[] lineStartX = new double[lineCnt * 2];
                double[] lineStartY = new double[lineCnt * 2];
                double[] lineEndX = new double[lineCnt * 2];
                double[] lineEndY = new double[lineCnt * 2];

                var width = imageSize.Width;
                var horizontalGap = width / (lineCnt + 1);
                var height = imageSize.Height;
                var verticalGap = height / (lineCnt + 1);
                int index = (lineCnt / 2) * -1;

                for (int i = 0; i < lineCnt; i++) {
                    // Vertical
                    var vLineStart = new ImagePoint(crossPos.X + (horizontalGap * index), 0);
                    var vLineEnd = new ImagePoint(vLineStart.X, height);
                    var verticalLine = new CogLineSegment();
                    verticalLine.Color = color;
                    verticalLine.LineStyle = lineStyle;

                    // Horizontal
                    var hLineStart = new ImagePoint(0, crossPos.Y + (verticalGap * index));
                    var hLineEnd = new ImagePoint(width, hLineStart.Y);
                    var horizontalLine = new CogLineSegment();
                    horizontalLine.Color = color;
                    horizontalLine.LineStyle = lineStyle;

                    if (errRotation != null) {  // 카메라 틀어진 각도를 고려한 그리드 라인
                        // Vertical
                        var vLineStartRotated = MathUtil.Rotate(vLineStart, errRotation, crossPos);
                        var vLineEndRotated = MathUtil.Rotate(vLineEnd, errRotation, crossPos);
                        lineStartX[i] = vLineStartRotated.X;
                        lineStartY[i] = vLineStartRotated.Y;
                        lineEndX[i] = vLineEndRotated.X;
                        lineEndY[i] = vLineEndRotated.Y;

                        // Horizontal
                        var hLineStartRotated = MathUtil.Rotate(hLineStart, errRotation, crossPos);
                        var hLineEndRotated = MathUtil.Rotate(hLineEnd, errRotation, crossPos);
                        lineStartX[lineCnt + i] = hLineStartRotated.X;
                        lineStartY[lineCnt + i] = hLineStartRotated.Y;
                        lineEndX[lineCnt + i] = hLineEndRotated.X;
                        lineEndY[lineCnt + i] = hLineEndRotated.Y;
                    }
                    else {
                        // Vertical
                        lineStartX[i] = vLineStart.X;
                        lineStartY[i] = vLineStart.Y;
                        lineEndX[i] = vLineEnd.X;
                        lineEndY[i] = vLineEnd.Y;

                        // Horizontal
                        lineStartX[lineCnt + i] = hLineStart.X;
                        lineStartY[lineCnt + i] = hLineStart.Y;
                        lineEndX[lineCnt + i] = hLineEnd.X;
                        lineEndY[lineCnt + i] = hLineEnd.Y;
                    }

                    verticalLine.SetStartEnd(lineStartX[i], lineStartY[i], lineEndX[i], lineEndY[i]);
                    horizontalLine.SetStartEnd(lineStartX[lineCnt + i], lineStartY[lineCnt + i], lineEndX[lineCnt + i], lineEndY[lineCnt + i]);

                    index++;

                    _cogDisplay.InteractiveGraphics.Add(verticalLine, groupName, false);
                    _cogDisplay.InteractiveGraphics.Add(horizontalLine, groupName, false);
                }
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
        }

        private int ConvertToOddNumber(int number) {
            if (number % 2 == 0) {
                return number + 1;
            }

            return number;
        }

        public void Clear() {
            RemoveInteractiveGraphics(KEY_INTERACTIVE_GRID_WIDE);
            RemoveInteractiveGraphics(KEY_INTERACTIVE_GRID_NARROW);
            RemoveInteractiveGraphics(KEY_INTERACTIVE_REF_LINE);
            RemoveInteractiveGraphics(KEY_INTERACTIVE_COORD_AXES);
            RemoveInteractiveGraphics(KEY_INTERACTIVE_LABEL_X);
            RemoveInteractiveGraphics(KEY_INTERACTIVE_LABEL_Y);
            _gridMode = eGridMode.None;
        }

        private void RemoveInteractiveGraphics(string key) {
            try {
                int index = _cogDisplay.InteractiveGraphics.FindItem(key, CogDisplayZOrderConstants.Front);
                if (index != -1) {
                    _cogDisplay.InteractiveGraphics.Remove(key);
                }
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
        }

        public void DrawCoordDirection(ImageSize imageSize) {
            DrawCoordDirection(
                GetCameraSetupDirection(_fovInfo),
                GetStaticCoordSystem(_fovInfo.ToSubSysInfo()),
                new ImagePoint(imageSize.Width - 150, 150), "S");
        }

        private void DrawCoordDirection(CoordSystem imageCoordSystem, CoordSystem staticCoordSystem, ImagePoint drawPosition, string prefix) {
            if (imageCoordSystem != null && staticCoordSystem != null) {
                var corrCS = staticCoordSystem - imageCoordSystem;
                var corrRotX = GetCorrectionRotationX(imageCoordSystem, staticCoordSystem);
                var corrFlipY = (int)GetCorrectionFlipY(imageCoordSystem, staticCoordSystem);

                var axisX = new ImagePoint(100, 0);
                var axisY = new ImagePoint(0, 100);
                axisX = MathUtil.Rotate(axisX, corrRotX) as ImagePoint;
                axisY = MathUtil.Rotate(axisY, corrRotX) * corrFlipY as ImagePoint;

                DisplayDrawer.AddVectors(_cogDisplay, CogColorConstants.Orange,
                    drawPosition,
                    new List<ImagePoint>() { axisX, axisY },
                    KEY_INTERACTIVE_COORD_AXES);

                var textOffset = -20;
                var textX = new ImagePoint(100 + textOffset, 50 - textOffset);
                var textY = new ImagePoint(0 - textOffset, 150 + textOffset);
                textX = (MathUtil.Rotate(textX, corrRotX) + drawPosition) as ImagePoint;
                textY = (MathUtil.Rotate(textY, corrRotX) * corrFlipY + drawPosition) as ImagePoint;

                DisplayDrawer.AddText(_cogDisplay, CogColorConstants.Orange, textX, prefix + "X", KEY_INTERACTIVE_LABEL_X);
                DisplayDrawer.AddText(_cogDisplay, CogColorConstants.Orange, textY, prefix + "Y", KEY_INTERACTIVE_LABEL_Y);
            }
        }

        public Angle GetCorrectionRotationX(CoordSystem sourceCS, CoordSystem targetCS) {
            return Angle.CreateWithDegree(
                (targetCS.RotationX.Degree - sourceCS.RotationX.Degree) * (int)sourceCS.FlipY);
        }

        public eFlipY GetCorrectionFlipY(CoordSystem sourceCS, CoordSystem targetCS) {
            return (eFlipY)((int)targetCS.FlipY * (int)sourceCS.FlipY);
        }
    }
}