#include "LXCamera.h"

#include <iostream>
#include <thread>
#include <sstream>
#include<filesystem>
#include "opencv2/opencv.hpp"
#include "lxCamera/lx_camera_api.h"

#pragma comment(lib,"LxCameraApi.lib")


static char wait_key = '0';
static DcHandle handle = 0;
#define checkTC(state) {LX_STATE val=state;                            \
    if(val != LX_SUCCESS){                                             \
        if(val == LX_E_RECONNECTING){                                  \
			qDebug()<< " device reconnecting";}                        \
        else if(val == LX_E_NOT_SUPPORT){                              \
            qDebug()<< " not support";}                                \
        else{                                                          \
            qDebug()<< DcGetErrorString(val);                          \
            DcCloseDevice(handle);                                     \
        }                                                              \
    }                                                                  \
}


LXCamera::~LXCamera()
{
	destroyCamera();
}

bool LXCamera::initCamera(const QString& serialNumber)
{
	int device_num = 0;
	LxDeviceInfo* p_device_list = NULL;	
	checkTC(DcGetDeviceList(&p_device_list, &device_num));
	if (device_num <= 0)
	{
		qDebug() << "Not found any tof device.";
		return false;
	}
	m_handle = serialNumber.toStdString();
	qInfo() << "Find tof device number = " << device_num;
	return true;
}

void LXCamera::destroyCamera()
{
	stopCamera();
	closeCamera();
}

bool LXCamera::openCamera()
{
	LxDeviceInfo device_info;
	LX_STATE lx_state = DcOpenDevice(LX_OPEN_MODE::OPEN_BY_SN, m_handle.c_str(), &handle, &device_info);
	if (LX_SUCCESS != lx_state) {
		QString str = QString("open device failed, open_param: %1, err = %2").arg(m_handle.c_str()).arg(lx_state);
		qDebug() << str;
		return false;
	}
	return true;
}

bool LXCamera::closeCamera()
{
	return DcCloseDevice(handle);
}

bool LXCamera::startCamera()
{
	//设置数据流
	bool test_depth = false, test_rgb = false;
	checkTC(DcGetBoolValue(handle, LX_BOOL_ENABLE_3D_DEPTH_STREAM, &test_depth));
	checkTC(DcGetBoolValue(handle, LX_BOOL_ENABLE_2D_STREAM, &test_rgb));

	//开启数据流
	checkTC(DcStartStream(handle));

	std::thread pthread = std::thread();
	//pthread.detach();

	//auto _time = std::chrono::system_clock::now();

	//更新数据
	auto ret = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);

	if (LX_SUCCESS != ret)
	{
		if (LX_E_RECONNECTING == ret) {
			std::cout << "device reconnecting" << std::endl;
		}
		std::this_thread::sleep_for(std::chrono::seconds(1));
		qDebug() << "TofCamera Open Failed!";
		return false;
	}

	qDebug() << "TofCamera Open Succeed!";
	return true;
}

bool LXCamera::stopCamera()
{
	return DcStopStream(handle);
}

bool LXCamera::setTriggerSource(int mode)
{
	return true;
}

bool LXCamera::setExposure(int exposure)
{
	return true;
}

bool LXCamera::setGain(double gain)
{
	return true;
}

bool LXCamera::takeAPic(cv::Mat& imgMat, cv::Mat&imgMat3D)
{
	//更新数据
	auto ret = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);
	if (LX_SUCCESS != ret)
	{
		if (LX_E_RECONNECTING == ret) {
			qDebug() << "device reconnecting";
		}
		std::this_thread::sleep_for(std::chrono::seconds(1));
		//continue;
	}

	FrameInfo* frame_ptr = nullptr;
	checkTC(DcGetPtrValue(handle, LX_PTR_FRAME_DATA, (void**)&frame_ptr));
	if (!frame_ptr || frame_ptr->frame_state != LX_SUCCESS) {
		qDebug() << "new frame not correct";
		return false;
	}

	//Depth
	if (frame_ptr->depth_data.frame_data != nullptr)
	{
		auto depth_data = frame_ptr->depth_data;
		cv::Mat depth_image = cv::Mat(depth_data.frame_height, depth_data.frame_width,
			CV_MAKETYPE(depth_data.frame_data_type, depth_data.frame_channel), depth_data.frame_data);
		
		depth_image.convertTo(imgMat, CV_8U, 1.0 / 16);
		cv::applyColorMap(imgMat, imgMat3D, cv::COLORMAP_JET);
	}

	//PointCloud
	auto retPCL = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);
	retPCL = DcSaveXYZ(handle, ".\\Cache\\1.pcd");
}