using System;
using Dll_CSharp_VSensorLaser;
using System.Collections.Generic;
using System.Threading.Tasks;
namespace Demo.Testdll
{
    public class Program
    {
        static void Main(string[] args)
        {
            try
            {
                //获取相对路径
                string path = System.IO.Directory.GetCurrentDirectory();
                //实例化设备
                VSensorLaserDLL VSensor = new VSensorLaserDLL();
                //当前SDK版本
                string version = "";
                VSensor.GetSdkVersionString(ref version);
                Console.WriteLine("sdk version: {0}\n", version);
                //获取设备列表
                List<VSensorLaserCameraInfoCLR> list = new List<VSensorLaserCameraInfoCLR>(20);
                for (int i = 0; i < 20; i++)
                {
                    list.Add(new VSensorLaserCameraInfoCLR());
                }
                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();
                }
                //设置曝光时间
                status = VSensor.SetExposureTime(2.0);
                if (status != 0)
                {
                    Console.WriteLine("set exposure time failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设置投影等级
                status = VSensor.SetProjectLight(1);
                if (status != 0)
                {
                    Console.WriteLine("set project light 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, height = 0;
                VSensor.GetImageResolution(ref width, ref height);
                VSensorLaserResultCLR result = new VSensorLaserResultCLR(width, height);
                VSensor.GetRestructionResult(ref result);
/*
                //读取VSensorLaserResult结构体获取无序点云
                for (int i = 0; i < result.iWidth; 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);
                if (status != 0)
                {
                    Console.WriteLine("save gray image failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //设置为预览模式
                status = VSensor.SetCaptureMode(3);
                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];
                status = VSensor.CameraOut(ref gray);
                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();
                }
                //点云录制
                status = VSensor.Record3DCloud(path + @"\test_record.pcd", 1, 10);
                if (status != 0)
                {
                    Console.WriteLine("set record 3D cloud failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //连续采集
                Console.WriteLine("start continuous capture...\n");
                VSensor.ContinuousRestructionStart(0);
                for (int i = 0; i < 10; i++)
                {
                    status = VSensor.CaptureFrame(1000);
                    if (status != 0)
                    {
                        Console.WriteLine("capture frame failed, status = {0}\n", status);
                    }
                    else
                    {
                        VSensor.GetRestructionResult(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();
                //获取录制点云
                List<Tuple<float, float, float>> cloud = new List<Tuple<float, float, float>>();
                status = VSensor.GetRecord3DCloud(ref cloud);
                if (status != 0)
                {
                    Console.WriteLine("get record 3D cloud failed, status = {0}\n", status);
                    Console.ReadLine();
                }
                //反初始化设备
                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();
        }
    }
}
