#include <string>
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>

#include "LibapiProcessThread.h"
#include "LibapiQueue.h"
#include "LibapiQueues.h"
#include "Libapi.h"

#include "IniHelper.h"
#include "Yolov5For928.h"

#include "PlguinConfig.h"
#include "MsgBase.h"
#include "Libapi2D3D.h"
#include "Log.h"
#include "CameraHelper.h"

// 标式系统是否已经启动
static bool bSysIsInit = false;

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_init_plugin(int idx, void* pthread)
{
	return 0;
}

// for test
static void init1()
{
	int idx = 0;
	//PlguinEnhance* _plguinEnhance = new PlguinEnhance();
	//LibapiProcessThread* _thread_plguinEnhance =
	//	new LibapiProcessThread(idx, "_plguinEnhance", (BaseRunnable*)_plguinEnhance);
	//idx++;

	//PlguinRoi* _plguinRoi = new PlguinRoi();
	//LibapiProcessThread* _thread_plguinRoi =
	//	new LibapiProcessThread(idx, "_plguinRoi", (BaseRunnable*)_plguinRoi);
	//idx++;

	//PlguinMotionDetection* _plguinMotionDetection = new PlguinMotionDetection();
	//LibapiProcessThread* _thread_plguinMotionDetection =
	//	new LibapiProcessThread(idx, "_plguinMotionDetection", (BaseRunnable*)_plguinMotionDetection);
	//idx++;

	//PlguinTrack* _plguinTrack = new PlguinTrack();
	//LibapiProcessThread* _thread_plguinTrack =
	//	new LibapiProcessThread(idx, "_plguinTrack", (BaseRunnable*)_plguinTrack);
	//idx++;

	//PlguinClassification* _plguinClassification = new PlguinClassification();
	//LibapiProcessThread* _thread_plguinClassification =
	//	new LibapiProcessThread(idx, "_plguinClassification", (BaseRunnable*)_plguinClassification);
	//idx++;

	//PlguinPostgressing* _plguinPostgressing = new PlguinPostgressing();
	//LibapiProcessThread* _thread_plguinPostgressing =
	//	new LibapiProcessThread(idx, "_plguinPostgressing", (BaseRunnable*)_plguinPostgressing);
	//idx++;

	//_thread_plguinEnhance->set_next_thread(_thread_plguinRoi);
	//_thread_plguinRoi->set_next_thread(_thread_plguinMotionDetection);
	//_thread_plguinMotionDetection->add_sub_thread(_thread_plguinTrack);
	//_thread_plguinMotionDetection->add_sub_thread(_thread_plguinClassification);
	//_thread_plguinMotionDetection->set_next_thread(_thread_plguinPostgressing);

	//_thread_plguinEnhance->start();
	//_thread_plguinRoi->start();
	//_thread_plguinMotionDetection->start();
	//_thread_plguinTrack->start();
	//_thread_plguinClassification->start();
	//_thread_plguinPostgressing->start();

	////_thread_plguinEnhance->join();
	////_thread_plguinRoi->join();
	////_thread_plguinMotionDetection->join();
	////_thread_plguinTrack->join();
	////_thread_plguinClassification->join();
	////_thread_plguinPostgressing->join();

	////pthread = _thread_plguinEnhance;
}


extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_init_plugin_config(void* config, void* pthread)
{
	return PlguinConfig::getInstance()->plguin_init_plugin_config(config, pthread);
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_create_MsgBase(char* msg_type, void** pMsg)
{
	if ((*pMsg) != NULL)
	{
		std::cout << "pMsg is not null" << std::endl;
		assert(0);
	}

	MsgBase* _pMsg = new MsgBase();
	//memcpy_s(_pMsg->version, sizeof(msg_type), msg_type, sizeof(msg_type));
	memcpy(_pMsg->version, msg_type, sizeof(msg_type));
	(*pMsg) = _pMsg;
	return 0;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_delete_MsgBase(void* pMsg)
{
	MsgBase::delete_msg((MsgBase*)pMsg);
	return 0;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_copy_image_to_msg(
	void* pMsg, int pos, int param_type, void* image_data, int width, int height, int flag)
{
	return MsgBase::copy_image_to_msg(
		pMsg, pos, param_type, image_data, width, height, flag);
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_copy(void* tar, void* src, int len)
{
	//memcpy_s(tar, len, src, len);
	memcpy(tar, src, len);
	return 0;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_bind_queue(int id, char* queue_name, int src_type)
{
	std::string qname(queue_name);
	LibapiQueue<void*>* queue = Queues::create_queue(id, qname);
	if (queue == NULL) return -1;
	return 0;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_ubbind_queue(char* queue_name)
{
	std::string qname(queue_name);
	LibapiQueue<void*>* queue = Queues::find_queue(qname);
	if (queue == NULL) return -1;
	Queues::delete_queue(queue);
	return 0;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_push_to_queue(char* queue_name, void* msg)
{
	((MsgBase*)msg)->add_ref();
	int ret = Queues::push_to_queue(queue_name, msg);
	if (ret != 0)
	{
		int ref = ((MsgBase*)msg)->sub_ref();
		if (ref == 0)
			((MsgBase*)msg)->delete_msg();
	}
	return ret;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD libapi_pop_from_queue(char* queue_name, void** msg)
{
	std::string qname(queue_name);
	LibapiQueue<void*>* queue = Queues::find_queue(qname);
	if (queue == NULL) return -1;
	queue->pop(*msg);
	return 0;
}

// 初始化
extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapiInit(const long id, const char* info, void (*callback)(void*))
{
	int ret = SYS_OK;

	// init config
	GINIT();

	// init yolo for 928 待优化
	ret = CYolov5For928::Get()->Init();
	if (ret != SYS_OK)
		return SYS_NPU_IS_NOT_INIT;

	//ret = CCameraHelper::Get()->Init();
	//if (ret != SYS_OK)
	//{
	//	return SYS_CAMERA_IS_NOT_INIT;
	//}

	// 初始化CMeasureInfo
	CMeasureInfo::Init(id, info);

	// 初始化回调函数
	MsgBase::SetCallBack(callback);

	// init task
	PlguinConfig::getInstance()->LibapiInitCameraProcessing(nullptr);
	PlguinConfig::getInstance()->LibapiInitLidarProcessing(nullptr);
	PlguinConfig::getInstance()->LibapiInitRoiProcessing(nullptr);
	LibapiThread::delay_second(2);

	// 标识系统已经启动
	bSysIsInit = true; 

	return ret;
}

// 启动定位角点
extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapiStartDetection()
{
	int ret = SYS_OK;

	if (!bSysIsInit)
		return SYS_IS_NOT_INIT;

	MsgBase* pMsgCamera = nullptr;
	MsgBase* pMsgLidar = nullptr;
	char msg_type[32] = "v1.0";

	libapi_create_MsgBase(msg_type, (void**)&pMsgCamera);
	libapi_create_MsgBase(msg_type, (void**)&pMsgLidar);
	pMsgCamera->mMsgType = "StartDetection";
	pMsgLidar->mMsgType = "StartDetection";

	ret = libapi_push_to_queue((char*)"_t_PluginCameraPro", pMsgCamera);
	ret = libapi_push_to_queue((char*)"_t_PlguinLidarPro", pMsgLidar);

	// 待优化
	LibapiThread::delay_second(1);

	return ret;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapiContinuetDetection()
{
	int ret = SYS_OK;

	if (!bSysIsInit)
		return SYS_IS_NOT_INIT;

	MsgBase::SetWaitInt(MsgBase::iClinetMsg, CLIENT_MSG_CONTINUE);

	return ret;
}

// 重新定位角点
extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapRestartConnerDetection()
{
	int ret = SYS_OK;

	if (!bSysIsInit)
		return SYS_IS_NOT_INIT;

	MsgBase::SetWaitInt(MsgBase::iClinetMsg, CLIENT_MSG_RESTART_DETECTION);

	return ret;
}

// 终止定位角点
extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapStopDetection()
{
	int ret = SYS_OK;

	if (!bSysIsInit)
		return SYS_IS_NOT_INIT;

	MsgBase::SetWaitInt(MsgBase::iClinetMsg, CLIENT_MSG_STOP_DETECTION);
	return ret;
}

// 获取结果
extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapGetRecord(void* p)
{
	int ret = SYS_OK;

	CallbackInfo* cbInfo = (CallbackInfo*)p;
	CMeasureInfo* measureInfo = CMeasureInfo::Get();

	if (cbInfo == nullptr || measureInfo == nullptr)
		assert(0);

	cbInfo->code = 0;
	//cbInfo->errorInfo = "";
	cbInfo->bGetData = true;
	std::vector<std::shared_ptr<CSubRoi>> rois = measureInfo->GetSubRois();
	cbInfo->roi_size = rois.size();
	for (int i = 0; i < rois.size(); ++i)
	{
		std::shared_ptr<CSubRoi> roi = rois[i];
		SubRoiData* cb_roi = &cbInfo->subRois[i];

		for (int n = 0; n < (roi.get()->H).total(); ++n)
			cb_roi->H[n] = (roi.get()->H).at<float>(0, n);
		for (int n = 0; n < roi.get()->mRoiPoints.size(); ++n)
		{
			cv::Point2f p2f = roi.get()->mRoiPoints[n];
			cb_roi->mRoiPoints[n * 2] = p2f.x;
			cb_roi->mRoiPoints[n * 2 + 1] = p2f.y;
		}
		for (int n = 0; n < roi.get()->mInnerConners.size(); ++n)
		{
			cv::Point2f p2f = roi.get()->mInnerConners[n];
			cb_roi->mInnerConners[n * 2] = p2f.x;
			cb_roi->mInnerConners[n * 2 + 1] = p2f.y;
		}
		for (int n = 0; n < roi.get()->mInnerConners3D.size(); ++n)
		{
			cv::Point3f p3f = roi.get()->mInnerConners3D[n];
			cb_roi->mInnerConners3D[n * 3] = p3f.x;
			cb_roi->mInnerConners3D[n * 3 + 1] = p3f.y;
			cb_roi->mInnerConners3D[n * 3 + 2] = p3f.z;
		}
		//for (int n = 0; n < roi.get()->mInnerConners3D.size(); ++n)
		//{
		//}

	}
	return ret;
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapiCameraSendMsgWithImage(
	void* image_data, int width, int height, int flag, char* type, char* queue_name)
{
	int ret = SYS_OK;
	MsgBase* _pMsg = new MsgBase();
	
	_pMsg->mMsgType = type;
	ret = MsgBase::copy_image_to_msg(_pMsg, PARAM_POS_SRC_IMAGE, 0, image_data, width, height, flag);
	if (ret != SYS_OK)
		return ret;

	return libapi_push_to_queue(queue_name, _pMsg);
}

extern "C" LIBAPI_EXPORT int LIBAPI_CALLMETHOD LibapiGetHQImage(
	void* image_data, int width, int height, int flag, char* type)
{
	int ret = SYS_OK;
	std::shared_ptr<cv::Mat> ptr = CMeasureInfo::Get()->GetIm();
	if (ptr != nullptr)
	{
		cv::Mat* imPtr = ptr.get();
		if (imPtr != nullptr && width == imPtr->cols && height == imPtr->rows)
		{
			memcpy(image_data, imPtr->data, imPtr->total() * 3);
			std::cout << "像素数量 ： " << imPtr->total() * 3 << std::endl;
		}
		else
		{
			std::cout << "cv::Mat* imPtr is None" << std::endl;
			std::cout << imPtr << std::endl;
			std::cout << width <<  " : " << imPtr->cols << std::endl;
			std::cout << height << " : " << imPtr->rows << std::endl;
			return SYS_ERROR;
		}
	}
	else
	{
		std::cout << "ptr is None width" << std::endl;
		return SYS_ERROR;
	}
	
	return ret;
}