/*
  * Copyright (c) 2025 LG Electronics Inc.
   * SPDX-License-Identifier: LicenseRef-LGE-Proprietary
   */
using Aurora.Api;
using VisionCommon;
using System.Windows.Input;
using Aurora.View;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Windows.Data;
using System.Linq;
using System.Windows.Media;
using VisionAligner;
using VisionCalibrator;
using DevExpress.Mvvm.Native;
using System.Text;
using DevExpress.Office.Utils;
using DevExpress.Xpf.Diagram.Native;

namespace VisionUI.ProcTest {
    /// <summary>
    /// 자재 공차 테스트 뷰 모델
    /// </summary>
    class MaterialToleranceTestViewModel : Aurora.View.NotifyPropertyChangedBase {
        private Logger _Logger = new Logger(typeof(MaterialToleranceTestViewModel).Name);

        private SubSysInfo _SubSysInfo;

        private ObservableCollection<MaterialToleranceTestModel> canvasMainModelShapes;
        /// <summary>
        /// 캔번스 메인모델 모양 컬렉션(자재공차테스트모델)
        /// </summary>
        public ObservableCollection<MaterialToleranceTestModel> CanvasMainModelShapes {
            get => canvasMainModelShapes;
            set {
                SetProperty(ref canvasMainModelShapes, value);
            }
        }
        /// <summary>
        /// 원의 포인트
        /// </summary>
        /// <param name="name">이름</param>
        /// <returns>원 모델</returns>
        public CircleModel CirclePoint(string name) {
            MaterialToleranceTestModel m;
            GetShape(name, out m);
            return m as CircleModel;
        }
        private MaterialToleranceTestModel _refPos;
        /// <summary>
        /// 기준 위치
        /// </summary>
        public PathModel RefPos {
            get {
                GetShape("ref", out _refPos);
                return _refPos as PathModel;
            }
        }
        private MaterialToleranceTestModel _meaPos;
        /// <summary>
        /// 측정 위치
        /// </summary>
        public PathModel MeaPos {
            get {
                GetShape("mea", out _meaPos);
                return _meaPos as PathModel;
            }
        }
        private MaterialToleranceTestModel _norPos;
        /// <summary>
        ///  레퍼런스 위치
        /// </summary>
        public PathModel NorPos {
            get {
                GetShape("nor", out _norPos);
                return _norPos as PathModel;
            }
        }
        private MaterialToleranceTestModel _icpPos;
        /// <summary>
        /// ICP 위치
        /// </summary>
        public PathModel IcpPos {
            get {
                GetShape("icp", out _icpPos);
                return _icpPos as PathModel;
            }
        }
        // Recipe Info
        private string _alignType;
        /// <summary>
        /// 얼라인 타입
        /// </summary>
        public string AlignType {
            get => _alignType;
            set { SetProperty(ref _alignType, value); }
        }
        private double _centerX;
        /// <summary>
        /// 센터 X 위치 ( Center Point Properties )
        /// </summary>
        public double CenterX {
            get => _centerX;
            set { SetProperty(ref _centerX, value); }
        }

        private double _centerY;

        /// <summary>
        /// 센터  Y 위치 ( Center Point Properties )
        /// </summary>
        public double CenterY {
            get => _centerY;
            set { SetProperty(ref _centerY, value); }
        }
        private string _infoText;
        /// <summary>
        /// 정보 텍스트
        /// </summary>
        public string InfoText {
            get => _infoText;
            set { SetProperty(ref _infoText, value); }
        }
        private bool _isChecked_n;
        /// <summary>
        /// 레퍼런스 확인 여부
        /// </summary>
        public bool IsChecked_N {
            get => _isChecked_n;
            set {
                SetProperty(ref _isChecked_n, value);
                if (value && NorPos != null) {
                    NorPos.ShapeVisible = System.Windows.Visibility.Visible;
                }
                else {
                    NorPos.ShapeVisible = System.Windows.Visibility.Hidden;
                }
                CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
            }
        }
        private bool _isChecked;
        /// <summary>
        /// ICP 타입인지 확인여부
        /// </summary>
        public bool IsChecked_I {
            get => _isChecked;
            set {
                SetProperty(ref _isChecked, value);
                if (value && IcpPos != null) {
                    IcpPos.ShapeVisible=System.Windows.Visibility.Visible;
                }
                else {
                    IcpPos.ShapeVisible = System.Windows.Visibility.Hidden;
                }
                CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
            }
        }
        private bool _isChecked_m;
        /// <summary>
        /// 측정포인트 타입인지 확인여부
        /// </summary>
        public bool IsChecked_M {
            get => _isChecked_m;
            set {
                SetProperty(ref _isChecked_m, value);
                if (value && MeaPos != null) {
                    MeaPos.ShapeVisible = System.Windows.Visibility.Visible;
                }
                else {
                    MeaPos.ShapeVisible = System.Windows.Visibility.Hidden;
                }
                CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
            }
        }
        // Measure Point Properties
        private double _x1, _y1, _x2, _y2, _x3, _y3, _x4, _y4, _ax, _ay, _at, _ox, _oy, rmse, rmse_icp;
        private double _axa, _aya, _ata, _axi, _ayi,_ati;
        /// <summary>측정 포인트 1의 X</summary>
        public double MeasureX1 { get => _x1; set { SetProperty(ref _x1, value); } }
        /// <summary>측정 포인트 1의 Y</summary>
        public double MeasureY1 { get => _y1; set { SetProperty(ref _y1, value); } }
        /// <summary>측정 포인트 2의 X</summary>
        public double MeasureX2 { get => _x2; set { SetProperty(ref _x2, value); } }
        /// <summary>측정 포인트 2의 Y</summary>
        public double MeasureY2 { get => _y2; set { SetProperty(ref _y2, value); } }
        /// <summary>측정 포인트 3의 X</summary>
        public double MeasureX3 { get => _x3; set { SetProperty(ref _x3, value); } }
        /// <summary>측정 포인트 3의 Y</summary>
        public double MeasureY3 { get => _y3; set { SetProperty(ref _y3, value); } }
        /// <summary>측정 포인트 4의 X</summary>
        public double MeasureX4 { get => _x4; set { SetProperty(ref _x4, value); } }
        /// <summary>측정 포인트 4의 Y</summary>
        public double MeasureY4 { get => _y4; set { SetProperty(ref _y4, value); } }
        /// <summary>얼라인 이동을 위한 dX</summary>
        public double AlignX { get => _ax; set { SetProperty(ref _ax, value); } }
        /// <summary>얼라인 이동을 위한 dY</summary>
        public double AlignY { get => _ay; set { SetProperty(ref _ay, value); } }
        /// <summary>얼라인 이동을 위한 dT</summary>
        public double AlignT { get => _at; set { SetProperty(ref _at, value); } }
        /// <summary>얼라인 결과값 dX (기존)</summary>
        public double AlignX_A { get => _axa; set { SetProperty(ref _axa, value); } }
        /// <summary>얼라인 결과값 dY (기존)</summary>
        public double AlignY_A { get => _aya; set { SetProperty(ref _aya, value); } }
        /// <summary>얼라인 결과값 dT (기존)</summary>
        public double AlignT_A { get => _ata; set { SetProperty(ref _ata, value); } }
        /// <summary>얼라인 결과값 dX (ICP)</summary>
        public double AlignX_I { get => _axi; set { SetProperty(ref _axi, value); } }
        /// <summary>얼라인 결과값 dY (ICP)</summary>
        public double AlignY_I { get => _ayi; set { SetProperty(ref _ayi, value); } }
        /// <summary>얼라인 결과값 dT (ICP)</summary>
        public double AlignT_I { get => _ati; set { SetProperty(ref _ati, value); } }
        /// <summary>Theta 기준점 X</summary>
        public double ThetaOrigin_X { get => _ox; set { SetProperty(ref _ox, value); } }
        /// <summary>Theta 기준점 Y</summary>
        public double ThetaOrigin_Y { get => _oy; set { SetProperty(ref _oy, value); } }
        /// <summary>기존 알고리즘 평균 제곱근 편차 </summary>
        public double RMSE { get => rmse; set { SetProperty(ref rmse, value); } }
        /// <summary>ICP 알고리즘 평균 제곱근 편차 </summary>
        public double RMSE_ICP { get => rmse_icp; set { SetProperty(ref rmse_icp, value); } }

        // Count Of FOV
        private string _f1, _f2, _f3, _f4;
        /// <summary>첫번째 Fov</summary>
        public string Fov1 { get => _f1; set { SetProperty(ref _f1, value); } }
        /// <summary>두번째 Fov</summary>
        public string Fov2 { get => _f2; set { SetProperty(ref _f2, value); } }
        /// <summary>세번째 Fov</summary>
        public string Fov3 { get => _f3; set { SetProperty(ref _f3, value); } }
        /// <summary>네번째 Fov</summary>
        public string Fov4 { get => _f4; set { SetProperty(ref _f4, value); } }


        // Command
        /// <summary>적용 커맨드</summary>
        public ICommand ApplyCommand { get; }
        /// <summary>얼라인 커맨드</summary>
        public ICommand AlignCommand { get; }
        /// <summary>리셋 커맨드</summary>
        public ICommand ResetCommand { get; }
        /// <summary>이동 커맨드</summary>
        public ICommand MoveCommand { get; }

        private List<StaticPoint> refPoints = new List<StaticPoint>();
        private StaticPoint thetaOrigin = null;
        private AlignDelta alignDelta = new AlignDelta();
        private VisionAligner.Aligner _Aligner;
        private AlignRecipe _AlignRecipe = null;
        private Calibrator _Calibrator;
        private int FOV_count = 0;

        private double cx = 400;
        private double cy = 300;

        private (double x, double y, double t) _align_res;
        private (double x, double y, double t) _align_res_icp;
        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="subSysInfo">서브시스템 정보</param>
        public MaterialToleranceTestViewModel(SubSysInfo subSysInfo) {
            _SubSysInfo = subSysInfo;
            CanvasMainModelShapes = new ObservableCollection<MaterialToleranceTestModel>();
            thetaOrigin = VisionRecipeManager.GetThetaOrigin(_SubSysInfo);
            _Aligner = AlignAgentManager.GetAlignAgent(new AlignInfo(subSysInfo, eAlignId.Align1, "Align1"));
            _AlignRecipe = VisionRecipeManager.GetAlignRecipe(new AlignInfo(subSysInfo, eAlignId.Align1, "Align1"));
            var fovInfos = VisionRecipeManager.GetFovInfos(_SubSysInfo);
            _Calibrator = CalibratorManager.GetCalibrator(fovInfos[(int)eFovId.Fov1]);
            ApplyCommand = new DelegateCommand(Apply);
            AlignCommand = new DelegateCommand(Align);
            ResetCommand = new DelegateCommand(Reset);
            MoveCommand = new DelegateCommand(Move);

            CanvasMainModelShapes.Add(new PathModel("ref", ""));
            CanvasMainModelShapes.Add(new PathModel("nor", ""));
            CanvasMainModelShapes.Add(new PathModel("icp", ""));
            CanvasMainModelShapes.Add(new PathModel("mea", ""));

            CanvasMainModelShapes.Add(new CircleModel("origin", thetaOrigin.X + cx, thetaOrigin.Y + cy, 10));
            //CanvasMainModelShapes.Add(new CircleModel("ref1", cx, cy, 10));
            //CanvasMainModelShapes.Add(new CircleModel("ref2", cx, cy, 10));
            //CanvasMainModelShapes.Add(new CircleModel("ref3", cx, cy, 10));
            //CanvasMainModelShapes.Add(new CircleModel("ref4", cx, cy, 10));
            CanvasMainModelShapes.Add(new CircleModel("mea1", cx, cy, 10));
            CanvasMainModelShapes.Add(new CircleModel("mea2", cx, cy, 10));
            CanvasMainModelShapes.Add(new CircleModel("mea3", cx, cy, 10));
            CanvasMainModelShapes.Add(new CircleModel("mea4", cx, cy, 10));
            CanvasMainModelShapes.Add(new CircleModel("center", cx, cy, 10));

            FOV_count = VisionRecipeManager.GetFovCnt(_SubSysInfo);
            ThetaOrigin_X = thetaOrigin.X;
            ThetaOrigin_Y = thetaOrigin.Y;
            Reset();
        }

        private void Move(object obj) {
            List<DoublePoint> meas = new List<DoublePoint>();

            for (int i = 1; i <= FOV_count; i++) {
                if (i == 1) meas.Add(new StaticPoint(MeasureX1, MeasureY1));
                else if (i == 2) meas.Add(new StaticPoint(MeasureX2, MeasureY2));
                else if (i == 3) meas.Add(new StaticPoint(MeasureX3, MeasureY3));
                else if (i == 4) meas.Add(new StaticPoint(MeasureX4, MeasureY4));
            }
            var n_measure = new List<DoublePoint>();
            n_measure = ApplyAlignment(meas, AlignT, AlignX, AlignY, thetaOrigin);

            for (int i = 1; i <= FOV_count; i++) {
                if (i == 1) {
                    MeasureX1 = n_measure[(int)eFovId.Fov1].X;
                    MeasureY1 = n_measure[(int)eFovId.Fov1].Y;
                }
                else if (i == 2) {
                    MeasureX2 = n_measure[(int)eFovId.Fov2].X;
                    MeasureY2 = n_measure[(int)eFovId.Fov2].Y;
                }
                else if (i == 3) {
                    MeasureX3 = n_measure[(int)eFovId.Fov3].X;
                    MeasureY3 = n_measure[(int)eFovId.Fov3].Y;
                }
                else if (i == 4) {
                    MeasureX4 = n_measure[(int)eFovId.Fov4].X;
                    MeasureY4 = n_measure[(int)eFovId.Fov4].Y;
                }
            }
            SetMeasureUI();
            StringBuilder pathData = new StringBuilder();
            pathData.Append($"M {MeasureX1 + cx},{MeasureY1 + cy} ");
            for (int i = 2; i <= FOV_count; i++) {
                if (i == 2) pathData.Append($"L {MeasureX2 + cx},{MeasureY2 + cy} ");
                else if (i == 3) pathData.Append($"L {MeasureX3 + cx},{MeasureY3 + cy} ");
                else if (i == 4) pathData.Append($"L {MeasureX4 + cx},{MeasureY4 + cy} ");
            }
            pathData.Append("Z");
            MeaPos.PathData = pathData.ToString();
            CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
        }

        private void Reset(object obj = null) {
            NorPos.StrokeColor = new SolidColorBrush(Colors.Cyan);
            IcpPos.StrokeColor = new SolidColorBrush(Colors.MediumPurple);
            RefPos.StrokeColor = new SolidColorBrush(Colors.White);
            MeaPos.StrokeColor = new SolidColorBrush(Colors.Yellow);
            MeaPos.Opacity = 0.8;
            CirclePoint("center").Tooltip = "x : 0\ny : 0";
            if(thetaOrigin.X!=0 || thetaOrigin.Y != 0) {
                CirclePoint("origin").Tooltip = $"x : {thetaOrigin.X}\nY : {thetaOrigin.Y}";
            }
            Fov1 = Fov2 = Fov3 = Fov4 = "0";
            AlignType = _AlignRecipe.ProcParams.AlignMode.ToString();
            List<StaticPoint> refpos = new List<StaticPoint>();
            switch (FOV_count) {
                case 4:
                    refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov4));
                    Fov4 = "1*";
                    goto case 3;
                case 3:
                    refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov3));
                    Fov3 = "1*";
                    goto case 2;
                case 2:
                    refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov2));
                    Fov2 = "1*";
                    goto case 1;
                case 1:
                    refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov1));
                    Fov1 = "1*";
                    CirclePoint("center").FillColor = new SolidColorBrush(Colors.Green);
                    break;
                default:
                    CirclePoint("center").FillColor = new SolidColorBrush(Colors.Red);
                    break;

            }

            refPoints = SortRefAndSetMeaPoint(refpos);
            SetMeasureUI();
            var toShow = new List<StaticPoint>();
            foreach (var point in refPoints) {
                toShow.Add(new StaticPoint(point.X, point.Y));
            }
            AddOffsetsToPoints(toShow, cx, cy);
            RefPos.PathData = $"M {toShow[(int)eFovId.Fov1].X},{toShow[(int)eFovId.Fov1].Y} " +
                string.Join(" ", Enumerable.Range(1, toShow.Count - 1).Select(i =>
                $"L {toShow[(int)eFovId.Fov1 + i].X},{toShow[(int)eFovId.Fov1 + i].Y}")) +
                " Z";
            MeaPos.PathData = "";
            IcpPos.PathData = "";
            NorPos.PathData = "";
            AlignX = 0; AlignY = 0; AlignT = 0;
            _align_res = (0, 0, 0);
            _align_res_icp = (0, 0, 0);
            RMSE = RMSE_ICP = 0;
            AlignX_A = 0;
            AlignY_A = 0;
            AlignT_A = 0;
            AlignX_I = 0;
            AlignY_I = 0;
            AlignT_I = 0;
            IsChecked_I = IsChecked_N = IsChecked_M = true;
            CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();

            if (FOV_count<2) {
                InfoText = "One-Point Align does not involve calulating T";
            }
            else {
                InfoText = "This represents the measure points ( reset to referece points )";
            }
        }

        private void Align(object obj) {

            List<DoublePoint> meas = new List<DoublePoint>();
            List<DoublePoint> refs = new List<DoublePoint>();

            for (int i = 1; i <= FOV_count; i++) {
                if (i == 1) meas.Add(new StaticPoint(MeasureX1, MeasureY1));
                else if (i == 2) meas.Add(new StaticPoint(MeasureX2, MeasureY2));
                else if (i == 3) meas.Add(new StaticPoint(MeasureX3, MeasureY3));
                else if (i == 4) meas.Add(new StaticPoint(MeasureX4, MeasureY4));
            }
            for (int i = 1; i <= FOV_count; i++) {
                if (i == 1) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov1].X, refPoints[(int)eFovId.Fov1].Y));
                else if (i == 2) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov2].X, refPoints[(int)eFovId.Fov2].Y));
                else if (i == 3) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov3].X, refPoints[(int)eFovId.Fov3].Y));
                else if (i == 4) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov4].X, refPoints[(int)eFovId.Fov4].Y));
            }

            AlignResult res;
            if (FOV_count<2) {
                res = _Aligner.Align(meas[0], refs[0], new Angle(), (int)_Calibrator.GetFlipY());
                _align_res_icp= _align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
            }
            else if(FOV_count==2){
                res = _Aligner.Align(meas, refs, (int)_Calibrator.GetFlipY());
                _align_res_icp =_align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
            }
            else {
                res = _Aligner.Align(meas, refs, (int)_Calibrator.GetFlipY());
                var res_new = CalculateICP(meas, refs);
                _align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
                _align_res_icp = (res_new.translation[0], res_new.translation[1], res_new.rotate * (180 / Math.PI));
            }

            var meatest = ApplyAlignment(meas, _align_res.t, _align_res.x, _align_res.y, thetaOrigin);
            var meatest_icp = ApplyAlignment(meas, _align_res_icp.t, _align_res_icp.x, _align_res_icp.y, thetaOrigin);
            RMSE = CalculateRMSE(refPoints, meatest);
            RMSE_ICP = CalculateRMSE(refPoints, meatest_icp);
            AlignX_A = _align_res.x;
            AlignY_A = _align_res.y;
            AlignT_A = _align_res.t;
            AlignX_I = _align_res_icp.x;
            AlignY_I = _align_res_icp.y;
            AlignT_I = _align_res_icp.t;
            StringBuilder pathData = new StringBuilder();
            pathData.Append($"M {meatest[0].X + cx},{meatest[0].Y + cy} ");
            for (int i = 2; i <= FOV_count; i++) {
                if (i == 2) pathData.Append($"L {meatest[1].X + cx},{meatest[1].Y + cy} ");
                else if (i == 3) pathData.Append($"L {meatest[2].X + cx},{meatest[2].Y + cy} ");
                else if (i == 4) pathData.Append($"L {meatest[3].X + cx},{meatest[3].Y + cy} ");
            }
            pathData.Append("Z");
            NorPos.PathData = pathData.ToString();
            pathData.Clear();
            pathData.Append($"M {meatest_icp[0].X + cx},{meatest_icp[0].Y + cy} ");
            for (int i = 2; i <= FOV_count; i++) {
                if (i == 2) pathData.Append($"L {meatest_icp[1].X + cx},{meatest_icp[1].Y + cy} ");
                else if (i == 3) pathData.Append($"L {meatest_icp[2].X + cx},{meatest_icp[2].Y + cy} ");
                else if (i == 4) pathData.Append($"L {meatest_icp[3].X + cx},{meatest_icp[3].Y + cy} ");
            }
            pathData.Append("Z");
            IcpPos.PathData = pathData.ToString();
            CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
        }

        private void Apply(object obj) {


        }

        private void SetMeasureUI() {
            CirclePoint("mea1").FillColor = new SolidColorBrush(Colors.Pink);
            CirclePoint("mea2").FillColor = new SolidColorBrush(Colors.Pink);
            CirclePoint("mea3").FillColor = new SolidColorBrush(Colors.Pink);
            CirclePoint("mea4").FillColor = new SolidColorBrush(Colors.Pink);
            CirclePoint("mea1").Tooltip = $" x1 : +{MeasureX1:F2}\ny1 : +{MeasureY1:F2}";
            CirclePoint("mea2").Tooltip = $" x2 : {MeasureX2:F2}\ny2 : +{MeasureY2:F2}";
            CirclePoint("mea3").Tooltip = $" x3 : {MeasureX3:F2}\ny3 : {MeasureY3:F2}";
            CirclePoint("mea4").Tooltip = $" x4 : +{MeasureX4:F2}\ny4 : {MeasureY4:F2}";
            CirclePoint("mea1").SetTopLeft(MeasureY1 + cy, MeasureX1 + cx);
            CirclePoint("mea2").SetTopLeft(MeasureY2 + cy, MeasureX2 + cx);
            CirclePoint("mea3").SetTopLeft(MeasureY3 + cy, MeasureX3 + cx);
            CirclePoint("mea4").SetTopLeft(MeasureY4 + cy, MeasureX4 + cx);
        }

        private List<StaticPoint> SortRefAndSetMeaPoint(List<StaticPoint> points) {
            CirclePoint("mea1").ShapeVisible = System.Windows.Visibility.Hidden;
            CirclePoint("mea2").ShapeVisible = System.Windows.Visibility.Hidden;
            CirclePoint("mea3").ShapeVisible = System.Windows.Visibility.Hidden;
            CirclePoint("mea4").ShapeVisible = System.Windows.Visibility.Hidden;
            // 1 -> 2 -> 3 -> 4 분면 순서
            var res = points.OrderBy(p => GetQuadrantOrder(p)).ToList();
            switch (res.Count) {
                case 4:
                    MeasureX4 = res[(int)eFovId.Fov4].X;
                    MeasureY4 = res[(int)eFovId.Fov4].Y;
                    CirclePoint("mea4").ShapeVisible = System.Windows.Visibility.Visible;
                    goto case 3;
                case 3:
                    MeasureX3 = res[(int)eFovId.Fov3].X;
                    MeasureY3 = res[(int)eFovId.Fov3].Y;
                    CirclePoint("mea3").ShapeVisible = System.Windows.Visibility.Visible;
                    goto case 2;
                case 2:
                    MeasureX2 = res[(int)eFovId.Fov2].X;
                    MeasureY2 = res[(int)eFovId.Fov2].Y;
                    CirclePoint("mea2").ShapeVisible = System.Windows.Visibility.Visible;
                    goto case 1;
                case 1:
                    MeasureX1 = res[(int)eFovId.Fov1].X;
                    MeasureY1 = res[(int)eFovId.Fov1].Y;
                    CirclePoint("mea1").ShapeVisible = System.Windows.Visibility.Visible;
                    break;
                default:
                    break;
            }
            return res;

        }

        private void ResetCenter(double centerX, double centerY) {
            CirclePoint("center").Left = centerX - CirclePoint("center").EllipseWidth;
            CirclePoint("center").Top = centerY - CirclePoint("center").EllipseWidth;
        }
        /// <summary>
        /// 모양 가져오기
        /// </summary>
        /// <param name="name">이름</param>
        /// <param name="shapemodel">모양모델</param>
        /// <returns></returns>
        public eShapeType GetShape(string name, out MaterialToleranceTestModel shapemodel) {
            MaterialToleranceTestModel matchedModel = CanvasMainModelShapes.FirstOrDefault(shape => shape.Name == name);
            shapemodel = matchedModel;
            switch (matchedModel) {
                case CircleModel m:
                    return eShapeType.Circle;
                //case RectangleModel m:
                //    return eShapeType.Rectangle;
                //case TextBoxModel m:
                //    return eShapeType.TextBox;
                case PathModel m:
                    return eShapeType.Path;
                default:
                    return eShapeType.Path;
            }
        }

        static int GetQuadrantOrder(StaticPoint p) {
            if (p.X >= 0 && p.Y >= 0) return 1; //이미지 좌표계상 ++
            if (p.X < 0 && p.Y >= 0) return 2; // -+
            if (p.X < 0 && p.Y < 0) return 3;  // --
            if (p.X >= 0 && p.Y < 0) return 4; // +-
            return 0;
        }

        static void AddOffsetsToPoints(List<StaticPoint> points, double cx, double cy) {
            foreach (var point in points) {
                point.X += cx;
                point.Y += cy;
            }
        }

        private List<DoublePoint> ApplyAlignment(List<DoublePoint> points, AlignDelta delta, DoublePoint rotationCenter) {
            var transformedPoints = new List<DoublePoint>();

            foreach (var point in points) {
                // 회전 중심으로 이동
                double translatedX = point.X - rotationCenter.X;
                double translatedY = point.Y - rotationCenter.Y;

                // 회전 변환 적용 (라디안 단위)
                double rad = delta.T.Radian;
                double rotatedX = translatedX * Math.Cos(rad) - translatedY * Math.Sin(rad);
                double rotatedY = translatedX * Math.Sin(rad) + translatedY * Math.Cos(rad);

                // 회전 중심 복원 및 이동 변환 적용
                double newX = rotatedX + rotationCenter.X + delta.X;
                double newY = rotatedY + rotationCenter.Y + delta.Y;

                transformedPoints.Add(new StaticPoint(newX, newY));
            }

            return transformedPoints;
        }

        private List<DoublePoint> ApplyAlignment(List<DoublePoint> points, double t, double x, double y, DoublePoint rotationCenter) {
            var transformedPoints = new List<DoublePoint>();

            foreach (var point in points) {
                // 회전 중심으로 이동
                double translatedX = point.X - rotationCenter.X;
                double translatedY = point.Y - rotationCenter.Y;

                // 회전 변환 적용 (라디안 단위)
                double rad = t * (Math.PI / 180);
                double rotatedX = translatedX * Math.Cos(rad) - translatedY * Math.Sin(rad);
                double rotatedY = translatedX * Math.Sin(rad) + translatedY * Math.Cos(rad);

                // 회전 중심 복원 및 이동 변환 적용
                double newX = rotatedX + rotationCenter.X + x;
                double newY = rotatedY + rotationCenter.Y + y;

                transformedPoints.Add(new StaticPoint(newX, newY));
            }

            return transformedPoints;
        }

        private static double CalculateRMSE(List<StaticPoint> refPoints, List<DoublePoint> meaPoints) {
            if (refPoints == null || meaPoints == null) return -1;
            int count = refPoints.Count;
            double sumOfSquaredDistances = 0.0;
            for (int i = 0; i < count; i++) {
                double diffX = refPoints[i].X - meaPoints[i].X;
                double diffY = refPoints[i].Y - meaPoints[i].Y;
                double squareDistance = diffX * diffX + diffY * diffY;
                sumOfSquaredDistances += squareDistance;
            }
            double meanSquaredDistance = Math.Sqrt(sumOfSquaredDistances/count);
            return meanSquaredDistance;
        }
        /// <summary>
        /// ICP 알고리즘 계산
        /// </summary>
        /// <param name="meas">평균값들</param>
        /// <param name="refs">기준점들</param>
        /// <returns></returns>
        public static (double rotate, double[] translation) CalculateICP(List<DoublePoint> meas, List<DoublePoint> refs) {
            int n = meas.Count;
            DoublePoint centroidMeas = CalculateCentroid(meas);
            DoublePoint centroidRefs = CalculateCentroid(refs);
            List<DoublePoint> centeredMeas = CenterPoints(meas, centroidMeas);
            List<DoublePoint> centeredRefs = CenterPoints(refs, centroidRefs);
            double Sxx = 0, Sxy = 0, Syx = 0, Syy = 0;
            for (int i = 0; i < n; i++) {
                Sxx += centeredMeas[i].X * centeredRefs[i].X;
                Sxy += centeredMeas[i].X * centeredRefs[i].Y;
                Syx += centeredMeas[i].Y * centeredRefs[i].X;
                Syy += centeredMeas[i].Y * centeredRefs[i].Y;
            }
            double theta = Math.Atan2(Sxy - Syx, Sxx + Syy);
            double[] translation = new double[2];
            translation[0] = centroidRefs.X - (centroidMeas.X * Math.Cos(theta) - centroidMeas.Y * Math.Sin(theta));
            translation[1] = centroidRefs.Y - (centroidMeas.X * Math.Sin(theta) + centroidMeas.Y * Math.Cos(theta));
            return (theta, translation);
        }
        private static DoublePoint CalculateCentroid(List<DoublePoint> points) {
            double sumX = 0, sumY = 0;
            foreach (var point in points) {
                sumX += point.X;
                sumY += point.Y;
            }
            return new StaticPoint { X = sumX / points.Count, Y = sumY / points.Count };
        }
        private static List<DoublePoint> CenterPoints(List<DoublePoint> points, DoublePoint centroid) {
            List<DoublePoint> centeredPoints = new List<DoublePoint>();
            foreach (var point in points) {
                centeredPoints.Add(new StaticPoint {
                    X = point.X - centroid.X,
                    Y = point.Y - centroid.Y
                });
            }
            return centeredPoints;
        }
    }
}