using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Intel.RealSense;
using Emgu.CV.Structure;
using Emgu.CV;
using System.IO;
using System.Drawing;
using System.Windows.Forms;
using Stream = Intel.RealSense.Stream;
using System.Threading;
using WindowsFormsApp1; // 根据你的项目命名空间进行修改
using ABB.Robotics.Controllers.IOSystemDomain;
using yolov5_onnx.Mod;
using static WindowsFormsApp1.ExtrinsicsExtensions;
using System.Diagnostics;
using System.Threading.Tasks;
using System.Runtime.InteropServices;
using NetTopologySuite.GeometriesGraph;
using System.Windows.Media.Media3D;

namespace RealSence_PathMove
{
    public class IntelRealSense
    {
        private readonly Form1 form;

        public IntelRealSense(Form1 form)
        {

            this.form = form;

        }
        /// <summary>
        /// 相機連結狀態
        /// </summary>
        enum CameraState
        {
            Disconnected = 0,
            Connected = 1,
            Opened = 2
        }
        Colorizer colorizer = new Colorizer();
        Align align = new Align(Intel.RealSense.Stream.Color);  // 深度影像MAP彩色影像
        CameraState CamState = CameraState.Disconnected;  // 初始狀態設為斷線
        public AdvancedDevice DeviceName;
        private Pipeline pipeline = new Pipeline();
        internal PictureBox pictureBox2;
        internal PictureBox pictureBox3;
        internal PictureBox pictureBox4;
        internal PictureBox pictureBox5;
        private CustomProcessingBlock block;
        internal CancellationTokenSource cts;
        private System.Threading.Tasks.Task dataProcessingTask = System.Threading.Tasks.Task.CompletedTask;  // 初始化为已完成的任务。
        VideoStreamProfile depthStream;
        VideoStreamProfile colorStream;
        public Intrinsics depthIntrinsics; // 定義成員變數，以便在不同方法中訪問
        public Extrinsics depthExtrinsics;
        public Intrinsics colorIntrinsics;
        public Extrinsics colorExtrinsics;
        public Bitmap bmpColor;  // 請確保這裡有定義 bmpColor 成員
        public Bitmap bmpDepth;
        public List<System.Drawing.PointF> yoloFeaturePoints = new List<System.Drawing.PointF>();
        // 假设您已经获得了特徵點在相機座標系中的坐标列表 cameraPointsList
        List<MathNet.Numerics.LinearAlgebra.Vector<double>> cameraPointsList = new List<MathNet.Numerics.LinearAlgebra.Vector<double>>();
        public int objectCounter = 1; // 初始化物體編號為1
        StringBuilder coordTextBuilder = new StringBuilder();
        private readonly object frameLock = new object();
        public int deepestX, deepestY, shallowestX, shallowestY, Goldsamplepointcloudnumber;
        public double X, Y, u, v, percentageInsideCrop;
        public float Z, x, y, z, Rx, Ry, Rz, threshold, deepestZ, shallowestZ, GolddeepestZ, GoldshallowestZ;
        public string folderPath, fileName, folderPath1, fileName1;
        public ListViewItem Item { get; set; }
        public bool camclose;
        public Extrinsics SharedExtrinsics { get; private set; }
        public ushort[] DepthData { get; private set; }
        public Bitmap yoloimage { get; private set; }
        public float MaxDepthValue { get; private set; }
        public System.Numerics.Vector3 cameraPoint { get; private set; }
        public float scale { get; private set; }
        public Image<Rgb, byte> ColorImg { get; private set; }
        public Image<Rgb, byte> DepthImg { get; private set; }
        public Image<Rgb, byte> ROIImg;
        public Bitmap bitmap { get; private set; }
        public ushort[] croppedDepthData;
        // 在類別中添加一個 List<Vector3> 來儲存三維座標
        public List<System.Numerics.Vector3> storedCoordinates = new List<System.Numerics.Vector3>();
        public Bitmap DepthBitmap { get; private set; }
        public Rectangle CroppingRect { get; private set; }
        public byte[] ColorData { get; private set; }
        public Bitmap DepthImg1Bitmap { get; private set; }
        public bool frameReceived = false;

        public bool captureThread1bool = true;
        public bool TakeImage;
        public int i = 0;
        public Thread uiThread; // UI 线程
        public CancellationTokenSource tokenSource = new CancellationTokenSource();
        public delegate void NewFrameEventHandler(VideoFrame colorFrame, DepthFrame depthFrame);
        public event NewFrameEventHandler NewFrameArrived;
        public event NewFrameEventHandler NewFrameArrived11;
            
        /// <summary>
        /// 初始化設定並連接RealSenseL515相機
        /// </summary>
        internal void Connect()
        {
            var ctx = new Context();
            // 查找L515设备
            var devices = ctx.QueryDevices();
            var adev = devices.FirstOrDefault(d => d.Info[CameraInfo.Name] == "Intel RealSense L515");

            if (adev == null)
            {
                throw new Exception("No RealSense L515 detected");
            }
            // 连接成功
            Console.WriteLine("Connected to: " + adev.Info[CameraInfo.Name]);         
        }

        /// <summary>
        /// 開RealSenseL515相機
        /// </summary>
        [Obsolete]
        internal void OpenCamera() //開啟相機
        {
            Connect();
            var cfg = new Config();  // 通過序列號啟用設備
            cfg.EnableStream(Stream.Depth, 640, 480, Format.Z16, 30);
            cfg.EnableStream(Stream.Color, 640, 480, Format.Rgb8, 30);
            pipeline = new Pipeline(); // 建立管線                                        
            var pp = pipeline.Start(cfg);// 啟動管線
            // 獲取深度流和彩色流的配置
            depthStream = pp.GetStream<VideoStreamProfile>(Stream.Depth);
            colorStream = pp.GetStream<VideoStreamProfile>(Stream.Color);
            SharedExtrinsics = depthStream.GetExtrinsicsTo(colorStream);// 獲取深度流到彩色流的外部參數  // extrinsics.rotation 是列主要的旋轉矩陣  // extrinsics.translation 是以米為單位的 xyz 位移
            //獲取並應用深度到彩色的外部參數
            var p1 = SharedExtrinsics.Transform(new Intel.RealSense.Math.Vector());
            Console.WriteLine($"將外部參數應用到原點:({p1.x}, {p1.y}, {p1.z})");
            InternalAndExternalGinseng();
            var sensor = pp.Device.QuerySensors().First(s => s.Is(Extension.DepthSensor));  // 取得深度感測器
            var blocks = sensor.ProcessingBlocks.ToList(); // 取得處理區塊
            Sensor sensor1 = pp.Device.Sensors[0];
            scale = sensor1.DepthScale;
            Console.WriteLine("取得深度單位" + scale + "m");
            try
            {
                // 建立自定義處理區塊
                block = new CustomProcessingBlock((f, src) =>
                    {
                        using (var releaser = new FramesReleaser())
                        {
                            // 對每個框架執行處理區塊
                            foreach (ProcessingBlock p in blocks)
                            {
                                f = p.Process(f).DisposeWith(releaser);
                                // 對齊
                                f = f.ApplyFilter(align).DisposeWith(releaser);
                                // 使用 colorizer 處理區塊上色
                                f = f.ApplyFilter(colorizer).DisposeWith(releaser);
                                // 取得影格
                                var frames = f.As<FrameSet>();
                                var colorFrame = frames[Stream.Color, Format.Rgb8].DisposeWith(releaser);
                                var depthFrame = frames[Stream.Depth, Format.Z16].DisposeWith(releaser);
                                var colorizedDepth = frames[Stream.Depth, Format.Rgb8].DisposeWith(releaser);
                                // 合成影格
                                var res = src.AllocateCompositeFrame(colorizedDepth, colorFrame, depthFrame);
                                src.FrameReady(res);
                            }
                        }
                    });
                // 啟動自定義區塊
                block.Start(f =>
                {
                    // 取得影格
                    using (var frames = f.As<FrameSet>())
                    {

                        // 取得彩色與深度影格
                        var _colorFrame = frames.ColorFrame.DisposeWith(frames);
                        var _depthFrame = frames.DepthFrame.DisposeWith(frames); // 原始的 depthFrame                                                                                    
                        var colorizedDepth = frames.First<VideoFrame>(Stream.Depth, Format.Rgb8).DisposeWith(frames); // 取得彩色化的深度影格

                        Image<Rgb, byte> emguImage = ColorFrameToImage(_colorFrame);  // 轉換成影像格式
                        bitmap = emguImage.ToBitmap();

                        var imageDepth = DepthFrameToImage(colorizedDepth);   // 轉換成影像格式
                        DepthBitmap = imageDepth.ToBitmap();// 將 Emgu CV 影像轉換成 Bitmap

                        // 在 UI 線程上更新 PictureBox
                        form.Invoke((MethodInvoker)delegate
                        {
                            form.SetPictureBox4Image(bitmap);
                            form.SetPictureBox5Image(DepthBitmap);
                        });
                        // 觸發 NewFrameArrived 事件
                        OnNewFrameArrived(_colorFrame, _depthFrame);
                        OnNewFrameArrived1(_colorFrame, _depthFrame);
                    }
                });
                cts = new CancellationTokenSource();
                // 创建并启动数据处理任务，覆盖现有的任务。
                dataProcessingTask = System.Threading.Tasks.Task.Factory.StartNew(async () =>
                {
                    while (!cts.Token.IsCancellationRequested)
                    {
                        using (var frames = pipeline.WaitForFrames())
                        {
                            if (frames != null)
                            {
                                block.Process(frames);
                                frameReceived = true;
                            }
                            else
                            {
                                frameReceived = false;
                            }
                        }
                        if (!frameReceived) // 如果没有接收到相机的帧数
                        {
                            if (form.cameraOpened) // 如果相机已经打开，则关闭它
                            {
                                form.cam = false;
                                form.cameraOpened = false;
                                await CloseCamera(); // 關閉相機
                            }

                            if (!form.cameraOpened) // 如果相机未打开，则打开它
                            {
                                form.cam = true;
                                OpenCamera(); // 打開像機
                            }
                            frameReceived = true;
                        }
                    }
                }, cts.Token);
            }

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

        }
        private void OnNewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
        {
            NewFrameEventHandler handler = NewFrameArrived;
            handler?.Invoke(colorFrame, depthFrame);
        }
        private void OnNewFrameArrived1(VideoFrame colorFrame, DepthFrame depthFrame)
        {
            NewFrameEventHandler handler11 = NewFrameArrived11;
            handler11?.Invoke(colorFrame, depthFrame);
        }

        // 將深度影格轉換為影像
        Image<Rgb, byte> DepthFrameToImage(VideoFrame frame)
        {
            // 轉換並複製影格資料
            var image = new Image<Rgb, byte>(frame.Width, frame.Height);
            var bytes = new byte[frame.Stride * frame.Height];
            System.Runtime.InteropServices.Marshal.Copy(frame.Data, bytes, 0, bytes.Length);
            // 指定影像的位元組資料
            image.Bytes = bytes;
            return image;
        }
        // 將彩色影格轉換為影像
        Image<Rgb, byte> ColorFrameToImage(VideoFrame frame)
        {
            // 轉換並複製影格資料
            var image = new Image<Rgb, byte>(frame.Width, frame.Height);
            var bytes = new byte[frame.Stride * frame.Height];
            System.Runtime.InteropServices.Marshal.Copy(frame.Data, bytes, 0, bytes.Length);
            // 指定影像的位元組資料
            image.Bytes = bytes;
            return image;
        }
        /// <summary>
        /// 關RealSenseL515相機
        /// </summary>
        /// <returns></returns>
        public async System.Threading.Tasks.Task CloseCamera()
        {
            if (pipeline != null)
            {
                CancellationTokenSource localCts = cts; // 避免并发访问
                System.Threading.Tasks.Task localDataProcessingTask = dataProcessingTask;

                try
                {
                    if (localCts != null && !localCts.IsCancellationRequested)
                    {
                        localCts.Cancel(); // 取消任务
                    }

                    if (localDataProcessingTask != null)
                    {
                        // 等待数据处理任务结束
                        await System.Threading.Tasks.Task.WhenAll(
                            localDataProcessingTask,
                            System.Threading.Tasks.Task.Delay(Timeout.Infinite, localCts.Token)
                        );
                    }
                }
                catch (OperationCanceledException)
                {
                    // 忽略任务取消异常
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Error during camera close: " + ex.Message);
                }
                finally
                {
                    if (pipeline != null)
                    {
                        pipeline.Stop(); // 停止管线
                        pipeline.Dispose(); // 释放资源
                        pipeline = null; // 确保不再引用已关闭的管线
                    }
                }
            }
        }
        /// <summary>
        /// 提取金樣本
        /// </summary>       
        [Obsolete]
        public void CaptureImagesAndSave() //提取金樣本
        {
           
            DigitalSignal digitalSignalDo0 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do0");
            DigitalSignal digitalSignalDo1 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do1");
            digitalSignalDo0.Value = 1;

            Form1.controller.Rapid.Start();
            // 使用一個新的執行緒來運行 while 迴圈
            Thread captureThread = new Thread(() =>
            {
                while (form.ImageProcessing)
                {
                    if (digitalSignalDo1.Value == 1)
                    {
                        // 清除舊的資料
                        coordTextBuilder.Clear();
                        objectCounter = 1; // 重置物體編號1                              
                        Yolo3D(); 
                        digitalSignalDo1.Value = 0;
                        form.ImageProcessing = false;
                        break;
                    }
                }
            });
            // 啟動執行緒
            captureThread.Start();
        }
        /// <summary>
        /// 提取比對樣本
        /// </summary>     
        [Obsolete]
        public void CompareSamples() //提取比對樣本
        {
            DigitalSignal digitalSignalDo0 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do0");
            DigitalSignal digitalSignalDo1 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do1");
            DigitalSignal digitalSignalDo3 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do3");
            digitalSignalDo1.Value = 0;
            digitalSignalDo0.Value = 1;
            Form1.controller.Rapid.Start();
            ManualResetEvent pauseEvent = new ManualResetEvent(true);
            // 使用一個新的執行緒來運行 while 迴圈
            Thread captureThread1 = new Thread(() =>
            {
                while (form.ImageProcessing1)
                {
                    Thread.Sleep(2); // 每 10 毫秒檢查一次
                    if (digitalSignalDo1.Value == 1)
                    {                   
                        Extract();
                        digitalSignalDo1.Value = 0;
                        digitalSignalDo3.Value = 0;
                        digitalSignalDo0.Value = 0;
                        form.ImageProcessing1 = false;            
                        // 暫停執行緒
                        pauseEvent.Reset();
                        break;
                    }
                }
            });
            // 啟動執行緒
            captureThread1.Start(); 
            // 恢復執行緒
            pauseEvent.Set(); 
        }
        [Obsolete]
        public void Extract()
        {       
                // 訂閱 NewFrameArrived 事件
           NewFrameArrived11 += Comparesamples_NewFrameArrived;    
        }
        [Obsolete]
        public async void Comparesamples_NewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
        {
            form.pictureBox6.Image = null;
            ConvertDepthToPointCloud(depthFrame);
            
            byte[] colorData1 = new byte[colorFrame.Width * colorFrame.Height * colorFrame.BitsPerPixel];
            colorFrame.CopyTo(colorData1);

            ushort[] depthData1 = new ushort[depthFrame.Width * depthFrame.Height];
            depthFrame.CopyTo(depthData1);

            using (VideoFrame depth1 = colorizer.Colorize(depthFrame))
            {
                Image<Rgb, byte> ColorImg1 = new Image<Rgb, byte>(colorFrame.Width, colorFrame.Height, colorFrame.Stride, colorFrame.Data);
                Image<Rgb, byte> DepthImg1 = new Image<Rgb, byte>(depth1.Width, depth1.Height, depth1.Stride, depth1.Data);

                //Image<Rgb, byte> DepthImg2 = DepthImg1;
                //DepthImg1Bitmap = DepthImg2.ToBitmap();

                string colorFileName = $"Compare samples_Color.bmp";
                string depthFileName = $"Compare samples_Depth.bmp";
                //string YOLO = $"YOLO.bmp";
                string colorPath = Path.Combine(folderPath1, colorFileName);
                string depthPath = Path.Combine(folderPath1, depthFileName);
                ColorImg1.Save(colorPath);
                form.pictureBox6.Image = ColorImg1.Bitmap;
                DepthImg1.Save(depthPath);
            }
            // 取得當前日期和時間
            DateTime now = DateTime.Now;
            if (deepestZ != float.MinValue && deepestZ - shallowestZ > (GolddeepestZ - GoldshallowestZ) / 2.5 && i > percentageInsideCrop && deepestZ - shallowestZ < GolddeepestZ - GoldshallowestZ + 2)
            {
                NewFrameArrived11 -= Comparesamples_NewFrameArrived;
                // 以日期時間為名稱建立資料夾
                folderPath1 = $"C:\\c#\\cam\\比對樣本{now:yyyy_MM_dd_HH_mm_ss}";
                form.Home();            
                await CalculateDisplacement();
            }
            else
            {
                // 以日期時間為名稱建立資料夾
                folderPath1 = $"C:\\c#\\cam\\比對樣本超過範圍{now:yyyy_MM_dd_HH_mm_ss}";
                if (form.checkBox1.Checked)
                {
                    MessageBox.Show("超過離鏡頭允許範圍，請確認物件擺放位置\n 距離誤超過樣本位置3公分\n 確保物件在ROI範圍內");
                    NewFrameArrived11 -= Comparesamples_NewFrameArrived;
                    form.button30.Enabled = true;
                    form.ok1 = true;
                }
            }
            // 检查文件夹是否存在，如果不存在则创建
            if (!Directory.Exists(folderPath1))
            {
                Directory.CreateDirectory(folderPath1);
            }
            
        }
       

        /// <summary>
        /// YOLO辨識
        /// </summary> 
        public System.Drawing.Image ProcessImage(System.Drawing.Image colorBitmap) //YOLOV5-ONNX
        {
            yoloFeaturePoints.Clear(); // 清空上次的特征点  

            // 先複製原始圖片
            var image = new Bitmap(colorBitmap);
            var scorer = new YoloScorer<YoloCocoP5Model>("C:/c#/20231102/exp/weights/best.onnx");
            List<YoloPrediction> predictions = scorer.Predict(image);
            var graphics = Graphics.FromImage(image);

            // 按照 centerX + centerY 的大小排序 predictions
            predictions = predictions.OrderBy(prediction => prediction.Rectangle.Left + prediction.Rectangle.Width / 2 + prediction.Rectangle.Top + prediction.Rectangle.Height / 2).ToList();

            foreach (var prediction in predictions)
            {
                double score = Math.Round(prediction.Score, 2);
                graphics.DrawRectangles(new System.Drawing.Pen(prediction.Label.Color, prediction.Label.PenWidth), new[] { prediction.Rectangle });
                var (x, y) = (prediction.Rectangle.X - 3, prediction.Rectangle.Y - 37);
                // 計算方寬中心點和方寬寬高
                float centerX = prediction.Rectangle.Left + prediction.Rectangle.Width / 2;
                float centerY = prediction.Rectangle.Top + prediction.Rectangle.Height / 2;
                float width = prediction.Rectangle.Width;
                float height = prediction.Rectangle.Height;
                // 將物體編號、物體名稱和信心值一起顯示在標籤中
                var labelWithScore = $"{objectCounter}"; //{prediction.Label.Name}
                graphics.DrawString(labelWithScore,
                    new Font("Times New Roman", 20, GraphicsUnit.Pixel), new SolidBrush(prediction.Label.Color),
                    new PointF(x, y));
                Item = new ListViewItem(new[]
                {
                    objectCounter.ToString(), // 編號
                    prediction.Label.Name.ToString(),// 物體名稱
                    score.ToString(),         // 信心值
                    (centerX,centerY).ToString(),// 方寬中心點                  
                    (height,width).ToString()   // 方寬寬高                  
                });

                form.listView4.Items.Add(Item);
                objectCounter++; // 增加物體编號
                if (prediction.Label.Name == "solder joint") //篩選特徵點
                {
                    yoloFeaturePoints.Add(new System.Drawing.PointF(centerX, centerY));// 將YOLO預測的特徵點座標添加到列表
                }
            }
            form.pictureBox1.Image = image;

            return new Bitmap(image); // 返回新的 Bitmap
        }
        /// <summary>
        /// 儲存彩色和深度影像和YOLO影像
        /// </summary>
        /// <param name="colorImage"></param>
        /// <param name="depthImage"></param>
        /// <param name="yoloImage"></param>
        private void SaveImages()// 儲存金樣本彩色和深度影像和YOLO影像
        {
            folderPath = "C:\\c#\\cam\\金樣本";

            // 检查文件夹是否存在，如果不存在则创建
            if (!Directory.Exists(folderPath))
            {
                Directory.CreateDirectory(folderPath);
            }
            // 在這裡加入儲存影像的程式碼，可以使用 Bitmap.Save 方法或其他方式進行儲存
            string yoloName = $"Gold Standard_YoLo.bmp";
            string yoloPath = Path.Combine(folderPath, yoloName);
            yoloimage.Save(yoloPath);
            // 釋放 yoloImage
            yoloimage.Dispose();
            string colorFileName = $"Gold Standard_Color.bmp";
            string depthFileName = $"Gold Standard_Depth.bmp";
            string colorPath = Path.Combine(folderPath, colorFileName);
            string depthPath = Path.Combine(folderPath, depthFileName);
            ColorImg.Save(colorPath);
            DepthImg.Save(depthPath);
            Console.WriteLine("影像已儲存");
        }
        #region 坐標系轉換
        /// <summary>
        /// $"YOLO特徵點({u}, {v})的三維座標: X={X}, Y={Y}, Z={Z}"
        /// </summary>
        [Obsolete]
        public void Yolo3D()
        {
            storedCoordinates.Clear();
            ColorData = null;
            DepthData = null;
            // 初始化彩色圖像和深度圖像
            ColorImg = null;
            DepthImg = null;
            // 訂閱 NewFrameArrived 事件
            NewFrameArrived += Yolo3D_NewFrameArrived;
        }
        [Obsolete]
        private void Yolo3D_NewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
        {
            // 有興趣範圍
            CroppingRect = new Rectangle(form.left, form.top, form.right - form.left, form.bottom - form.top);

            ColorData = new byte[colorFrame.Width * colorFrame.Height * colorFrame.BitsPerPixel];
            colorFrame.CopyTo(ColorData);

            DepthData = new ushort[depthFrame.Width * depthFrame.Height];
            depthFrame.CopyTo(DepthData);


            using (VideoFrame depth = colorizer.Colorize(depthFrame))
            {
                ColorImg = new Image<Rgb, byte>(colorFrame.Width, colorFrame.Height, colorFrame.Stride, colorFrame.Data);
                DepthImg = new Image<Rgb, byte>(depth.Width, depth.Height, depth.Stride, depth.Data);
                Image<Rgb, byte> DepthImg1 = DepthImg;
                DepthImg1Bitmap = DepthImg1.ToBitmap();
            }
            // 確保彩色圖丟進 YOLO 出來的特徵點是跟深度影像點位一樣的
            yoloimage = (Bitmap)ProcessImage(ColorImg.Bitmap); // 丟入 YOLO
            Bitmap yolo = (Bitmap)yoloimage.Clone(); // 使用 Clone 方法進行複製
            using (Graphics g = Graphics.FromImage(DepthImg1Bitmap))
            {

                // 假設 deph 是包含深度值的集合
                List<float> deph2 = new List<float>();
                // 使用 yoloFeaturePoints 進行處理
                foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
                {

                    // 假设 maxDepth 是您深度图的最大深度值
                    //float maxDepth = 100.0f;

                    //featurePoint 為彩色YOLO過的特徵點座標
                    int u = (int)featurePoint.X;
                    int v = (int)featurePoint.Y;
                    // 在深度图上对应像素的坐标
                    int depthPixelX = (int)u;
                    int depthPixelY = (int)v;


                    Z = depthFrame.GetDistance(depthPixelX, depthPixelY);
                    // 將深度值加入到 Z 集合中
                    deph2.Add(Z);

                    //獲取影像相機座標系
                    cameraPoint = Compute3DCoordinate(depthPixelX, depthPixelY, Z);

                    MathNet.Numerics.LinearAlgebra.Vector<double> cameraPointVector = MathNet.Numerics.LinearAlgebra.Vector<double>.Build.Dense(new double[] { cameraPoint.X, cameraPoint.Y, cameraPoint.Z });
                    cameraPointsList.Add(cameraPointVector);

                    // 输出深度值
                    Console.WriteLine($"YOLO特徵點 ({depthPixelX}, {depthPixelY}): {Z * 100} cm");
                    coordTextBuilder.AppendLine($"YOLO特徵點({u}, {v}): 深度值: {Z * 100} cm");
                    form.label16.Text = coordTextBuilder.ToString();
                    Console.WriteLine($"相機座標系: ({cameraPoint.X * 1000}, {cameraPoint.Y * 1000}, {cameraPoint.Z * 1000}) mm");
                    System.Numerics.Vector3 currentCoordinate = new System.Numerics.Vector3(cameraPoint.X * 1000, cameraPoint.Y * 1000, cameraPoint.Z * 1000);
                    storedCoordinates.Add(currentCoordinate);
                }
                // 找出 Z 集合中的最大值
                MaxDepthValue = deph2.Max();
                Console.WriteLine($"maxDepthValue {MaxDepthValue} cm");
                form.label21.Text = "待焊物離鏡頭距離不超過" + (MaxDepthValue * 100 + 3) + "cm";

            }
            using (Graphics g = Graphics.FromImage(DepthImg1Bitmap))
            {
                // 使用 yoloFeaturePoints 進行處理
                foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
                {
                    // 假设 maxDepth 是您深度图的最大深度值
                    float maxDepth = 100.0f;
                    //featurePoint 為彩色YOLO過的特徵點座標
                    float u = featurePoint.X;
                    float v = featurePoint.Y;
                    // 在深度图上对应像素的坐标
                    int depthPixelX = (int)u;
                    int depthPixelY = (int)v;
                    // 在畫布上繪製特徵中心點
                    float depthPointRadius = 10.0f;
                    g.FillEllipse(System.Drawing.Brushes.Red, Convert.ToSingle(u - depthPointRadius), Convert.ToSingle(v - depthPointRadius), Convert.ToSingle(depthPointRadius * 2), Convert.ToSingle(depthPointRadius * 2));

                    float rectSize = 5.0f;
                    // 将深度值映射到颜色值（这里是简单的映射示例，具体映射关系可能需要根据实际情况调整）
                    byte mappedDepthColor = (byte)(Z / maxDepth * 255);

                    g.FillEllipse(new SolidBrush(System.Drawing.Color.FromArgb(mappedDepthColor, mappedDepthColor, mappedDepthColor)), depthPixelX - rectSize, depthPixelY - rectSize, 2 * rectSize, 2 * rectSize);

                }
            }
            using (Graphics g = Graphics.FromImage(yolo))
            {
                // 使用 yoloFeaturePoints 進行處理
                foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
                {
                    // 假设 maxDepth 是您深度图的最大深度值
                    float maxDepth = 100.0f;
                    //featurePoint 為彩色YOLO過的特徵點座標
                    float u = featurePoint.X;
                    float v = featurePoint.Y;
                    // 在深度图上对应像素的坐标
                    int depthPixelX = (int)u;
                    int depthPixelY = (int)v;
                    // 在畫布上繪製特徵中心點
                    float depthPointRadius = 10.0f;
                    g.FillEllipse(System.Drawing.Brushes.Red, Convert.ToSingle(u - depthPointRadius), Convert.ToSingle(v - depthPointRadius), Convert.ToSingle(depthPointRadius * 2), Convert.ToSingle(depthPointRadius * 2));
                    // 在pictureBox1上绘制矩形表示深度图上的颜色值
                    float rectSize = 5.0f;
                    // 将深度值映射到颜色值（这里是简单的映射示例，具体映射关系可能需要根据实际情况调整）
                    byte mappedDepthColor = (byte)(Z / maxDepth * 255);
                    // 在pictureBox1上绘制矩形表示深度图上的颜色值                 
                    g.FillEllipse(new SolidBrush(System.Drawing.Color.FromArgb(mappedDepthColor, mappedDepthColor, mappedDepthColor)), depthPixelX - rectSize, depthPixelY - rectSize, 2 * rectSize, 2 * rectSize);

                }
            }        
            form.groupBox2.Enabled = true;
            form.pictureBox1.Image = yolo;
            form.pictureBox2.Image = ColorImg.Bitmap;
            form.pictureBox3.Image = DepthImg1Bitmap;
            form.pictureBox3.Refresh();
            form.pictureBox3.Refresh();
            form.PlanningAOI();
            // 檢查是否滿足條件，如果沒有，就等待直到滿足
            while (!form.ROI)
            {
                Thread.Sleep(10); // 每 10 毫秒檢查一次
                if (form.ImageProcessing == true)
                {
                    break;
                }
            }
            form.goldsample = true;
            //建立點雲
            ConvertDepthToPointCloud(depthFrame);
            form.goldsample = false;
            SaveImages();
            TakeImage = true;
            NewFrameArrived -= Yolo3D_NewFrameArrived;
        }
        //建立點雲      
        public void ConvertDepthToPointCloud(DepthFrame depthFrame)
        {
            i = 0;
            threshold = MaxDepthValue * 1000 + 40;
            
            // 初始化追蹤最深點的座標和深度值
            deepestZ = float.MinValue;
            deepestX = 0;
            deepestY = 0;
            // 初始化追蹤最淺點的座標和深度值
            shallowestZ = float.MaxValue;
            shallowestX = 0;
            shallowestY = 0;
            // 初始化追蹤 y 軸最小值和最大值
            float yMin = float.MaxValue;
            float yMax = float.MinValue;
            // 建立一個StringBuilder來儲存.pcd檔的內容
            StringBuilder pcdData = new StringBuilder();

            // 初始化一個列表來儲存點雲數據
            List<string> pointCloudData = new List<string>();
            // 取得影像中間的y座標值
           
            // 遍歷深度數據的每一個元素
            for (int y = 0; y < depthFrame.Height; y++)
            {
                for (int x = 0; x < depthFrame.Width; x++)
                {
                    // 檢查點是否在裁剪範圍內
                    if (x >= form.left && x <= form.right && y >= form.top && y <= form.bottom)
                    {
                        // 將深度數據轉換為點雲數據
                        float Z2 = (float)depthFrame.GetDistance(x, y);
                        Z2 *= 1000;
                        if (Z2 < threshold && Z2 != 0)
                        {
                            float xx = (float)((x - colorIntrinsics.ppx) * Z2 / colorIntrinsics.fx);
                            float yy = (depthFrame.Height - y - colorIntrinsics.ppy) * Z2 / colorIntrinsics.fy;  // 翻轉y軸的座標
                            float z = Z2;

                            // 更新 y 軸的最小值和最大值
                            if (yy < yMin) yMin = yy;
                            if (yy > yMax) yMax = yy;


                            // 將點雲數據儲存到列表中
                            pointCloudData.Add($"{xx} {yy} {z}");
                            i++;
                            // 根據 y 軸的範圍來定義矩形框的大小
                            float boxSize = (yMax - yMin) / 5;  // 以五行為主
                            float boxMin = yMin;
                            float boxMax = boxMin + boxSize;

                            // 在定義的矩形框中找到最淺的點和最深的點
                            if (yy >= boxMin && yy <= boxMax)
                            {
                                // 更新最深點的座標和深度值
                                if (Z2 > deepestZ)
                                {
                                    deepestZ = Z2;
                                    deepestX = (int)xx;
                                    deepestY = (int)yy;
                                }
                                // 更新最淺點的座標和深度值
                                if (Z2 < shallowestZ)
                                {
                                    shallowestZ = Z2;
                                    shallowestX = (int)xx;
                                    shallowestY = (int)yy;
                                }
                            }

                        }
                    }
                }
            }
            // 最深點的座標和深度值
            Console.WriteLine($"深度限制: {threshold}");
            Console.WriteLine($"最深點: X={deepestX}, Y={deepestY}, Z={deepestZ}"); 
            Console.WriteLine($"最淺點: X={shallowestX}, Y={shallowestY}, Z={shallowestZ}");
            // 寫入.pcd檔的頭部資訊
            pcdData.AppendLine("VERSION .7");
            pcdData.AppendLine("FIELDS x y z");
            pcdData.AppendLine("SIZE 4 4 4");
            pcdData.AppendLine("TYPE F F F");
            pcdData.AppendLine("COUNT 1 1 1");
            pcdData.AppendLine("WIDTH " + i);
            pcdData.AppendLine("HEIGHT " + 1);
            pcdData.AppendLine("VIEWPOINT 0 0 0 1 0 0 0");
            pcdData.AppendLine("POINTS " + i);
            pcdData.AppendLine("DATA ascii");
            // 寫入.pcd檔時，從列表中取出點雲數據並寫入.pcd檔
            foreach (string point in pointCloudData)
            {
                pcdData.AppendLine(point);
            }
            if (form.goldsample == true)
            {
                Goldsamplepointcloudnumber = i;
                // 計算在裁剪範圍內的點的百分比
                percentageInsideCrop = Goldsamplepointcloudnumber * 0.7;
                GolddeepestZ = deepestZ;
                GoldshallowestZ = shallowestZ;
                Console.WriteLine($"金樣本點雲數量 {Goldsamplepointcloudnumber}");
                // 指定儲存.pcd檔的資料夾
                folderPath = "C:\\c#\\cam\\金樣本";
                // 指定.pcd檔的檔名
                fileName = "Gold Standard.pcd";
                // 檢查資料夾是否存在，如果不存在，則建立該資料夾
                if (!Directory.Exists(folderPath))
                {
                    Directory.CreateDirectory(folderPath);
                }
                // 檔案將被儲存到指定的資料夾
                File.WriteAllText(Path.Combine(folderPath, fileName), pcdData.ToString());
            }
            else
            {
                Console.WriteLine($"比對樣本點雲數量 {i}");
                // 取得當前日期和時間
                DateTime now = DateTime.Now;
                if (deepestZ != float.MinValue && deepestZ - shallowestZ > (GolddeepestZ - GoldshallowestZ) / 2.5 && i > percentageInsideCrop && deepestZ - shallowestZ < GolddeepestZ - GoldshallowestZ  + 2)
                {
                    // 以日期時間為名稱建立資料夾
                    folderPath1 = $"C:\\c#\\cam\\比對樣本{now:yyyy_MM_dd_HH_mm_ss}";

                }
                else
                {
                    // 以日期時間為名稱建立資料夾
                    folderPath1 = $"C:\\c#\\cam\\比對樣本超過範圍{now:yyyy_MM_dd_HH_mm_ss}";
                }
                // 指定.pcd檔的檔名
                fileName1 = "Compare samples.pcd";
                // 檢查資料夾是否存在，如果不存在，則建立該資料夾
                if (!Directory.Exists(folderPath1))
                {
                    Directory.CreateDirectory(folderPath1);
                }
                File.WriteAllText(Path.Combine(folderPath1, fileName1), pcdData.ToString());
            }           
        }
        /// <summary>
        /// 相機座標系中的像素座標轉換成相機座標系中的三維空間座標
        /// </summary>
        /// <param name="u"></param>
        /// <param name="v"></param>
        /// <param name="Z"></param>
        /// <param name="dist">DepthFrame在座標(x,y)的深度值(單位：m)</param>
        /// <returns></returns>
        public System.Numerics.Vector3 Compute3DCoordinate(double x, double y, double Z)
        {
            X = (float)((x - colorIntrinsics.ppx) * Z / colorIntrinsics.fx);
            Y = (float)((y - colorIntrinsics.ppy) * Z / colorIntrinsics.fy);
            // 畸變模型
            if (colorIntrinsics.model == Distortion.ModifiedBrownConrady)
            {
                float r2 = (float)(X * X + Y * Y);
                float f = 1 + colorIntrinsics.coeffs[0] * r2 + colorIntrinsics.coeffs[1] * r2 * r2 + colorIntrinsics.coeffs[4] * r2 * r2 * r2;
                float ux = (float)(X * f + 2 * colorIntrinsics.coeffs[2] * X * Y + colorIntrinsics.coeffs[3] * (r2 + 2 * X * X));
                float uy = (float)(Y * f + 2 * colorIntrinsics.coeffs[3] * X * Y + colorIntrinsics.coeffs[2] * (r2 + 2 * Y * Y));
                X = ux;
                Y = uy;
            }
            Console.WriteLine($"Compute3DCoordinate: {X} m ,{Y} m ,{Z} m");
            return new System.Numerics.Vector3((float)X, (float)Y, (float)Z);
        }
        /// <summary>
        /// 相機內外參參數
        /// </summary> 
        internal void InternalAndExternalGinseng()//內參出輸
        {
            // 打印深度流和彩色流的内参信息
            depthIntrinsics = depthStream.GetIntrinsics();
            depthExtrinsics = depthStream.GetExtrinsicsTo(colorStream);
            colorIntrinsics = colorStream.GetIntrinsics();
            colorExtrinsics = colorStream.GetExtrinsicsTo(depthStream);
            Console.WriteLine("深度相機内参:");
            Console.WriteLine("寬度: " + depthIntrinsics.width);// 深度相機寬度
            Console.WriteLine("高度: " + depthIntrinsics.height);// 深度相機高度
            Console.WriteLine("Fx: " + depthIntrinsics.fx);// 深度相機的水平焦距
            Console.WriteLine("Fy: " + depthIntrinsics.fy);// 深度相機的垂直焦距
            Console.WriteLine("Cx: " + depthIntrinsics.ppx);// 深度相機的主點在水平方向上的坐標
            Console.WriteLine("Cy: " + depthIntrinsics.ppy);// 深度相機的主點在垂直方向上的坐標
            Console.WriteLine("模型: " + depthIntrinsics.model);// 深度相機的内參模型

            Console.WriteLine("彩色相機内参:");
            Console.WriteLine("寬度: " + colorIntrinsics.width);
            Console.WriteLine("高度: " + colorIntrinsics.height);
            Console.WriteLine("Fx: " + colorIntrinsics.fx);
            Console.WriteLine("Fy: " + colorIntrinsics.fy);
            Console.WriteLine("Cx: " + colorIntrinsics.ppx);
            Console.WriteLine("Cy: " + colorIntrinsics.ppy);
            Console.WriteLine("模型: " + colorIntrinsics.model);
        }
        #endregion
        /// <summary>
        /// 計算位移量
        /// </summary>
        [Obsolete]
        public async Task CalculateDisplacement()
        {
            await Task.Run(async () =>
            {
                using (Process process = new Process())
                {
                    form.cam = false;
                    form.cameraOpened = false;
                    form.label17.Text = "相機狀態:關";
                    form.button27.Text = "開相機";
                    form.label17.BackColor = Color.Transparent;
                    await CloseCamera(); // 關閉相機
                    process.StartInfo.FileName = "C:\\Users\\asasa\\Desktop\\PCLdemo\\build\\Debug\\PCLdemo.exe"; // C++程式的路徑
                    process.StartInfo.UseShellExecute = false;
                    process.StartInfo.RedirectStandardOutput = true;
                    process.Start();
                    // 讀取C++程式的輸出
                    string output = process.StandardOutput.ReadToEnd();
                    process.WaitForExit();

                    // 顯示C++程式的完整輸出
                    Console.WriteLine("C++程式的輸出為:");
                    Console.WriteLine(output);

                    // 指定輸出文件的路徑
                    string outputFilePath = "C:\\c#\\cam\\output.txt";

                    // 讀取輸出文件的整個內容
                    string fileContent = File.ReadAllText(outputFilePath);

                    // 將文件內容按逗號分割成字符串數組
                    string[] values = fileContent.Split(',');

                    // 檢查值的數量是否正確
                    if (values.Length == 6)
                    {
                        // 提取並使用每個值
                        if (float.TryParse(values[0], out x) &&
                            float.TryParse(values[1], out y) &&
                            float.TryParse(values[2], out z) &&
                            float.TryParse(values[3], out Rx) &&
                            float.TryParse(values[4], out Ry) &&
                            float.TryParse(values[5], out Rz))
                        {
                            // 显示提取到的值
                            Console.WriteLine($"x: {x}, y: {y}, z: {z}, Rx: {Rx}, Ry: {Ry}, Rz: {Rz}");
                            if (x != 0)
                            {
                                // 嘗試關閉 C++ 進程
                                if (!process.HasExited)
                                {
                                    process.Kill(); // 強制關閉                            
                                }

                                form.Pathchange();
                            }
                        }
                        else
                        {
                            Console.WriteLine("無法解析一個或多個值。");
                        }
                    }
                    else
                    {
                        Console.WriteLine("輸出格式無效。");
                    }
                }
            });
     
        }
    }
}