using Aurora.Api;
using MilCommon;
using VisionCommon;
using Hermes;
using Matrox.MatroxImagingLibrary;
using System;
using System.Collections.Generic;
using System.Windows;
using Aurora.Api.DataType;
using System.Windows.Controls.Primitives;

namespace MilPattern {
    public class ModelFinderRunParam : IRunParam, ICloneable {
        public int FindNumber { get; set; } = 1;
        public double Acceptance { get; set; } = 60;
        public double Certainty { get; set; } = 90;
        public Angle AngleTolerance { get; set; } = Angle.CreateWithDegree(180);    // (0, 180)
        public double ScaleTolerance { get; set; } = 0.2;
        public eMilAccuracy Accuracy { get; set; } = eMilAccuracy.Default;
        public eMilDetailLevel DetailLevel { get; set; } = eMilDetailLevel.Default;
        public eMilSpeed Speed { get; set; } = eMilSpeed.Default;
        public double Score { get; set; } = 60;
        public eMilPolarity Polarity { get; set; } = eMilPolarity.Any;
        public MmfParamExt MmfParamExt { get; set; } = new MmfParamExt();
        public double Smoothness { get; set; } = 50.0;
        public bool IsValidFindRoi(ImageSize imageSize) {
            return (0 < MmfParamExt.FindROI.Width && MmfParamExt.FindROI.Width < imageSize.Width) &&
                (0 < MmfParamExt.FindROI.Height && MmfParamExt.FindROI.Height < imageSize.Height);
        }

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

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

        public object Clone() {
            return new ModelFinderRunParam() {
                FindNumber = FindNumber,
                Acceptance = Acceptance,
                Certainty = Certainty,
                AngleTolerance = AngleTolerance.Clone() as Angle,
                ScaleTolerance = ScaleTolerance,
                Accuracy = Accuracy,
                DetailLevel = DetailLevel,
                Speed = Speed,
                Score = Score,
                Polarity = Polarity,
                Smoothness = Smoothness,
                MmfParamExt = MmfParamExt.Clone() as MmfParamExt
            };
        }
    }

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

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

            return contextId;
        }

        public static MIL_ID CreateQuickFindContext(MIL_ID milSystem) {
            MIL_ID contextId = MIL.M_NULL;
            MIL.MmodAlloc(milSystem, MIL.M_GEOMETRIC_CONTROLLED, MIL.M_DEFAULT, ref contextId);

            return contextId;
        }

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

            if (HasModel(contextId)) {
                MIL.MmodPreprocess(contextId, MIL.M_DEFAULT);
            }

            return contextId;
        }

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

            return E1.GENERIC;
        }

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

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

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

        public static MIL_ID GetSystemOfContext(MIL_ID contextId) {
            return (MIL_ID)MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_OWNER_SYSTEM, MIL.M_NULL);
        }

        public static bool IsPreprocessed(MIL_ID contextId) {
            if (contextId == MIL.M_NULL) {
                return false;
            }

            return MIL.M_TRUE == MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_PREPROCESSED, MIL.M_NULL);
        }

        public static void DeleteAllModels(MIL_ID contextId) {
            if (HasModel(contextId)) {
                MIL.MmodDefine(contextId, MIL.M_DELETE, MIL.M_ALL, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT);
            }
        }

        public static bool HasModel(MIL_ID contextId) {
            return MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_NUMBER_MODELS, MIL.M_NULL) > 0;
        }

        public static ROI GetFindRoiFromMmf(MIL_ID contextId) {
            if (HasModel(contextId)) {
                double centerX = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_X, ref centerX);
                double centerY = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_Y, ref centerY);

                if (centerX != MIL.M_DEFAULT || centerY != MIL.M_DEFAULT) {
                    double deltaPosX = 0;
                    MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_POS_X, ref deltaPosX);
                    double deltaNegX = 0;
                    MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_NEG_X, ref deltaNegX);
                    double deltaPosY = 0;
                    MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_POS_Y, ref deltaPosY);
                    double deltaNegY = 0;
                    MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_NEG_Y, ref deltaNegY);

                    return new ROI(
                        new ImagePoint(centerX, centerY),
                        deltaPosX + deltaNegX,
                        deltaPosY + deltaNegY);
                }
            }

            return new ROI();
        }

        public static int SetFindRoiToMmf(MIL_ID contextId, ROI findRoi) {
            if (findRoi.Width > 0 && findRoi.Height > 0) {
                if (HasModel(contextId)) {
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_X, findRoi.CenterX);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_X, findRoi.Width * 0.5);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_X, findRoi.Width * 0.5);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_Y, findRoi.CenterY);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_Y, findRoi.Height * 0.5);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_Y, findRoi.Height * 0.5);
                }
            }
            else {
                ClearFindRoi(contextId);
            }

            return E1.OK;
        }

        public static int ClearFindRoi(MIL_ID contextId) {
            if (HasModel(contextId)) {
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_X, MIL.M_DEFAULT);
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_X, MIL.M_DEFAULT);
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_X, MIL.M_DEFAULT);
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_Y, MIL.M_DEFAULT);
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_Y, MIL.M_DEFAULT);
                MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_Y, MIL.M_DEFAULT);
            }

            return E1.OK;
        }

        public static ModelFinderRunParam GetRunParam(MIL_ID contextId) {
            var modelFinderRunParam = new ModelFinderRunParam();

            if (HasModel(contextId)) {
                double acceptance = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_ACCEPTANCE, ref acceptance);
                if (acceptance != MIL.M_DEFAULT) {
                    modelFinderRunParam.Acceptance = acceptance;
                }

                double certainty = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_CERTAINTY, ref certainty);
                if (certainty != MIL.M_DEFAULT) {
                    modelFinderRunParam.Certainty = certainty;
                }

                double angleDeg = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_ANGLE_DELTA_POS, ref angleDeg);
                if (angleDeg != MIL.M_DEFAULT) {
                    modelFinderRunParam.AngleTolerance = Angle.CreateWithDegree(angleDeg);
                }

                double scaleTolerance = 0;
                MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_SCALE_MAX_FACTOR, ref scaleTolerance);
                modelFinderRunParam.ScaleTolerance = scaleTolerance - 1;

                modelFinderRunParam.Smoothness = Convert.ToDouble(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_SMOOTHNESS, MIL.M_NULL));

                modelFinderRunParam.Accuracy =
                    (eMilAccuracy)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_ACCURACY, MIL.M_NULL));
                modelFinderRunParam.DetailLevel =
                    (eMilDetailLevel)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_DETAIL_LEVEL, MIL.M_NULL));
                modelFinderRunParam.Speed =
                    (eMilSpeed)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_SPEED, MIL.M_NULL));
                modelFinderRunParam.FindNumber = Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_NUMBER, MIL.M_NULL));
                modelFinderRunParam.Polarity =
                    (eMilPolarity)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POLARITY, MIL.M_NULL));
            }

            return modelFinderRunParam;
        }

        public static int SetRunParam(MIL_ID contextId, IRunParam runParam) {
            if (runParam is ModelFinderRunParam) {
                var param = runParam as ModelFinderRunParam;
                if (HasModel(contextId)) {
                    MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_ACCURACY, (double)param.Accuracy);
                    MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_DETAIL_LEVEL, (double)param.DetailLevel);
                    MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_SPEED, (double)param.Speed);

                    MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_SMOOTHNESS, (double)param.Smoothness);

                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_NUMBER, param.FindNumber);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ACCEPTANCE, param.Acceptance);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_CERTAINTY, param.Certainty);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ANGLE_DELTA_POS, param.AngleTolerance.Degree);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ANGLE_DELTA_NEG, param.AngleTolerance.Degree);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_SCALE_MIN_FACTOR, 1 - param.ScaleTolerance);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_SCALE_MAX_FACTOR, 1 + param.ScaleTolerance);
                    MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POLARITY, (double)param.Polarity);

                    ClearFindRoi(contextId);
                    MIL.MmodPreprocess(contextId, MIL.M_DEFAULT);
                }
                return E1.OK;
            }

            return E1.GENERIC;
        }

        public static PatternResult Find(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi, bool isShape = false) {
            if (MilEnvironment.IsColorImage(image)) {
                var grayImageId = MilEnvironment.ConvertToGray(image);
                var patternResult = FindInternal(contextId, resultContextId, grayImageId, findRoi, isShape);
                MilEnvironment.FreeImage(grayImageId);

                return patternResult;
            }
            return FindInternal(contextId, resultContextId, image, findRoi, isShape);
        }

        private static PatternResult FindInternal(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi, bool isShape) {
            PatternResult finderResult;
            MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_RESET + MIL.M_SYNCHRONOUS, MIL.M_NULL);

            if (HasModel(contextId)) {
                if (ROI.IsNullOrEmpty(findRoi)) {
                    MIL.MmodFind(contextId, image, resultContextId);
                    finderResult = ExtractResults(contextId, resultContextId, MilEnvironment.GetImageCenter(image), findRoi, isShape);
                }
                else {
                    var fittedFindRoi = FindWithCropImage(contextId, resultContextId, image, findRoi);
                    finderResult = ExtractResults(contextId, resultContextId, MilEnvironment.GetImageCenter(image), fittedFindRoi, isShape);
                }
            }
            else {
                finderResult = GetNotFoundPatternResult();
            }

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

            return finderResult;
        }

        private static ROI FindWithCropImage(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi) {
            MIL_ID cropImage = MIL.M_NULL;
            ROI fittedFindRoi = findRoi.Clone() as ROI;

            try {
                var imageSzie = MilEnvironment.GetImageSize(image);
                if (ROI.IsOutsideImage(findRoi, imageSzie)) {
                    fittedFindRoi = ROI.FitRoiToImage(findRoi, imageSzie);
                }

                cropImage = MilEnvironment.CropImage(image, fittedFindRoi);
                MIL.MmodFind(contextId, cropImage, resultContextId);
            }
            catch (Exception e) {
                _Logger.E(e.Message);
            }
            finally {
                MilEnvironment.FreeImage(cropImage);
            }

            return fittedFindRoi;
        }

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

        private static PatternResult ExtractResults(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi, bool isShape) {
            if (isShape) {
                List<ShapeModelFinderResult> results = GetShapeResult(contextId, contextResult, imageCenter, findRoi);
                return new PatternResult() {
                    Status = (results.Count > 0) ? eFoundStatus.FoundOK : eFoundStatus.NotFound,
                    Results = results
                };
            }
            else {
                List<ModelFinderResult> results = GetModResult(contextId, contextResult, imageCenter, findRoi);
                return new PatternResult() {
                    Status = (results.Count > 0) ? eFoundStatus.FoundOK : eFoundStatus.NotFound,
                    Results = results
                };
            }
        }

        private static List<ShapeModelFinderResult> GetShapeResult(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi) {
            var results = new List<ShapeModelFinderResult>();
            List<ModelFinderResult> modResults = GetModResult(contextId, contextResult, imageCenter, findRoi);
            var runParam = GetRunParam(contextId);
            for (int resultIndex = 0; resultIndex < modResults.Count; resultIndex++) {
                double width = 0, height = 0, radius = 0, innerRadius = 0, outerRadius = 0;
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_WIDTH, ref width);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_HEIGHT, ref height);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_RADIUS, ref radius);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_INNER_RADIUS, ref innerRadius);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_OUTER_RADIUS, ref outerRadius);

                if (Math.Abs(modResults[resultIndex].Angle.Degree) < runParam.AngleTolerance.Degree || 360 - modResults[resultIndex].Angle.Degree < runParam.AngleTolerance.Degree) {
                    results.Add(new ShapeModelFinderResult(
                        position: modResults[resultIndex].ImagePosition,
                        offset: modResults[resultIndex].Offset,
                        angle: modResults[resultIndex].Angle,
                        score: modResults[resultIndex].Score,
                        width: width,
                        height: height,
                        radius: radius,
                        innerRadius : innerRadius,
                        outerRadius : outerRadius));
                }
            }
            return results;
        }

        private static List<ModelFinderResult> GetModResult(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi) {
            var results = new List<ModelFinderResult>();
            MIL_INT numOfResult = 0;
            MIL.MmodGetResult(contextResult, MIL.M_DEFAULT, MIL.M_NUMBER + MIL.M_TYPE_MIL_INT, ref numOfResult);
            var runParam = GetRunParam(contextId);

            for (int resultIndex = 0; resultIndex < numOfResult; resultIndex++) {
                double posX = 0, posY = 0;
                double score = 0;
                double angleDeg = 0;
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_POSITION_X, ref posX);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_POSITION_Y, ref posY);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_SCORE, ref score);
                MIL.MmodGetResult(contextResult, resultIndex, MIL.M_ANGLE, ref angleDeg); // Warning : Counter Clockwise

                posX += findRoi.Left;
                posY += findRoi.Top;

                if (Math.Abs(angleDeg) < runParam.AngleTolerance.Degree || Math.Abs(360 - angleDeg) < runParam.AngleTolerance.Degree) {
                    results.Add(new ModelFinderResult(
                        position: new ImagePoint(posX, posY),
                        offset: (new ImagePoint(posX, posY) - imageCenter) as ImagePoint,
                        angle: Angle.NormalizeAngle(Angle.CreateWithDegree(angleDeg)),
                        score: score));
                }
            }

            return results;
        }

        public static void Draw(
            PatternResult patternResult, IMilCameraDisplay milCameraDisplay, bool isFixturePattern, MIL_ID resultContextId, ROI roi) {
            if (patternResult == null || patternResult.Results == null) {
                return;
            }

            var crosses = new List<Cross>();

            foreach (BaseResult baseResult in patternResult.Results) {
                var result = baseResult as ModelFinderResult;

                if (result.Score > 0) {
                    var color = GraphicColors.GetFindResultColor(patternResult.Status, isFixturePattern);
                    // MIL : 시계 방향(-), 반시계 방향(+)
                    var cross = new Cross(result.ImagePosition.Clone() as ImagePoint, -result.Angle, 50);
                    crosses.Add(cross);

                    milCameraDisplay.SetPatternTracking(cross);
                    milCameraDisplay.AddCross(cross, color);
                    milCameraDisplay.AddPatternEdge(resultContextId, roi);

                    var msgs = new List<string>();
                    msgs.Add(string.Format("I({0:0.0}, {1:0.0})", result.ImagePosition.X, result.ImagePosition.Y));
                    msgs.Add(result.ConvertMaterialPosition());
                    msgs.Add(string.Format("Angle : {0:0.00}", result.Angle.Degree));

                    if (patternResult.PatternType == ePatternType.ShapeModelFinder) {
                        var shapeResult = baseResult as ShapeModelFinderResult;
                        if (shapeResult.Width != 0 && shapeResult.Height != 0) {
                            msgs.Add(string.Format("Width : {0:0.00}", shapeResult.Width));
                            msgs.Add(string.Format("Height : {0:0.00}", shapeResult.Height));
                        }
                        if (shapeResult.Radius != 0) {
                            msgs.Add(string.Format("Radius : {0:0.00}", shapeResult.Radius));
                        }
                        if (shapeResult.InnerRadius != 0) {
                            msgs.Add(string.Format("Inner Radius : {0:0.00}", shapeResult.InnerRadius));
                        }
                        if (shapeResult.OuterRadius != 0) {
                            msgs.Add(string.Format("Outer Radius : {0:0.00}", shapeResult.OuterRadius));
                        }
                    }

                    msgs.Add(string.Format("Score : {0:0.00}", result.Score));
                    msgs.Add(string.Format("TactTime : {0} ms", Math.Round(patternResult.TactTimeInMilliSec)));
                    msgs.Add(string.Format("ID : {0}_{1}", patternResult.PatternIndex.PatternId, patternResult.PatternIndex.PatternSubId));

                    milCameraDisplay.AddTextList(
                        color, new ImagePoint(result.ImagePosition.X, result.ImagePosition.Y), msgs);
                }
            }
            milCameraDisplay.DrawPatternTrackings(crosses);
        }
    }
}