using System;
using Dll_CSharp_VSensor;
using System.Collections.Generic;
namespace Demo.Testdll
{
    public class Program
    {
        static void Main(string[] args)
        {
            try
            {
                //获取相对路径
                string path = System.IO.Directory.GetCurrentDirectory();
                //实例化设备
                VSensorDLL VSensor = new VSensorDLL();
                //当前SDK版本
                string version = "";
                VSensor.GetSdkVersionString(ref version);
                Console.WriteLine("sdk version: {0}\n", version);
                //获取设备列表
                List<VSensorCameraInfoCLR> list = new List<VSensorCameraInfoCLR>(20);
                for (int i = 0; i < 20; i++)
                {
                    list.Add(new VSensorCameraInfoCLR());
                }
                int num = 0;
                VSensor.GetDeviceList(ref list, ref num);
                for (int i = 0; i < num; i++)
                {
                    Console.WriteLine("device num {0} : list[{1}] = {2}\n", i, i, list[i].CameraName);
                }
                //连接第一台设备
                int status = VSensor.DeviceConnect(0);
                if (status != 0)
                {
                    Console.WriteLine("connect device failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设备参数初始化，仅上电时需要进行此操作，初始化需要等待15s
                Console.WriteLine("waiting for parameter init...\n");
                status = VSensor.DeviceParameterInit();
                if (status != 0)
                {
                    Console.WriteLine("parameter init failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设置为采集模式
                status = VSensor.SetCaptureMode(1);
                if (status != 0)
                {
                    Console.WriteLine("set capture mode failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //单次采集测试
                Console.WriteLine("single restruction ...\n");
                status = VSensor.SingleRestruction(0);
                if (status != 0)
                {
                    Console.WriteLine("single restruction failed, status = {0}\n", status);
                    Console.ReadLine();
                }
/*
                //获取当前相机输出分辨率(适用于有彩色相机的设备)
                int width = 0;
                int height = 0;
                int widthRgb = 0;
                int heightRgb = 0;
                VSensor.GetImageResolution(ref width, ref height, ref widthRgb, ref heightRgb);
                VSensorResultCLR result = new VSensorResultCLR(width, height, widthRgb, heightRgb);
                VSensor.GetRestructionResult(0, 0, 1, ref result);
*/
                //获取当前相机输出分辨率
                int width = 0;
                int height = 0;
                VSensor.GetImageResolution(ref width, ref height);
                VSensorResultCLR result = new VSensorResultCLR(width, height);
                VSensor.GetRestructionResult(0, 0, 1, ref result);
/*
               //读取VSensorResult结构体获取无序点云
               for (int i = 0; i < result.iWidth * result.iHeight; i++)
               {
                   if (result.mask[i])
                   {
                       //点云操作
                       float x = result.pointx[i];
                       float y = result.pointy[i];
                       float z = result.pointz[i];
                   }
               }
*/
                //保存点云为ply格式
                status = VSensor.Save3DCloud(path + @"\test.ply", result, 0);
                if (status != 0)
                {
                    Console.WriteLine("save 3D points failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //保存灰度图
                status = VSensor.SaveGrayMap(path + @"\gray.bmp", result, 0);
                if (status != 0)
                {
                    Console.WriteLine("save gray image failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //保存深度图
                status = VSensor.SaveDepthMap(path + @"\depth.png", result);
                if (status != 0)
                {
                    Console.WriteLine("save depth image failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设置为预览模式
                status = VSensor.SetCaptureMode(0);
                if (status != 0)
                {
                    Console.WriteLine("set capture mode failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //获取当前相机输出分辨率
                VSensor.GetImageResolution(ref width, ref height);
                //获取2D图
                Console.WriteLine("start view capture...\n");
                var gray = new byte[width * height * 2];
                var rgb = new byte[width * height * 3];
                status = VSensor.CameraOut(ref gray, ref rgb);
                if (status != 0)
                {
                    Console.WriteLine("camera out failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设置为采集模式
                status = VSensor.SetCaptureMode(1);
                if (status != 0)
                {
                    Console.WriteLine("set capture mode failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //连续采集
                Console.WriteLine("start continuous capture...\n");
                VSensor.ContinuousRestructionStart(1);
                for (int i = 0; i < 5; i++)
                {
                    status = VSensor.CaptureFrame(8000);
                    if (status != 0)
                    {
                        Console.WriteLine("capture frame failed, status = {0}\n", status);
                    }
                    else
                    {
                        VSensor.GetRestructionResult(0, 1, 1, ref result);
                        Console.WriteLine("{0}   num =  {1}    ret:{2}\n", i + 1, result.nPointNum, status);
                        string varString = Convert.ToString(i);
                        string str = path + @"\test" + varString + ".ply";
                        VSensor.Save3DCloud(str, result, 0);
                    }
                }
                VSensor.ContinuousRestructionStop();
                //反初始化设备
                status = VSensor.DeviceUnInit();
                if (status != 0)
                {
                    Console.WriteLine("device release failed, status = {0}\n", status);
                    Console.ReadLine();
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine($"ex:{ex}");
            }
            Console.WriteLine("run success");
            Console.ReadLine();
            Console.ReadKey();
        }
    }
}
