Kyungshin-Welder/AuroraVision/MilPattern/Edge/MilEdge.cs

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

namespace MilPattern {
    public enum eMilEdgeThresholdMode {
        Default = MIL.M_DEFAULT,
        High = MIL.M_HIGH,
        Low = MIL.M_LOW,
        Medium = MIL.M_MEDIUM,
        VeryHigh = MIL.M_VERY_HIGH,
        UserDefined = MIL.M_USER_DEFINED
    }

    public class EdgeFinderRunParam : IRunParam, ICloneable {
        public double FilterSmoothness { get; set; } = 50.0;
        public eMilEdgeThresholdMode ThresholdMode { get; set; } = eMilEdgeThresholdMode.Default;
        public int ThresholdLow { get; set; } = 0;
        public int ThresholdHigh { get; set; } = 100;
        public EdgeParamExt EdgeParamExt { get; set; } = new EdgeParamExt();

        public bool IsValidFindRoi(ImageSize imageSize) {
            return (0 < EdgeParamExt.FindROI.Width && EdgeParamExt.FindROI.Width < imageSize.Width) &&
                (0 < EdgeParamExt.FindROI.Height && EdgeParamExt.FindROI.Height < imageSize.Height);
        }

        public ROI GetFindRoi() {
            return EdgeParamExt.FindROI.Clone() as ROI;
        }

        public void SetFindROI(ROI findRoi) {
            EdgeParamExt.FindROI = findRoi;
        }

        public object Clone() {
            return new EdgeFinderRunParam() {
                FilterSmoothness = FilterSmoothness,
                ThresholdMode = ThresholdMode,
                ThresholdLow = ThresholdLow,
                ThresholdHigh = ThresholdHigh,
                EdgeParamExt = EdgeParamExt.Clone() as EdgeParamExt
            };
        }
    }

    internal static class MilEdge {
        private static Logger _Logger = new Logger(typeof(MilEdge).Name);

        public static MIL_ID CreateContext(MIL_ID milSystem) {
            MIL_ID contextId = MIL.M_NULL;
            MIL.MedgeAlloc(milSystem, MIL.M_CREST, MIL.M_DEFAULT, ref contextId);

            return contextId;
        }

        public static MIL_ID LoadContext(MIL_ID milSystem, string filePath) {
            MIL_ID contextId = MIL.M_NULL;
            MIL.MedgeRestore(filePath, milSystem, MIL.M_DEFAULT, ref contextId);

            return contextId;
        }

        public static int SaveContext(MIL_ID contextId, string filePath) {
            if (contextId != MIL.M_NULL && filePath != null) {
                MIL.MedgeSave(filePath, contextId, MIL.M_DEFAULT);
                return E1.OK;
            }

            return E1.GENERIC;
        }

        public static void FreeContext(MIL_ID contextId) {
            if (contextId != MIL.M_NULL) {
                MIL.MedgeFree(contextId);
            }
        }

        public static MIL_ID CreateResultContext(MIL_ID contextId) {
            var milSystem = GetSystemOfContext(contextId);

            return MIL.MedgeAllocResult(milSystem, MIL.M_DEFAULT, MIL.M_NULL);
        }

        private static MIL_ID GetSystemOfContext(MIL_ID contextId) {
            return (MIL_ID)MIL.MedgeInquire(contextId, MIL.M_OWNER_SYSTEM, MIL.M_NULL);
        }

        public static EdgeFinderRunParam GetRunParam(MIL_ID contextId) {
            var edgeFinderRunParam = new EdgeFinderRunParam();

            double filterSmoothness = 0;
            MIL.MedgeInquire(contextId, MIL.M_FILTER_SMOOTHNESS, ref filterSmoothness);
            if (filterSmoothness != MIL.M_DEFAULT) {
                edgeFinderRunParam.FilterSmoothness = filterSmoothness;
            }

            double thresholdLow = 0;
            MIL.MedgeInquire(contextId, MIL.M_THRESHOLD_LOW, ref thresholdLow);
            if (thresholdLow != MIL.M_DEFAULT) {
                edgeFinderRunParam.ThresholdLow = (int)thresholdLow;
            }

            double thresholdHigh = 0;
            MIL.MedgeInquire(contextId, MIL.M_THRESHOLD_HIGH, ref thresholdHigh);
            if (thresholdHigh != MIL.M_DEFAULT) {
                edgeFinderRunParam.ThresholdHigh = (int)thresholdHigh;
            }

            edgeFinderRunParam.ThresholdMode =
                (eMilEdgeThresholdMode)Convert.ToInt32(MIL.MedgeInquire(contextId, MIL.M_THRESHOLD_MODE, MIL.M_NULL));

            return edgeFinderRunParam;
        }

        public static int SetRunParam(MIL_ID contextId, IRunParam runParam) {
            if (runParam is EdgeFinderRunParam) {
                var param = runParam as EdgeFinderRunParam;
                MIL.MedgeControl(contextId, MIL.M_FILTER_SMOOTHNESS, param.FilterSmoothness);
                MIL.MedgeControl(contextId, MIL.M_THRESHOLD_MODE, (double)param.ThresholdMode);
                MIL.MedgeControl(contextId, MIL.M_THRESHOLD_LOW, param.ThresholdLow);
                MIL.MedgeControl(contextId, MIL.M_THRESHOLD_HIGH, param.ThresholdHigh);

                return E1.OK;
            }

            return E1.GENERIC;
        }

        public static PatternResult Find(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, IRunParam runParam) {
            SetRunParam(contextId, runParam);

            return Find(contextId, resultContextId, image);
        }

        public static PatternResult Find(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image) {
            MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_RESET + MIL.M_SYNCHRONOUS, MIL.M_NULL);

            MIL.MedgeCalculate(contextId, image, MIL.M_NULL, MIL.M_NULL, MIL.M_NULL, resultContextId, MIL.M_DEFAULT);

            double tactTime = 0;
            MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, ref tactTime);

            var finderResult = ExtractResults(resultContextId);
            finderResult.TactTimeInMilliSec = MathUtil.ConvertUnitSecToMilli(tactTime);

            return finderResult;
        }

        private static PatternResult ExtractResults(MIL_ID contextResult) {
            var results = new List<EdgeFinderResult>();
            MIL_INT numOfResult = 0;

            MIL.MedgeGetResult(contextResult, MIL.M_DEFAULT, MIL.M_NUMBER_OF_CHAINS + MIL.M_TYPE_MIL_INT, ref numOfResult);

            if (0 < numOfResult) {
                // To-Do : MedgeGetResult
                results.Add(new EdgeFinderResult(new ImagePoint(0, 0), new Angle(), 0, 0, 0));
            }

            return new PatternResult() {
                Status = (results.Count > 0) ? eFoundStatus.FoundOK : eFoundStatus.NotFound,
                Results = results
            };
        }

        public static PatternResult GetNotFoundPatternResult() {
            return new PatternResult() {
                Status = eFoundStatus.NotFound,
                Results = new List<EdgeFinderResult>()
            };
        }

        public static void Draw(PatternResult patternResult, IMilCameraDisplay milCameraDisplay) {
            MIL_ID staticGraphicList = 0;
            MIL.MdispInquire(milCameraDisplay.GetMilDisplayId(), MIL.M_ASSOCIATED_GRAPHIC_LIST_ID, ref staticGraphicList);

            if (patternResult.Status == eFoundStatus.FoundOK) {
                var results = patternResult.Results as List<EdgeFinderResult>;
                var crosses = new List<Cross>();

                foreach (var result in results) {
                    var color = GraphicColors.PatternFoundOK;
                    var cross = new Cross(result.ImagePosition.Clone() as ImagePoint, 50);
                    crosses.Add(cross);

                    milCameraDisplay.SetPatternTracking(cross);
                    milCameraDisplay.AddCross(cross, color);
                }
                milCameraDisplay.DrawPatternTrackings(crosses);
            }
        }
    }
}