#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';
QMap<QString, DcHandle> handleMap ;
#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;
	DcHandle* handlePtr = new DcHandle();
	LX_STATE lx_state = DcOpenDevice(LX_OPEN_MODE::OPEN_BY_SN, m_handle.c_str(), handlePtr, &device_info);
	for(int i=0;i<10;i++){
		if (LX_SUCCESS == lx_state) {
			break;
		}
		qDebug() << "TofCamera Open Failed重启次数="<<i<<"相机id："<< m_handle.c_str();;
		lx_state = DcOpenDevice(LX_OPEN_MODE::OPEN_BY_SN, m_handle.c_str(), handlePtr, &device_info);
	}
	handle = *handlePtr;
	qInfo() << "openCamera3D handle:" << handle << "cameraSN:" << m_handle.c_str();
	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();

	LX_STATE ret;
	int count = 5;
	int i = 0;
	for (; i <= count; i++)
	{
		//更新数据
		ret = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);
		if (LX_SUCCESS == ret)
		{
			break;
		}

		if (LX_E_RECONNECTING == ret) {
			std::cout << "device reconnecting" << std::endl;
		}
		std::this_thread::sleep_for(std::chrono::seconds(1));
		if (i == 0)
		{
			qDebug() << "TofCamera Open Failed! DcSetCmd = " << QString::number(ret);
		}

		if (i != count)
		{
			qDebug() << "TofCamera Reopen " << QString::number(i + 1);
		}
		else
		{
			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)
{

	qInfo() << "getPicCamera3D handle:" << handle << "cameraSN:" << m_handle.c_str();
	//更新数据
	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");
}