#include "VSensor.h"
#include <unistd.h>
#include <functional>
#include <sys/time.h>
using namespace VSENSOR;
using namespace std;
int main()
{
	//实例化设备
    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);
        return status;
    }
    //设备参数初始化，仅上电时需要进行此操作，部分型号设备初始化需要等待15s
    printf("waiting for parameter init...\n");
    status =pVSensor->DeviceParameterInit();
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("parameter init failed\n");
        return status;    
    }
    //设置为采集模式
    status = pVSensor->SetCaptureMode(emVSensorCaptureMode::MODE_CAPTURE);
    if (status != CAMERA_STATUS_SUCCESS)
    {      
		printf("Set capture mode failed\n");
        return status;
    }
    //单次采集测试
    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);
        return status;
    }
    //保存点云为pcd格式
    status = pVSensor->Save3DCloud(".//test.pcd", presult, emVSensorPointType::POINT_TYPR_DISORDER);
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("save 3D points failed, status = %d\n", status);
        return status;
    }
    //保存灰度图
    status =pVSensor->SaveGrayMap(".//gray.bmp", presult, emVSensorGrayType::GRAY_TYPR_UNCORRECT);
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("save gray image failed, status = %d\n", status);
        return status;
    }
    //保存深度图
    status =pVSensor->SaveDepthMap(".//depth.png", presult);
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("save depth image failed, status = %d\n", status);
        return status;
    }
    //连续采集
    printf("start continuous capture...\n");
    pVSensor->ContinuousRestructionStart(emVSensorCaptureOutputMode::OUTPUT_MODE_ALL);
    for (int i = 0; i < 5; i++)
    {
        pVSensor->CaptureFrame(presult, 8000);
        printf("%d   num =  %d\n", i + 1, presult->nPointNum);
        std::string str(".//test" + std::to_string(i) + ".pcd");
        status = pVSensor->Save3DCloud(str, presult, emVSensorPointType::POINT_TYPR_DISORDER);
		if (status != CAMERA_STATUS_SUCCESS)
		{
			printf("save 3D points failed, status = %d\n", status);
            return status;
		}
    }
    pVSensor->ContinuousRestructionStop();
	//反初始化设备
    status = pVSensor->DeviceUnInit();
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("device release failed\n");
        return status;
    }
    delete presult;
	delete pVSensor;
    return 0;
}
