#include "PluginLidarProcessing.h"
#include "MsgBase.h"
#include "Libapi3d.h"
#include "IniHelper.h"
#include "Log.h"


PluginLidarProcessing::PluginLidarProcessing() {


};

PluginLidarProcessing::~PluginLidarProcessing()
{

};

static int CapCloudPointsAndPretreatment(
	std::shared_ptr<geometry::PointCloud>& cloud_ptr,
	std::shared_ptr<geometry::PointCloud>& cloud_ptr_by_dist, 
	std::shared_ptr<geometry::PointCloud>& cloud_ptr_by_correct)
{
	int ret = CAL_OK;

	ret = Cal3D::LibapiCapCloudPoints(cloud_ptr);
	if (ret != CAL_OK)
	{
		return ret;
		//assert(0);
	}
		
	ret = Cal3D::LibapiRotAndTrans(cloud_ptr);
	if (ret != CAL_OK)
	{
		return ret;
		//assert(0);
	}

	ret = Cal3D::LibapiFilterCloudPointsByDist(cloud_ptr, cloud_ptr_by_dist, -2.5, 2.5, -2.5, 2.5, 0, 3.8);
	if (ret != CAL_OK)
	{
		return ret;
		//assert(0);
	}

	if (cloud_ptr_by_dist->IsEmpty())
	{
		return ret;
		//assert(0);
	}

	ret = Cal3D::LibapiCorrectCloudPoint(cloud_ptr_by_dist, cloud_ptr_by_correct, 0);
	if (ret != CAL_OK)
	{
		return ret;
		//assert(0);
	}

	return ret;
}

int PluginLidarProcessing::OnProcess(LibapiThread* pThisThread, void* pMsg)
{
	int ret = 0;
	bool bCircle = true;

	//utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);
	while(bCircle)
	{
		auto cloud_ptr = std::make_shared<geometry::PointCloud>();
		auto cloud_ptr_by_dist = std::make_shared<geometry::PointCloud>();
		auto cloud_ptr_by_correct = std::make_shared<geometry::PointCloud>();

		ret = CapCloudPointsAndPretreatment(cloud_ptr, cloud_ptr_by_dist, cloud_ptr_by_correct);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "[ERROR] CapCloudPointsAndPretreatment is error!");
			DoCallback(ret);
		}
		Loger(LEVEL_INFOR, "CapCloudPointsAndPretreatment is success!");

		std::vector<cv::Point3d> vec_point_3d;
		ret = Cal3D::LibapiCloudPointToVec(cloud_ptr_by_correct, vec_point_3d);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "[ERROR] LibapiCloudPointToVec is error!");
			DoCallback(ret);
		}
		Loger(LEVEL_INFOR, "LibapiCloudPointToVec is success!");

		// 等待相机线程发来的消息
		int msg_rst = MsgBase::WaitInt(MsgBase::iCameraToLidarMsg);
		switch (msg_rst)
		{
		case CLIENT_MSG_NONE:
			assert(0);
		case CLIENT_MSG_CONTINUE:
			bCircle = false;
			break;
		case CLIENT_MSG_RESTART_DETECTION:
			continue;
		case CLIENT_MSG_STOP_DETECTION:
			return ret;
		default:
			assert(0);
		}

		if (cloud_ptr.get()->IsEmpty())
		{
			DoCallback(CAL_PRAMA_EMPUTY);
			MsgBase::SetWaitBool(MsgBase::bCloudProjectIsOk);
			Loger(LEVEL_INFOR, "[ERROR] cloud_ptr.get()->IsEmpty()");
			return CAL_PRAMA_EMPUTY;
		}
	
		cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << G(fx), 0, G(cx), 0, G(fy), G(cy), 0, 0, 1);
		std::vector<cv::Point2d> vec_point_2d;
		//cv::Mat test_src_im = cv::imread("output.png");
		cv::Mat test_src_im;

		//生成点云2D-3D数据
		std::shared_ptr<cv::Mat> im_2D3D = std::make_shared<cv::Mat>(cv::Size(9344, 7000), CV_32FC3);
		im_2D3D->setTo(cv::Scalar(0, 0, 0)); // 设置为全黑背景
		//ret = Cal3D::LibapiCloudPointsPorject(vec_point_3d, cameraMatrix, vec_point_2d, im_2D3D.get(), false, test_src_im);
		CMeasureInfo* measureInfo = CMeasureInfo::Get();
		int roiSize = measureInfo->GetSubRois().size();
		std::vector<std::vector<cv::Point2f>> vecInConners2D(roiSize);
		std::vector<std::vector<cv::Point3f>> vecInConners3D(roiSize);
	
		std::vector <std::vector<cv::Point2f>> vecConners = measureInfo->GetAllRoiConners();

		ret = Cal3D::LibapiCloudPointsPorject(vec_point_3d, cameraMatrix, vecConners, im_2D3D.get(), vecInConners2D, vecInConners3D);
		if (ret != CAL_OK)
		{
			DoCallback(ret);
			std::shared_ptr<cv::Mat> empty = nullptr;
			measureInfo->SetIm2D3D(empty);
			MsgBase::SetWaitBool(MsgBase::bCloudProjectIsOk);
			Loger(LEVEL_INFOR, "[ERROR] LibapiCloudPointsPorject is error");
			return ret;
			//assert(0);
		}
		Loger(LEVEL_INFOR, "LibapiCloudPointsPorject is success!");
		measureInfo->SetIm2D3D(im_2D3D);
		
		std::vector<std::shared_ptr<CSubRoi>> rois = measureInfo->GetSubRois();
		for (int i = 0; i < rois.size(); ++i)
		{
			CSubRoi* roi = rois[i].get();
			roi->mVc2D.resize(vecInConners3D[i].size());
			roi->mVc3D.resize(vecInConners3D[i].size());
			std::copy_n(vecInConners3D[i].begin(), vecInConners3D[i].size(), roi->mVc3D.begin());
			std::copy_n(vecInConners2D[i].begin(), vecInConners2D[i].size(), roi->mVc2D.begin());
		}
	
		MsgBase::SetWaitBool(MsgBase::bCloudProjectIsOk);

		// 查看点云的投影(用于测试)
		//ret = Cal3D::LibapiCloudPointProjectTest(*im_2D3D);

		// 创建CMeasureInfo数据结构
		//long id;
		//ret = CalUtils::LibapiGenRandomId(id);
		//std::string info = "abc";
		//CMeasureInfo* measureInfoPtr = CMeasureInfo::Init(id, info);
		//measureInfoPtr->SetIm2D3D(im_2D3D);
		//MsgBase::set_spointer_to_msg<CMeasureInfo>((LibapiMsg*)pCameraMsg, PARAM_POS_LIDAR_2D3D, CMeasureInfo_ptr);

		//visualization::DrawGeometries({ cloud_ptr_by_correct }, "TestFileFormat", 1920, 1080);
	}	

	return ret;
}

void PluginLidarProcessing::OnEndProcess(LibapiThread* pThisThread, void* pMsg)
{
	MACRO_RECORD_RUN_TIME(pThisThread->get_name());

	std::cout << " PluginLidarProcessing is end! " << std::endl;
}