#include "VSensor.h" 
#include <signal.h> 
#include <direct.h> 
#pragma comment(lib, "VSensorSDK.lib")
using namespace std;
using namespace VSENSOR;
int main()
{
    //获取相对路径
    char cwd[200];
    if (_getcwd(cwd, 200) == nullptr)
    {
        printf("get path failed\n");
    }
    //实例化设备
    VSensor* pVSensor = new VSensor();
    //获取sdk版本
    string version = string();
    pVSensor->GetSdkVersionString(version);
    printf("sdk version: %s\n", version.c_str());
    //获取设备列表
    VSensorCameraInfo cameraList[20];
    int num = 0;
    pVSensor->GetDeviceList(cameraList, &num);
    for (int i = 0; i < num; i++)
    {
        printf("device num %d : list[%d] = %s\n", i, i, cameraList[i].CameraName);
    }
    //连接设备，0为连接第一台设备  
    int status = pVSensor->DeviceConnect(0);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("connect device failed status = %d\n", status);
        system("pause");
    }
    //设备参数初始化，仅初次上电时需要进行此操作且该函数需要等待15s
    printf("waiting for parameter init...\n");
    status = pVSensor->DeviceParameterInit();
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("parameter init failed, status = %d\n", status);
        system("pause");
    }
    //设置为采集模式
    status = pVSensor->SetCaptureMode(emVSensorCaptureMode::MODE_CAPTURE);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("set capture mode failed, status = %d\n", status);
        system("pause");
    }
    //单次采集
    printf("start single capture...\n");
    VSensorResult *pResult = new VSensorResult;
    status = pVSensor->SingleRestruction(pResult, emVSensorCaptureOutputMode::OUTPUT_MODE_ALL);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("single restruction failed, status = %d\n", status);
        system("pause");
    }
/*
    //读取VSensorResult结构体获取无序点云
    for (int i = 0; i < pResult->ImageResolution.Width * pResult->ImageResolution.Height; i++)
    {
        if (pResult->mask[i])
        {
            //点云操作
            float x = pResult->pointx[i];
            float y = pResult->pointy[i];
            float z = pResult->pointz[i];
        }
    }
*/
    //保存无序点云为pcd格式
    status = pVSensor->Save3DCloud(string(cwd) + "//test.pcd", pResult, emVSensorPointType::POINT_TYPR_DISORDER);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("save 3D points failed, status = %d\n", status);
        system("pause");
    }
    //保存原始灰度图
    status = pVSensor->SaveGrayMap(string(cwd) + "//gray.bmp", pResult, emVSensorGrayType::GRAY_TYPR_UNCORRECT);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("save gray image failed, status = %d\n", status);
        system("pause");
    }
    //保存深度图
    status = pVSensor->SaveDepthMap(string(cwd) + "//depth.png", pResult);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("save depth image failed, status = %d\n", status);
        system("pause");
    }
    //设置为预览模式
    status = pVSensor->SetCaptureMode(emVSensorCaptureMode::MODE_VIEW);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("set capture mode failed, status = %d\n", status);
        system("pause");
    }
    //获取分辨率
    VSensorImageResolution ImageResolution;
    status = pVSensor->GetImageResolution(&ImageResolution);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("get image resolution failed, status = %d\n", status);
        system("pause");
    }
    //获取2D图
    printf("start view capture...\n");
    unsigned char* gray = new unsigned char[ImageResolution.Width * ImageResolution.Height * 2];
    status = pVSensor->CameraOut(gray, nullptr);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("camera out failed, status = %d\n", status);
        system("pause");
    }
    delete[] gray;
    //设置为采集模式
    status = pVSensor->SetCaptureMode(emVSensorCaptureMode::MODE_CAPTURE);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("set capture mode failed, status = %d\n", status);
        system("pause");
    }
    //连续采集
    printf("start continuous capture...\n");
    pVSensor->ContinuousRestructionStart(emVSensorCaptureOutputMode::OUTPUT_MODE_ALL);
    for (int i = 0; i < 5; i++)
    {
        status = pVSensor->CaptureFrame(pResult, 8000);
        if (status != CAMERA_STATUS_SUCCESS)
        {
            printf("capture frame failed, status = %d\n", status);
        }
        else
        {
            printf("%d  num =  %d\n", i + 1, pResult->nPointNum);
            string str(string(cwd) + "//test" + to_string(i + 1) + ".pcd");
            status = pVSensor->Save3DCloud(str, pResult, emVSensorPointType::POINT_TYPR_DISORDER);
            if (status != CAMERA_STATUS_SUCCESS)
            {
                printf("save 3D points failed, status = %d\n", status);
                system("pause");
            }
        }
    }
    pVSensor->ContinuousRestructionStop();
    //反初始化设备
    status = pVSensor->DeviceUnInit();
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("device release failed, status = %d\n", status);
        system("pause");
    }
    delete pResult;
    delete pVSensor;
    printf("run success\n");
    system("pause");
    return 0;
}