#include "VSensorLaser.h"
#include <unistd.h>
#include <functional>
#include <sys/time.h>
using namespace VSENSORLASER;
using namespace std;
int main()
{
	//实例化设备
    VSensorLaser* pVSensor = new VSensorLaser();
    //获取sdk版本
    string version = string();
    pVSensor->GetSdkVersionString(version);
    printf("sdk version: %s\n", version.c_str());
    //获取设备列表
	VSensorLaserCameraInfo 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;
    }
    //设置投影等级
    status = pVSensor->SetProjectLight(1);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("set project light failed, status = %d\n", status);
        return status;
    }
    //设置曝光时间
    status = pVSensor->SetExposureTime(2.0);
    if (status != CAMERA_STATUS_SUCCESS)
    {
        printf("set exposure time failed, status = %d\n", status);
        return status;
    }
    //设置为采集模式
    status = pVSensor->SetCaptureMode(emVSensorLaserCaptureMode::MODE_SOFT);
    if (status != CAMERA_STATUS_SUCCESS)
    {      
		printf("Set capture mode failed, status = %d\n", status);
        return status;
    }
    //单次采集测试
    printf("start single capture...\n");
    VSensorLaserResult *presult = new VSensorLaserResult;
    status = pVSensor->SingleRestruction(presult, emVSensorLaserCaptureOutputMode::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, emVSensorLaserPointType::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);
    if (status != CAMERA_STATUS_SUCCESS)
    {
		printf("save gray image failed, status = %d\n", status);
        return status;
    }
    //连续采集
    printf("start continuous capture...\n");
    pVSensor->ContinuousRestructionStart(emVSensorLaserCaptureOutputMode::OUTPUT_MODE_ALL);
    for (int i = 0; i < 5; i++)
    {
        pVSensor->CaptureFrame(presult, 2000);
        printf("%d   num =  %d\n", i + 1, presult->nPointNum);
        std::string str(".//test" + std::to_string(i) + ".pcd");
        status = pVSensor->Save3DCloud(str, presult, emVSensorLaserPointType::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;
}
