using Cognex.VisionPro;
using Cognex.VisionPro.Dimensioning;
using Cognex.VisionPro.ImageFile;
using Cognex.VisionPro.ToolBlock;
using Cognex.VisionPro.ImageProcessing;
using Cognex.VisionPro.Caliper;
using ConVX.VXData;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Xml.Serialization;
using System.Diagnostics;
using Cognex.VisionPro.PMAlign;


namespace WaferAlignment
{
    public partial class Form1 : Form
    {
        Camera _cam;
        SiemensS7 _s7;
        DataPostbox ImageData;
        TCP _tcp;
        
        string arSizeX = "DB7.DBD48";
        string arSizeY = "DB7.DBD52";
        string arSizeR = "DB7.DBD56";
        string arSpeedX = "DB7.DBD60";
        string arSpeedY = "DB7.DBD64";
        string arSpeedR = "DB7.DBD68";
        /// <summary>
        /// 执行模式
        /// </summary>
        string arMode = "DB7.DBW72";//
        /// <summary>
        /// 执行上面设置的动作
        /// </summary>
        string arAction = "DB7.DBX74.0";
        /// <summary>
        ///动作执行完成 
        /// </summary>
        string arOver = "DB7.DBX74.1";
        string arLED = "M3.0";
        /// <summary>
        /// 偏转角度
        /// </summary>
        string arAngle = "DB7.DBD76";//设置触发拍照的偏转角度



        string vppPath;
        CogToolBlock mTB;
        CogToolBlock mTB_1;
        CogToolBlock mTB_2;


        float R = 11.25F;//1024->12
        //CogImage8Grey OutputImg;

        //旋转中心的坐标应将标定的数据保存后获取
        double circleCenterX;
        double circleCenterY;
        //XML文件读取与写入
        ConfigStore _ConfigStore = new ConfigStore();
        CenterOfRotation _center = new CenterOfRotation();
        //偏差量
        double deviationX;
        double deviationY;

        //
        string TCP_time = "0";
        string TCP_X = "0";
        string TCP_Y = "0";
        string TCP_R = "0";
        string TCP_RMS = "0";

        bool OKorNG = false;//判断检查是否完成
        //


        public Form1()
        {
            InitializeComponent();
        }
        private void Form1_Load(object sender, EventArgs e)
        {

            // 连接相机
            try
            {
                _cam = new Camera();
                _cam.Connect();
                _cam.OutputImageEvent += _cam_OutputImageEvent;
                lbl_CameraState.BackColor = Color.Lime;
                lbl_CameraState.Text = "已连接";
            }
            catch (Exception ex)
            {
                lbl_CameraState.BackColor = Color.Red;
                lbl_CameraState.Text = "连接失败";
                MessageBox.Show(ex.Message);
            }
            // 连接通信
            try
            {
                _s7 = new SiemensS7("192.168.1.10");
                _s7.Connect();
                while (!_s7.OK)
                {

                }
                _s7.SetValue(arSpeedX, (float)2);//5
                _s7.SetValue(arSpeedY, (float)2);//2
                _s7.SetValue(arSpeedR, (float)360);//100
                _s7.SetValue(arSizeX, (float)0);
                _s7.SetValue(arSizeY, (float)0);
                _s7.SetValue(arSizeR, (float)0);
                _s7.SetValue(arAngle, Convert.ToSingle(R));//角度为10
                lbl_CommunState.BackColor = Color.Lime;
                lbl_CommunState.Text = "已连接";
            }
            catch (Exception ex)
            {
                lbl_CommunState.BackColor = Color.Red;
                lbl_CommunState.Text = "连接失败";
                MessageBox.Show(ex.Message);
            }
            //链接TCP通信
            try
            {
                //通信初始化
                _tcp = new TCP();
                _tcp.DataReceived += _tcp_DataReceived;
                _tcp.StartServer();

            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);

            }
            try
            {
                // 加载VPP文件
                vppPath = Path.Combine(System.AppDomain.CurrentDomain.SetupInformation.ApplicationBase, "vpp", "ToolBlock.vpp");

                mTB = CogSerializer.LoadObjectFromFile(vppPath) as CogToolBlock;
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
            try
            {
                ImageData = new DataPostbox();

                //启动
                ImageData.Start();
                // 注册事件处理程序到 DeliverLetterEvent 事件中
                ImageData.DeliverLetterEvent += MonitorSpot;//查找所有点
                ImageData.ErrorEvent += ImageData_ErrorEvent;
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);

            }

            // 指定XML文件路径
            string filePath = Path.Combine(System.AppDomain.CurrentDomain.SetupInformation.ApplicationBase, "vpp", "CenterOfRotation.xml"); //@"D:\晶圆寻边机\XML文件\CenterOfRotation.xml";
            _center = _ConfigStore.ReadConfigFromFile(_center.GetType(), filePath) as CenterOfRotation;
            circleCenterX = _center.Center_X;
            circleCenterY = _center.Center_Y;
        }
        /// <summary>
        /// 接收到TCP数据时，根据接收到的数据，进行不同操作
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void _tcp_DataReceived(object sender, DataReceivedEventArgs e)
        {
            if (this.InvokeRequired)
            {
                // 如果当前线程不是UI线程，则使用Invoke在UI线程上执行
                this.Invoke(new Action<object,DataReceivedEventArgs>(_tcp_DataReceived), sender, e);
            }
            else
            {
                if (e.Data == "T0")
                {
                    string str;
                    str = ToZero();
                    _tcp.SendData(e.ClientIp, str);
                }
                else if (e.Data == "GET")
                {
                    string str;
                    str = ToAction();
                    _tcp.SendData(e.ClientIp, str);
                }
                else if (e.Data == "RUN")
                {
                    SavePath = Path.Combine(System.AppDomain.CurrentDomain.SetupInformation.ApplicationBase, "SaveImage", DateTime.Now.ToString("yyyyMMdd_HHmmss"));

                    ClearDisplay();
                    string str;
                    str = RunTest();
                    _tcp.SendData(e.ClientIp, str);
                    _tcp.SendData(e.ClientIp, "DetectionTime：" + TCP_time + " " + "X：" + TCP_X + " " + "Y：" + TCP_Y + " " + "Angle：" + TCP_R + " " + "RMS：" + TCP_RMS);
                }
            }

            
        }

        void _cam_OutputImageEvent(Bitmap obj)
        {
            lstNasR.Add((acqNum + 1) * R);
            acqNum++;
            //lstNasR.Add((testNum) * R);
            //保存图像
            //SaveImage(new CogImage8Grey(obj));
            ImageData.Mailing(new CogImage8Grey(obj));
            Images.Add(new CogImage8Grey(obj));

        }

        void ImageData_ErrorEvent(string obj)
        {
            //throw new NotImplementedException();
        }


        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            try
            {
                if (_s7 != null)
                {
                    _s7.Disconnect();
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }

            try
            {
                if(_cam.IsStarted)
                {
                    _cam.Stop();
                }
                if (_cam.IsConnected)
                {
                    _cam.Disconnect();
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
            try
            {

                //停止位
                ImageData.Stop();
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);

            }
            try
            {
                _tcp.StopServer();
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }
        private void btn_ToZero_Click(object sender, EventArgs e)
        {
            Task.Run(() =>
            {
                this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = false));
                string str = "";
                str = ToZero();
                MessageBox.Show(str);

                this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = true));
            });
        }

        public string ToZero()
        {
            if (_s7 != null)
            {

                _s7.SetValue(arMode, (ushort)4);
                _s7.SetValue(arAction, true);
                Thread.Sleep(20);
                while (true)
                {
                    bool value = false;
                    if (_s7.GetValue(arOver, out value))
                    {
                        if (value)
                        {
                            _s7.SetValue(arAction, false);
                            break;
                        }
                        else
                        {
                            Thread.Sleep(20);
                            continue;
                        }
                    }
                    else
                    {
                        Thread.Sleep(20);
                        continue;
                    }
                }
                return "OK_T0";
            }
            else
            {
                return "NG_T0";
            }
        }
        private void btn_Action_Click(object sender, EventArgs e)
        {
            Task.Run(() =>
           {
               this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = false));
               string str = "";
               str = ToAction();
               MessageBox.Show(str);

               this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = true));
           });
        }

        public string ToAction()
        {
            if (_s7 != null)
            {

                _s7.SetValue(arMode, (ushort)0);
                _s7.SetValue(arSizeX, Convert.ToSingle(nud_SizeX.Value));
                _s7.SetValue(arSizeY, Convert.ToSingle(nud_SizeY.Value));
                _s7.SetValue(arSizeR, Convert.ToSingle(nud_SizeR.Value));
                _s7.SetValue(arAction, true);
                Thread.Sleep(20);
                while (true)
                {
                    bool value = false;
                    if (_s7.GetValue(arOver, out value))
                    {
                        if (value)
                        {
                            _s7.SetValue(arAction, false);
                            break;
                        }
                        else
                        {
                            Thread.Sleep(20);
                            continue;
                        }
                    }
                    else
                    {
                        Thread.Sleep(20);
                        continue;
                    }
                }
                return "OK_GET";

            }
            else
            {
                return "NG_GET";
            }
        }

        List<double> lstRotation;
        List<double> lstX;
        List<double> lstY;
        List<double> lstRadius;
        List<double> lstNasX;
        List<double> lstNasY;
        List<double> lstNasR;
        List<CogImage8Grey> Images;

        int acqNum = 0;
        int testNum = 0;
        double angle = 0;
        Stopwatch sw_1 = new Stopwatch();//检测找点时间
        Stopwatch sw_2 = new Stopwatch();//检测拟合圆时间
        double times = 0;//整体检测时间
        private void btn_RunTest_Click(object sender, EventArgs e)
        {
            SavePath = Path.Combine(System.AppDomain.CurrentDomain.SetupInformation.ApplicationBase, "SaveImage", DateTime.Now.ToString("yyyyMMdd_HHmmss"));

            ClearDisplay();
            ////---------------------------------------
            Task.Run(() =>
            {
                this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = false));
                string str = "";
                str = RunTest();
                MessageBox.Show(str);

                this.splitContainer1.Invoke(new Action(() => this.splitContainer1.Panel2.Enabled = true));
            });

            //---------------------------------------
        }

        public string RunTest()
        {
            if (!_cam.IsConnected)
            {
                return "CF_CAM";//启动检测失败，相机未连接

            }
            else
            {

                //R轴回0
                if (_s7 != null)
                {
                    _s7.SetValue(arMode, (ushort)3);//写入PLC控制模式 3 
                    Thread.Sleep(100);
                    _s7.SetValue(arAction, true);//写入PLC控制 行动开始 
                    Thread.Sleep(20);
                    while (true)//判断运动是否结束 
                    {
                        bool value = false;
                        if (_s7.GetValue(arOver, out value))//读取地址为arOver的结束数据是否成功 并返回读取结果   
                        {
                            if (value)//判断读取结束结果
                            {
                                _s7.SetValue(arAction, false);//写入PLC控制 行动结束 
                                break;
                            }
                            else
                            {
                                Thread.Sleep(20);
                                continue;
                            }
                        }
                        else
                        {
                            Thread.Sleep(20);
                            continue;
                        }
                    }
                }
                Thread.Sleep(500);

                _cam.Start();
                if (!_cam.IsStarted)
                {
                    return "SF_CAM";//启动检测失败，相机启动失败

                }
                else
                {
                    angle = 0;
                    acqNum = 0;
                    testNum = 0;
                    lstRotation = new List<double>();
                    lstX = new List<double>();
                    lstY = new List<double>();
                    lstRadius = new List<double>();
                    lstNasX = new List<double>();
                    lstNasY = new List<double>();
                    lstNasR = new List<double>();
                    Images = new List<CogImage8Grey>();

                    sw_1 = new Stopwatch();//检测时间
                    sw_2 = new Stopwatch();//检测时间
                    deviationX = 0;
                    deviationY = 0;
                    times = 0;
                    //启动拍照

                    if (_s7 != null)
                    {
                        //_s7.SetValue(arOver, false);
                        //_s7.SetValue(arMode, (ushort)5);//写入PLC控制模式 5 启动拍照
                        _s7.SetValue(arMode, (ushort)0);//写入PLC控制模式 0 启动拍照
                        _s7.SetValue(arSizeR, 360F);
                        //Thread.Sleep(100);
                        _s7.SetValue(arAction, true);//写入PLC控制 行动开始 

                        //Thread.Sleep(100);
                        while (true)//判断运动是否结束
                        {
                            bool value = false;
                            if (_s7.GetValue(arOver, out value))
                            {
                                if (value)
                                {
                                    _s7.SetValue(arAction, false);
                                    break;
                                }
                                else
                                {
                                    Thread.Sleep(20);
                                    continue;
                                }
                            }
                            else
                            {
                                Thread.Sleep(20);
                                continue;
                            }
                        }
                        //Thread.Sleep(100);

                    }

                }

                _cam.SoftwareGrab();
                _cam.Stop();

                while (testNum < acqNum)
                {
                    Thread.Sleep(20);
                }
                FitCircle();

                //////////三轴回零
                ////////if (_s7 != null)
                ////////{
                ////////    _s7.SetValue(arMode, (ushort)4);
                ////////    _s7.SetValue(arAction, true);
                ////////    Thread.Sleep(20);
                ////////    while (true)
                ////////    {
                ////////        bool value = false;
                ////////        if (_s7.GetValue(arOver, out value))
                ////////        {
                ////////            if (value)
                ////////            {
                ////////                _s7.SetValue(arAction, false);
                ////////                break;
                ////////            }
                ////////            else
                ////////            {
                ////////                Thread.Sleep(20);
                ////////                continue;
                ////////            }
                ////////        }
                ////////        else
                ////////        {
                ////////            Thread.Sleep(20);
                ////////            continue;
                ////////        }
                ////////    }
                ////////    //MessageBox.Show("动作已完成！");
                ////////}

                ////////调整圆心到旋转中心


                //////if (_s7 != null)
                //////{
                //////    double _angle = angle;
                //////    //顺时针旋转
                //////    //if (_angle > 0)
                //////    //{
                //////    //    _angle = 360 - _angle;
                //////    //}
                //////    //else
                //////    //{

                //////    //    _angle = -_angle;
                //////    //}
                //////    //逆时针旋转
                //////    if (_angle > 0)
                //////    {
                //////        _angle = _angle;
                //////    }
                //////    else
                //////    {

                //////        _angle = 360 + _angle;
                //////    }

                //////    _s7.SetValue(arMode, (ushort)0);
                //////    _s7.SetValue(arSizeX, Convert.ToSingle((double)nud_SizeX.Value + deviationX));
                //////    _s7.SetValue(arSizeY, Convert.ToSingle((double)nud_SizeY.Value + deviationY));
                //////    _s7.SetValue(arSizeR, Convert.ToSingle(_angle));
                //////    _s7.SetValue(arAction, true);
                //////    Thread.Sleep(20);
                //////    while (true)
                //////    {
                //////        bool value = false;
                //////        if (_s7.GetValue(arOver, out value))
                //////        {
                //////            if (value)
                //////            {
                //////                _s7.SetValue(arAction, false);
                //////                break;
                //////            }
                //////            else
                //////            {
                //////                Thread.Sleep(20);
                //////                continue;
                //////            }
                //////        }
                //////        else
                //////        {
                //////            Thread.Sleep(20);
                //////            continue;
                //////        }
                //////    }
                //////    //MessageBox.Show("动作已完成！");

                //////}
                Task.Run(() => { Save(); });
            }
            if (OKorNG)
            {
                return "OK_RUN";

            }
            else
            {
                return "NG_RUN";
            }
        }

        //所有点
        // 调整vpp中CogCaliperTool工具数据需要同时调整Y值与 for (double i = Y; i < 12; i = i + 0.5)中12的值
        private void MonitorSpot(CogImage8Grey outputImg)
        {
            sw_1 = new Stopwatch();//检测时间
            sw_1.Start();
            //double Y = -17;
            //double X = 0;
            double X = -15;
            double Y = 0;
            CogImage8Grey inputImg = new CogImage8Grey();
            CogGraphicCollection graphics = new CogGraphicCollection();
            CogImage8Grey _OutputImg = new CogImage8Grey();

            inputImg = outputImg;
            mTB_1 = mTB.Tools["CogToolBlock1"] as CogToolBlock;
            mTB_2 = mTB.Tools["CogToolBlock2"] as CogToolBlock;
            mTB_1.Inputs["InputImage"].Value = inputImg;
            mTB_1.Run();
            _OutputImg = mTB_1.Outputs["OutputImage"].Value as CogImage8Grey;

            CogCaliperTool mCT = mTB_1.Tools["CogCaliperTool1"] as CogCaliperTool;


            
            //x′= cx + (x−cx)⋅cos(r)−(y−cy)⋅sin(r)
            //y′= cy + (x−cx)⋅sin(r) + (y−cy)⋅cos(r)​

            int _count = 0;

            for (double i = X; i < 14; i = i + 0.5)
            {
                mCT.InputImage = _OutputImg;
                mCT.Region.CenterX = i;
                mCT.Run();
                _count = mCT.Results.Count;

                if (_count != 0)
                {
                    Y = mCT.Results[0].Edge0.PositionY;
                    //旋转角度
                    double r = lstNasR[testNum] / 180 * Math.PI;
                    //旋转移动到实际位置
                    lstNasX.Add(i * Math.Cos(r) - Y * Math.Sin(r) + circleCenterX * (1 - Math.Cos(r)) + circleCenterY * Math.Sin(r));
                    lstNasY.Add(i * Math.Sin(r) + Y * Math.Cos(r) - circleCenterX * Math.Sin(r) + circleCenterY * (1 - Math.Cos(r)));

                    CogPointMarker marker_1 = new CogPointMarker();//圆

                    marker_1.X = i;
                    marker_1.Y = Y;
                    marker_1.Color = CogColorConstants.Blue;
                    marker_1.Interactive = false;
                    graphics.Add(marker_1);
                }
                else
                {
                    OKorNG = false;
                    testNum++;
                    return;
                }
                if (i == X)
                {
                    double r = lstNasR[testNum] / 180 * Math.PI;
                    lstX.Add(i * Math.Cos(r) - Y * Math.Sin(r) + circleCenterX * (1 - Math.Cos(r)) + circleCenterY * Math.Sin(r));
                    lstY.Add(i * Math.Sin(r) + Y * Math.Cos(r) - circleCenterX * Math.Sin(r) + circleCenterY * (1 - Math.Cos(r)));
                }

            }
            sw_1.Stop();
            TimeSpan ts = sw_1.Elapsed;
            ImageDisplay(_OutputImg, graphics);
            DataDisplay(lstNasR[testNum], lstNasX[0], lstNasY[0], ts.TotalMilliseconds);
            testNum++;
            times += ts.TotalMilliseconds;
            OKorNG = true;
        }

        //比较半径和点间距，拟合圆
        private void FitCircle()
        {
            if (OKorNG)
            {
                sw_2 = new Stopwatch();//检测时间
                sw_2.Start();
                int Image_i = -1;
                // 调整vpp中CogCaliperTool工具数据需要同时调整step为x点数*2
                int step = 58;
                CogPMAlignTool mPM = mTB_2.Tools["CogPMAlignTool2"] as CogPMAlignTool;



                CogImage8Grey outputImg = mTB_1.Outputs["OutputImage"].Value as CogImage8Grey;
                //添加找圆工具的  忽略点数  和  RMS偏差量
                CogFitCircleTool fitCircle_1 = new CogFitCircleTool();
                CogFitCircleTool fitCircle_2 = new CogFitCircleTool();


                CogGraphicCollection graphics = new CogGraphicCollection();

                fitCircle_1.InputImage = outputImg;
                for (int i = 0; i < lstX.Count; i++)
                {
                    fitCircle_1.RunParams.AddPoint(lstX[i], lstY[i]);

                    //CogPointMarker marker_1 = new CogPointMarker();//圆

                    //marker_1.X = lstNasX[i];
                    //marker_1.Y = lstNasY[i];
                    //marker_1.Color = CogColorConstants.White;
                    //marker_1.Interactive = false;
                    //graphics.Add(marker_1);

                }

                //fitCircle_1.RunParams.NumToIgnore = 0;
                fitCircle_1.Run();
                if (fitCircle_1.Result != null)
                {
                    double _RMS = fitCircle_1.Result.RMSError;
                    CogCircle circle_1 = new CogCircle(fitCircle_1.Result.GetCircle());
                    circle_1.Color = CogColorConstants.Cyan;
                    circle_1.Interactive = false;

                    CogFitLineTool fitLine = new CogFitLineTool();

                    //晶圆圆心位置
                    double _centerX = circle_1.CenterX;
                    double _centerY = circle_1.CenterY;
                    double _radius = circle_1.Radius;


                    fitCircle_2.InputImage = outputImg;

                    CogDistancePointPointTool pTp = new CogDistancePointPointTool();
                    pTp.InputImage = outputImg;
                    pTp.StartX = _centerX;
                    pTp.StartY = _centerY;
                    //double[] _DistanceMin = new double[lstNasX.Count];
                    //int _distanceMin_i = 0;
                    for (int i = 0; i < lstNasX.Count; i += step)
                    {

                        for (int j = i; j < i + step && j < lstNasX.Count; j++)
                        {
                            if (j < (i + step - 15))//忽略重合数据的末尾的15个点
                            {
                                pTp.EndX = lstNasX[j];
                                pTp.EndY = lstNasY[j];
                                pTp.Run();
                                double Distance = pTp.Distance;
                                if (Distance - _radius < 0.1 && Distance - _radius > -0.1)
                                {
                                    fitCircle_2.RunParams.AddPoint(lstNasX[j], lstNasY[j]);
                                    CogPointMarker marker_1 = new CogPointMarker();//圆

                                    marker_1.X = lstNasX[j];
                                    marker_1.Y = lstNasY[j];
                                    marker_1.Color = CogColorConstants.Green;
                                    marker_1.Interactive = false;
                                    graphics.Add(marker_1);
                                }
                                else//忽略的点数
                                {
                                    if (!(Distance - _radius < 1 && Distance - _radius > -1))
                                    {
                                        if (Image_i == -1)
                                        {
                                            Image_i = (int)(j / step);
                                        }
                                    }


                                    CogPointMarker marker_1 = new CogPointMarker();//圆

                                    marker_1.X = lstNasX[j];
                                    marker_1.Y = lstNasY[j];
                                    marker_1.Color = CogColorConstants.Yellow;
                                    marker_1.Interactive = false;
                                    graphics.Add(marker_1);
                                }
                                //_DistanceMin[i] = Distance;
                            }
                            else
                            {
                                CogPointMarker marker_1 = new CogPointMarker();//圆

                                marker_1.X = lstNasX[j];
                                marker_1.Y = lstNasY[j];
                                marker_1.Color = CogColorConstants.Red;
                                marker_1.Interactive = false;
                                //graphics.Add(marker_1);
                            }
                        }

                    }
                    //忽略的拟合点数
                    fitCircle_2.RunParams.NumToIgnore = 0;
                    fitCircle_2.Run();

                    double RMS = fitCircle_2.Result.RMSError;
                    CogCircle circle_2 = new CogCircle(fitCircle_2.Result.GetCircle());
                    circle_2.Color = CogColorConstants.Cyan;
                    circle_2.Interactive = false;
                    //晶圆圆心位置
                    double centerX = circle_2.CenterX;
                    double centerY = circle_2.CenterY;
                    double radius = circle_2.Radius;


                    double PM_X = 0;
                    double PM_Y = 0;
                    int _count = 0;
                    if (Image_i != -1)
                    {
                        mTB_2.Inputs["InputImage"].Value = Images[Image_i];
                        mTB_2.Run();
                        _count = mPM.Results.Count;

                        if (_count != 0)
                        {

                            double r = lstNasR[Image_i] / 180 * Math.PI;
                            PM_X = mPM.Results[0].GetPose().TranslationX * Math.Cos(r) - mPM.Results[0].GetPose().TranslationY * Math.Sin(r) + circleCenterX * (1 - Math.Cos(r)) + circleCenterY * Math.Sin(r);
                            PM_Y = mPM.Results[0].GetPose().TranslationX * Math.Sin(r) + mPM.Results[0].GetPose().TranslationY * Math.Cos(r) - circleCenterX * Math.Sin(r) + circleCenterY * (1 - Math.Cos(r));

                        }
                    }
                    //_distanceMin_i = DistanceMin(_DistanceMin);

                    ////纳什口
                    //CogLine line_ = new CogLine();
                    //line_.SetFromStartXYEndXY(centerX, centerY, lstNasX[_distanceMin_i], lstNasY[_distanceMin_i]);
                    //line_.Color = CogColorConstants.Green;
                    //line_.Interactive = false;
                    //graphics.Add(line_);
                    //纳什口
                    CogLine line_ = new CogLine();
                    line_.SetFromStartXYEndXY(centerX, centerY, PM_X, PM_Y);
                    line_.Color = CogColorConstants.Green;
                    line_.Interactive = false;
                    graphics.Add(line_);
                    //角度值为0
                    CogLine line_0 = new CogLine();
                    line_0.SetXYRotation(centerX, centerY, 0);
                    line_0.Color = CogColorConstants.Blue;
                    line_0.Interactive = false;
                    graphics.Add(line_0);
                    CogAngleLineLineTool llTool = new CogAngleLineLineTool();
                    llTool.InputImage = outputImg;
                    llTool.LineA = line_0;
                    llTool.LineB = line_;
                    llTool.Run();
                    angle = CogMisc.RadToDeg(llTool.Angle);

                    CogPointMarker marker_2 = new CogPointMarker();//圆心

                    marker_2.X = centerX;
                    marker_2.Y = centerY;
                    marker_2.Color = CogColorConstants.Green;
                    marker_2.Interactive = false;
                    graphics.Add(marker_2);

                    CogPointMarker marker_3 = new CogPointMarker();//旋转圆心

                    marker_3.X = circleCenterX;
                    marker_3.Y = circleCenterY;
                    marker_3.Color = CogColorConstants.Orange;
                    marker_3.Interactive = false;
                    graphics.Add(marker_3);
                    graphics.Add(circle_2);


                    deviationX = centerX - circleCenterX;
                    deviationY = centerY - circleCenterY;

                    sw_2.Stop();
                    TimeSpan ts = sw_2.Elapsed;

                    times += ts.TotalMilliseconds;


                    ImageDisplay(outputImg, graphics);
                    TCP_time = ts.TotalMilliseconds.ToString();
                    TCP_X = centerX.ToString("F4");
                    TCP_Y = centerY.ToString("F4");
                    TCP_RMS = RMS.ToString("F4");
                    TCP_R = angle.ToString("F4");
                    ResultDisplay(times, ts.TotalMilliseconds, centerX, centerY, radius, RMS, deviationX, deviationY, angle);

                    OKorNG = true;
                }
                else
                {
                    //MessageBox.Show("检测结果为空。");
                    OKorNG = false;
                }
            }
            


        }
        //private int DistanceMin(double[] numbers)
        //{
        //    double number = numbers[0];
        //    int num1 = 0;
        //    double num2 = ((IEnumerable<double>)numbers).Sum() / (double)((IEnumerable<double>)numbers).Count<double>();
        //    for (int index = 1; index < numbers.Length; ++index)
        //    {
        //        if (numbers[index] < number)
        //        {
        //            number = numbers[index];
        //            num1 = index;
        //        }
        //    }
        //    //return Math.Abs(number - num2) > 0.0 ? num1 : -1;
        //    return num1;
        //}


        private void ResultDisplay(double Times,double Time, double X, double Y, double R, double RMS, double dX, double dY, double dR)
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => ResultDisplay(Times,Time,X, Y, R, RMS, dX, dY, dR)));
                return;
            }
            txt_Times.Text = Times.ToString();
            txt_Time.Text = Time.ToString();
            txt_RX.Text = X.ToString();
            txt_RY.Text = Y.ToString();
            txt_RR.Text = R.ToString();
            txt_RMS.Text = RMS.ToString();
            txt_DX.Text = dX.ToString();
            txt_DY.Text = dY.ToString();
            txt_DR.Text = dR.ToString();

        }
        private void ClearDisplay()
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => ClearDisplay()));
                return;
            }
            this.cogRecordDisplay1.InteractiveGraphics.Clear();
            this.cogRecordDisplay1.StaticGraphics.Clear();
            this.cogRecordDisplay1.Image = null;
        }

        private void ImageDisplay(CogImage8Grey img, CogGraphicCollection graphics)
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => ImageDisplay(img, graphics)));
                return;
            }
            this.cogRecordDisplay1.Image = img;
            this.cogRecordDisplay1.StaticGraphics.AddList(graphics, "");
            this.cogRecordDisplay1.Fit(true);
        }
        private void ImageDisplay(CogGraphicCollection graphics)
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => ImageDisplay(graphics)));
                return;
            }
            this.cogRecordDisplay1.StaticGraphics.AddList(graphics, "");
            this.cogRecordDisplay1.Fit(true);
        }
        private void ImageDisplay(CogImage8Grey img)
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => ImageDisplay(img)));
                return;
            }
            this.cogRecordDisplay1.Image = img;
            this.cogRecordDisplay1.Fit(true);
        }

        private void DataDisplay(double Rotation, double X, double Y, double Radius)
        {
            if (this.InvokeRequired)
            {
                this.BeginInvoke(new Action(() => DataDisplay(Rotation, X, Y, Radius)));
                return;
            }
            int rowIndex = dataGridView1.Rows.Add();
            dataGridView1.Rows[rowIndex].Cells["Column1"].Value = Rotation;
            dataGridView1.Rows[rowIndex].Cells["Column2"].Value = X;
            dataGridView1.Rows[rowIndex].Cells["Column3"].Value = Y;
            dataGridView1.Rows[rowIndex].Cells["Column4"].Value = Radius;

        }
        string SavePath;
        private void SaveImage(ICogImage img)
        {
            if (!Directory.Exists(SavePath))
            {
                Directory.CreateDirectory(SavePath);
            }
            using (CogImageFile imgFile = new CogImageFile())
            {
                imgFile.Open(Path.Combine(SavePath, DateTime.Now.ToString("yyyyMMdd_HHmmss_fff") + ".bmp"), CogImageFileModeConstants.Write);
                imgFile.Append(img);
            }
        }
        private void Save()
        {
            if (!Directory.Exists(SavePath))
            {
                Directory.CreateDirectory(SavePath);
            }
            foreach (var item in Images)
            {
                using (CogImageFile imgFile = new CogImageFile())
                {
                    imgFile.Open(Path.Combine(SavePath, DateTime.Now.ToString("yyyyMMdd_HHmmss_fff") + ".bmp"), CogImageFileModeConstants.Write);
                    imgFile.Append(item);
                }
            }
        }
    }
}
