#include "Libapi2d.h"
#include "Libapi3d.h"
#include "IniHelper.h"
#include "Yolov5For928.h"
#include "Log.h"
#include "CameraHelper.h"

#include <vector>
#include <cmath>
#include <algorithm>
#include <atomic>

static std::atomic_flag atomic_lock = ATOMIC_FLAG_INIT;

static cv::Mat Sobel(cv::Mat& mat, double alpha, double beta)
{
	cv::Mat tmpImage;
	cv::Mat tmpImageX;
	cv::Mat tmpImageY;

	if (mat.type() != CV_8UC1)
		cvtColor(mat, tmpImage, cv::COLOR_BGR2GRAY);
	else
		tmpImage = mat;

	cv::Sobel(tmpImage, tmpImageX, CV_64F, 1, 0, 1);
	cv::Sobel(tmpImage, tmpImageY, CV_64F, 0, 1, 1);

	cv::convertScaleAbs(tmpImageX, tmpImageX);
	cv::convertScaleAbs(tmpImageY, tmpImageY);

	 cv::bitwise_or(tmpImageX, tmpImageY, tmpImage);
	//cv::addWeighted(tmpImageX, alpha, tmpImageY, beta, 0, tmpImage);
	return tmpImage;
}


static cv::Mat CopyToEx(cv::Mat& src, cv::Mat& mask, cv::Mat& tar, int x_offset, int y_offset)
{
	cv::Mat dst;
	tar.copyTo(dst);
	cv::Rect roi_rect = cv::Rect(x_offset, y_offset, src.cols, src.rows);
	src.copyTo(dst(roi_rect), mask);
	return dst;
}


static cv::Rect OutSquare(const std::vector<cv::Point>& vp)
{
	int x1 = 9999;
	int y1 = 9999;
	int x2 = 0;
	int y2 = 0;
	for (int i = 0; i < vp.size(); i++)
	{
		if (vp[i].x < x1)
			x1 = vp[i].x;
		if (vp[i].x > x2)
			x2 = vp[i].x;
		if (vp[i].y < y1)
			y1 = vp[i].y;
		if (vp[i].y > y2)
			y2 = vp[i].y;
	}

	return cv::Rect(x1, y1, x2 - x1, y2 - y1);
}


static std::vector<cv::Point> Offset(const std::vector<cv::Point>& vp, int x, int y)
{
	std::vector<cv::Point> vpTar;

	for (int i = 0; i < vp.size(); i++)
	{
		cv::Point p(vp[i].x + x, vp[i].y + y);
		vpTar.push_back(p);
	}

	return vpTar;
}


static cv::Mat GetRoi(cv::Mat& mat, const std::vector<cv::Point>& vp)
{
	cv::Mat dst;
	cv::Rect rt = OutSquare(vp);
	cv::Mat src = mat(rt);
	cv::Mat roi = cv::Mat::zeros(src.size(), CV_8U);

	std::vector<std::vector<cv::Point>> contour;
	std::vector<cv::Point> vpOffset = Offset(vp, -rt.x, -rt.y);
	//contour.push_back(vpOffset);
	//cv::drawContours(roi, contour, 0, cv::Scalar::all(255), -1);
	src.copyTo(dst, roi);

	return dst;
}


static std::vector<std::vector<float>> SortLines(std::vector<std::vector<float>>& lines4)
{
	float Ax = 1, Bx = 0, Cx = 0;
	float Ay = 0, By = 1, Cy = 0;
	float min_dx = 10000, max_dx = 0, min_dy = 10000, max_dy = 0;
	float min_dx_idx = -1, max_dx_idx = -1, min_dy_idx = -1, max_dy_idx = -1;
	float cx = 0.0, cy = 0.0;
	float dx = 0.0, dy = 0.0;

	std::vector<std::vector<float>> sort_lines4(4); // 上、右、下、左四条线
	for (int i = 0; i < sort_lines4.size(); ++i) {
		sort_lines4[i].resize(6); // x1, y1, x2, y2, theta, r
	}
	
	for (int i = 0; i < lines4.size(); ++i)
	{
		cx = (lines4[i][2] + lines4[i][0]) / 2, cy =  (lines4[i][3] + lines4[i][1]) / 2;
		dx = abs(Ay * cx + By * cy + Cy) / sqrt(pow(Ay, 2) + pow(By, 2));
		dy = abs(Ax * cx + Bx * cy + Cx) / sqrt(pow(Ax, 2) + pow(Bx, 2));
		if (dx < min_dx)
		{
			min_dx = dx;
			min_dx_idx = i;
		}
		if (dx > max_dx)
		{
			max_dx = dx;
			max_dx_idx = i;
		}
		if (dy < min_dy)
		{
			min_dy = dy;
			min_dy_idx = i;
		}
		if (dy > max_dy)
		{
			max_dy = dy;
			max_dy_idx = i;
		}
	}
	sort_lines4[0] = lines4[min_dx_idx];
	sort_lines4[1] = lines4[max_dy_idx];
	sort_lines4[2] = lines4[max_dx_idx];
	sort_lines4[3] = lines4[min_dy_idx];
	return sort_lines4;
}


static std::vector<std::vector<float>> GetIntersections(cv::Mat& im,	std::vector<std::vector<float>>& sort_lines4)
{
	float x1, y1, x2, y2;
	float x1_next, y1_next, x2_next, y2_next;
	float m, m_next;
	float A, B, C, A_next, B_next, C_next;
	float x_p, y_p;

	std::vector<std::vector<float>> sort_conners4(4);

	for (int i = 0; i < sort_conners4.size(); ++i) {
		sort_conners4[i].resize(2); // x1, y1, x2, y2, theta, r
	}
	
	for (int i = 0; i < 4; ++i)
	{
		std::vector<float> line = sort_lines4[i];
		x1 = line[0], y1 = line[1], x2 = line[2], y2 = line[3];
		std::vector<float> line_next = sort_lines4[(i + 1) % 4];
		x1_next = line_next[0], y1_next = line_next[1], x2_next = line_next[2], y2_next = line_next[3];

		// 检查是否为垂直线
		if (x2 == x1) 
			x2 += 0.001f;
		m = (y2 - y1) / (x2 - x1);
		A = m, B = -1, C = y1 - m * x1;

		if (x2_next == x1_next) 
			x2_next += 0.001f;
		m_next = (y2_next - y1_next) / (x2_next - x1_next);
		A_next = m_next, B_next = -1, C_next = y1_next - m_next * x1_next;

		// 检查是否平行(理论上不会)
		 if (A * B_next == A_next * B)
			 continue;

		// 计算交点
		x_p = (B_next * C - B * C_next) / (A_next * B - A * B_next);
		y_p = (A * x_p + C) / -B;
		
		//  确保交点在图像范围内
		if (0 <= int(x_p) && int(x_p) <= im.cols && 0 <= int(y_p) && int(y_p) < im.rows)
			sort_conners4[(i + 1) % 4] = { x_p, y_p };
	}

	return sort_conners4;
}


static int GetSortConnersAndEdgess(
	cv::Mat& src, 
	std::vector<std::vector<float>>& sort_lines4,
	std::vector<std::vector<float>>& sort_conners4,
	int thresh_hold = 80,
	int minLineLength = 400)
{
	int ret = CAL_OK;

	if (G(conners_detect_fake) == "true")
	{
		sort_conners4.push_back({ 465,436 });
		sort_conners4.push_back({ 615, 260 });
		sort_conners4.push_back({ 783, 405 });
		sort_conners4.push_back({ 630, 581 });
		for (int i = 0; i < sort_conners4.size(); ++i)
			sort_conners4[i] = {
			float(sort_conners4[i][0] * 9344 / 934 / 1.5),
			float(sort_conners4[i][1] * 7000 / 700 / 1.5) };
		return ret;
	}

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	cv::Mat gray;
	cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
	cv::GaussianBlur(gray, gray, cv::Size(5, 5), 0, 0);
	gray = Sobel(gray, 0.0, 0.0);
	cv::Mat mask;
	cv::threshold(gray, mask, 5, 255, cv::THRESH_BINARY);
	
	float PI = 3.1415926f;
	std::vector<cv::Vec4f> lines;
	cv::HoughLinesP(mask, lines, 1, PI / 180, thresh_hold, minLineLength, G(edge_max_line_gap));

	float x1, y1, x2, y2, theta, r, m;
	float A, B, C;
	bool bRepeat = false;
	float cx, cy, d, r_max;
	std::vector<std::vector<float>> _line4; // 上、右、下、左四条线
	//for (int i = 0; i < _line4.size(); ++i) {
	//	_line4[i].resize(6); // x1, y1, x2, y2, theta, r
	//}
	for (int l = 0; l < lines.size(); ++l)
	{
		cv::Vec4f _line = lines[l];
		x1 = _line[0];
		y1 = _line[1];
		x2 = _line[2];
		y2 = _line[3];

		// 计算极坐标
		if (x1 == x2) 
			x2 += 0.001f;
		theta = atan((y2 - y1) / (x2 - x1)) * (180 / PI);
		r = sqrt(pow((y2 - y1), 2) + pow((x2 - x1), 2));
		
		// 计算直线的斜率(如果 1 ≠ 2),待优化
		m = (y2 - y1) / (x2 - x1);
		A = m;
		B = -1;
		C = y1 - m * x1;

		// 查找符合条件的直线边缘
		bRepeat = false;
		for (int i = 0; i < _line4.size(); ++i)
		{
			if (abs(abs(theta) - abs(_line4[i][4])) < G(edge_min_angle_th)) // 33°
			{
				cx = (_line4[i][2] + _line4[i][0]) / 2;
				cy = (_line4[i][3] + _line4[i][1]) / 2;

				// 中点到目标线的垂线小于系数，选择 r 较长的作为边缘
				d = abs(A * cx + B * cy + C) / sqrt(pow(A, 2) + pow(B, 2));
				r_max = _line4[i][5] > r ? _line4[i][5] : r;
				if (d < r_max / 3)
				{
					bRepeat = true;
					if (r > _line4[i][5])
					{
						std::vector<float> v;
						v.resize(6);
						v = { x1, y1, x2, y2, theta, r };
						_line4[i] = v;
					}
				}
			}
		}
		if (!bRepeat)
		{
			std::vector<float> v;
			v.resize(6);
			v = { x1, y1, x2, y2, theta, r };
			_line4.push_back(v);
		}
	}

	Loger(LEVEL_INFOR, "_line4.size() < 4 is %d", _line4.size());
	if (_line4.size() < 4)
	{
		return CAL_INNER_COONERS_ISNOT_4;
	}


	sort_lines4 = SortLines(_line4);
	if (sort_lines4.size() != 4)
		assert(0);
	sort_conners4 = GetIntersections(src, sort_lines4);
	if (sort_conners4.size() != 4)
		assert(0);

	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal3D static GetSortConnersAndEdgess time is : " << elapsex.count() << std::endl;
	return ret;
}


static std::vector<std::vector<float>> UpdateConners(std::vector<std::vector<float>>& sort_conners4)
{
	std::vector<std::vector<float>> sort_conners4_new(4);

	return sort_conners4_new;
}

static void delay_second(double second)
{
	clock_t start_time;
	start_time = clock();
	for (; (clock() - start_time) < second * CLOCKS_PER_SEC;);
}


//广角摄像机和高清相机图像映射
int Cal2D::LibapiGet2D2DMapRect(cv::Mat& dst_im, bool fake, std::string fakeFilePath)
{
	int ret = CAL_OK;


	
	return ret;
}

//读取广角相机图像
int Cal2D::LibapiCapLQImage(cv::Mat& dst_im)
{
	int ret = CAL_OK;


	return ret;
}

//采集高清图像
static int camera_idxx = 0;
int Cal2D::LibapiCapHQImage(CMeasureInfo* measureInfoPtr, std::shared_ptr<cv::Mat>& dst_im)
{
	int ret = CAL_OK;
	if (G(camera_cap_fake) == "true")
	{
		if (G(fake_image_fpath) == "")
			assert(0);
		else
		{
			// 读取fake文件，获取点云数据
			ret = LibapiReadHQImage(G(fake_image_fpath), *dst_im);
			if (ret != CAL_OK)
			{
				return ret;
			}
			if (dst_im->empty())
			{
				return CAL_READFILE_FAILED;
			}
		}
	}
	else
	{
		while (atomic_lock.test_and_set(std::memory_order_acquire)) {}
		if (camera_idxx == 0)
		{
			//等待初始化
			ret = CCameraHelper::Get()->Init();
			Loger(LEVEL_INFOR, "CCameraHelper::Get()->Init() ret is %d", ret);
			ret = CCameraHelper::Get()->Start(G(export_time) * 10000);
			Loger(LEVEL_INFOR, "CCameraHelper::Get()->Start() ret is %d", ret);
			camera_idxx = 1;
		}
		// cap camera
		/*CCameraHelper::Get()->Start();*/
		int times = 3;
		while (true)
		{
			void* pData = (void*)(dst_im.get()->data);
			int w, h;
			ret = CCameraHelper::Get()->Cap((void**) & pData, w, h, 5000);
			Loger(LEVEL_INFOR, "Capture Image times : %d", 3 - times +1);
			if (ret == 0)
			{
				Loger(LEVEL_INFOR, "Capture Image sucess!");
				break;
			}
			delay_second(1.5);
			times--;
			if (times == 0)
			{
				Loger(LEVEL_INFOR, "Capture Image faild!");
				//CCameraHelper::Get()->Stop();
				//CCameraHelper::Get()->Init();
				//CCameraHelper::Get()->Start();
				return CAL_2D_CAP_IMAGE_FAILED;
			}
		}
		atomic_lock.clear(std::memory_order_release);
		//CCameraHelper::Get()->Stop();
	}
	measureInfoPtr->SetIm(dst_im);
	return ret;
}

//从文件读取高清图像
int Cal2D::LibapiReadHQImage(const std::string plyPath, cv::Mat& dst_im)
{
	int ret = CAL_OK;

	dst_im = cv::imread(plyPath, cv::IMREAD_UNCHANGED);

	if (dst_im.empty())
	{
		Loger(LEVEL_INFOR, "dst_im.empty() dst_im : %s", plyPath.c_str());
		return CAL_PRAMA_EMPUTY;
		//assert(0);
	}

	Loger(LEVEL_INFOR, "Image read success: %s", plyPath.c_str());
	return ret;
}

//保存高清图像
int Cal2D::LibapiCopyHQImage(cv::Mat& src_im, cv::Mat& dst_im)
{
	int ret = CAL_OK;

	return ret;
}

//图像去畸变
int Cal2D::LibapiUndistort(cv::Mat& src_im, cv::Mat& dst_im)
{
	int ret = CAL_OK;

	cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << G(fx), 0, G(cx), 0, G(fy), G(cy), 0, 0, 1);
	cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << G(k1), G(k2), G(p1), G(p2), G(k3));
	cv::undistort(src_im, dst_im, cameraMatrix, distCoeffs);

	return ret;
}

//持续采集广角图像(PYTHON)
//AI识别广角图像中的预埋件，返回其ROI坐标
//待优化
static int idxx = 0;
int Cal2D::LibapiDetectObj(CMeasureInfo* measureInfo)
{
	int ret = CAL_OK;

	if (G(npu_fake) == "true")
	{
		std::shared_ptr<CSubRoi> subRoi1Ptr = std::make_shared<CSubRoi>();
		// 待修改
		subRoi1Ptr->mRoiPoints.push_back({ 465,436 });
		subRoi1Ptr->mRoiPoints.push_back({ 615, 260 });
		subRoi1Ptr->mRoiPoints.push_back({ 783, 405 });
		subRoi1Ptr->mRoiPoints.push_back({ 630, 581 });
		for (int i = 0; i < subRoi1Ptr->mInnerConners.size(); ++i)
			subRoi1Ptr->mInnerConners[i] = {
			float(subRoi1Ptr->mInnerConners[i].x * 9344 / 934 / 1.5),
			float(subRoi1Ptr->mInnerConners[i].y * 7000 / 700 / 1.5) };
		std::shared_ptr<CSubRoi> subRoi2Ptr = std::make_shared<CSubRoi>();
		subRoi2Ptr->mRoiPoints.push_back({ 465,436 });
		subRoi2Ptr->mRoiPoints.push_back({ 615, 260 });
		subRoi2Ptr->mRoiPoints.push_back({ 783, 405 });
		subRoi2Ptr->mRoiPoints.push_back({ 630, 581 });
		for (int i = 0; i < subRoi2Ptr->mInnerConners.size(); ++i)
			subRoi2Ptr->mInnerConners[i] = {
			float(subRoi2Ptr->mInnerConners[i].x * 9344 / 934 / 1.5),
			float(subRoi2Ptr->mInnerConners[i].y * 7000 / 700 / 1.5) };
		std::shared_ptr<CSubRoi> subRoi3Ptr = std::make_shared<CSubRoi>();
		subRoi3Ptr->mRoiPoints.push_back({ 465,436 });
		subRoi3Ptr->mRoiPoints.push_back({ 615, 260 });
		subRoi3Ptr->mRoiPoints.push_back({ 783, 405 });
		subRoi3Ptr->mRoiPoints.push_back({ 630, 581 });
		for (int i = 0; i < subRoi3Ptr->mInnerConners.size(); ++i)
			subRoi3Ptr->mInnerConners[i] = {
			float(subRoi3Ptr->mInnerConners[i].x * 9344 / 934 / 1.5),
			float(subRoi3Ptr->mInnerConners[i].y * 7000 / 700 / 1.5) };

		measureInfo->GetSubRois().push_back(subRoi1Ptr);
		measureInfo->GetSubRois().push_back(subRoi2Ptr);
		measureInfo->GetSubRois().push_back(subRoi3Ptr);
	}
	else
	{
		//////////////////////////////////////////////////////////////////////////////////////////
		if (idxx == 0)
		{
			// 待优化
			// init sig
			ret = CYolov5For928::Get()->pfucLibapiSvpNpuHandleSig();
			if (ret != RST_OK)
			{
				assert(0);
				return ret;
			}
			// init acl
			ret = CYolov5For928::Get()->pfucLibapiSvpNpuAclPrepareInit();
			if (ret != RST_OK)
			{
				assert(0);
				return ret;
			}

			// init model
			const char* om_model_path = G(yolo_modol_path).c_str();
			ret = CYolov5For928::Get()->pfucLibApiSvpNpuLoadModel(om_model_path, 0, false);
			if (ret != RST_OK)
			{
				assert(0);
				return ret;
			}
			idxx = 1;
		}
		//////////////////////////////////////////////////////////////////////////////////////////

		cv::Mat* hq_im = measureInfo->GetIm().get();
		// run yolo model to detect 
		ret = CYolov5For928::Get()->pfucLibApiSvpNpuLoadDataset(0, (void*)hq_im->data, hq_im->total() * hq_im->channels(),
			hq_im->size().width , hq_im->size().height , CV_8UC3);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuLoadDataset() has error %d" , ret);
			Loger(LEVEL_INFOR, " w: %d h : %d ch : %d", hq_im->size().width, hq_im->size().height, hq_im->channels());
			return CAL_NPU_LOAD_DATASET_FAILED;
		}
		Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuLoadDataset() success");
		ret = CYolov5For928::Get()->pfucLibApiSvpNpuModelExecute(0);
		if (ret != CAL_OK) 
		{
			CYolov5For928::Get()->pfucLibApiSvpNpuUnloadDataset();
			Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuModelExecute() has error %d", ret);
			return CAL_NPU_MODEL_EXE_FAILED;
		}
		Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuModelExecute() success");
		RetrunObject objs;
		ret = CYolov5For928::Get()->pfucLibApiSvpNpuGetModelExecuteResult(0, &objs);
		if (ret != CAL_OK)
		{
			CYolov5For928::Get()->pfucLibApiSvpNpuUnloadDataset();
			Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuGetModelExecuteResult() has error %d", ret);
			return CAL_NPU_GET_RESULT_FAILED;
		}
		Loger(LEVEL_INFOR, "pfucLibApiSvpNpuGetModelExecuteResult objs.count = %d", objs.count);
		if (objs.count == 0)
		{
			Loger(LEVEL_INFOR, " pfucLibApiSvpNpuGetModelExecuteResult() objs.count == 0");
		}
		else if (objs.count > 128)
		{
			Loger(LEVEL_INFOR, " pfucLibApiSvpNpuGetModelExecuteResult() objs.count > 128");
		}
		else
		{
			for (int i = 0; i < objs.count; ++i)
			{
				std::cout << " label: " << G(yolo_label) << " - " << objs.objects[i].label
					<< " prob: " << G(yolo_prob) << " - " << objs.objects[i].prob << std::endl;
				if (objs.objects[i].label == (int)G(yolo_label) && objs.objects[i].prob > (float)G(yolo_prob))
				{
					std::shared_ptr<CSubRoi> subRoi1Ptr = std::make_shared<CSubRoi>();
					//subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x - 20), (float)(objs.objects[i].y + G(roi_y_offset) - 20) });
					//subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x + 20 + (float)objs.objects[i].width), (float)(objs.objects[i].y + G(roi_y_offset) - 20) });
					//subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x + 20 + (float)objs.objects[i].width), (float)(objs.objects[i].y + objs.objects[i].height + G(roi_y_offset) + 20) });
					//subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x - 20), (float)(objs.objects[i].y + objs.objects[i].height + G(roi_y_offset) + 20) });
					subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x - 2), (float)(objs.objects[i].y + G(roi_y_offset) - 2) });
					subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x + 2 + (float)objs.objects[i].width), (float)(objs.objects[i].y + G(roi_y_offset) - 2) });
					subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x + 2 + (float)objs.objects[i].width), (float)(objs.objects[i].y + objs.objects[i].height + G(roi_y_offset) + 2) });
					subRoi1Ptr->mRoiPoints.push_back({ (float)(objs.objects[i].x - 2), (float)(objs.objects[i].y + objs.objects[i].height + G(roi_y_offset) + 2) });

					for (int j = 0; j < 4; ++j)
					{
						if (subRoi1Ptr->mRoiPoints[j].x < 0)
							subRoi1Ptr->mRoiPoints[j].x = 0;
						if (subRoi1Ptr->mRoiPoints[j].x > hq_im->cols)
							subRoi1Ptr->mRoiPoints[j].x = hq_im->cols;
						if (subRoi1Ptr->mRoiPoints[j].y < 0)
							subRoi1Ptr->mRoiPoints[j].y = 0;
						if (subRoi1Ptr->mRoiPoints[j].y > hq_im->rows)
							subRoi1Ptr->mRoiPoints[j].y = hq_im->rows;
					}
					measureInfo->GetSubRois().push_back(subRoi1Ptr);
					
					if (measureInfo->GetSubRois().size() == 16)
					{
						Loger(LEVEL_INFOR, "[ERROR] pfucLibApiSvpNpuGetModelExecuteResult() objs.count > 16");
					}
					else
					{
						Loger(LEVEL_INFOR, "measureInfo->GetSubRois().size() is %d", measureInfo->GetSubRois().size());
					}
				}
			}
		}
		ret = CYolov5For928::Get()->pfucLibApiSvpNpuUnloadDataset();
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, " CYolov5For928::Get()->pfucLibApiSvpNpuUnloadDataset() has error %d", ret);
			return CAL_NPU_UNLOAD_DATASET_FAILED;
		}

		// 待优化
		//////////////////////////////////////////////////////////////////////////////////////////////////////////
		//ret = CYolov5For928::Get()->pfucLibApiSvpNpuUnLoadModel(0);
		//ret = CYolov5For928::Get()->pfucLibApiSvpNpuAclPrepareExit();
		//////////////////////////////////////////////////////////////////////////////////////////////////////////
		Loger(LEVEL_INFOR, "LibApiSvpNpuModelExecute success");

	}

	return ret;
}

//通过一组ROI坐标获取ROI图像(弃用)
//id: parentId
int Cal2D::LibapiGetSubImFromRois(cv::Mat& im, const int& id, const std::string& info,
	std::vector<cv::Rect2f> rois, std::vector<CSubRoi>& subMat, bool fake)
{
	if (im.empty() || rois.size() == 0)
	{
		Loger(LEVEL_INFOR, "im.empty() || rois.size() == 0");
		return CAL_2D_ROI_DETECTIION_IS_0;
		//assert(0);
	}

	int ret = CAL_OK;

	for (int i = 0; i < rois.size(); i++)
	{
		CSubRoi mat;
		//mat
	}

	return ret;
}

//在单个预埋件图像中计算区域内预埋件边缘和角点
int Cal2D::LibapiGetEdegsAndConners(cv::Mat& src,
	std::vector<std::vector<float>>& sort_lines4,
	std::vector<std::vector<float>>& sort_conners4)
{
	return GetSortConnersAndEdgess(
		src,
		sort_lines4,
		sort_conners4,
		80,
		G(edge_min_line_len));
}

// 挑选出的内在区域(亚像素检测算法)
int Cal2D::LibapiSelectInnerRegion(CSubRoi& subMat)
{
	int ret = CAL_OK;
	if (subMat.mRoiPoints.size() != 4)
		assert(0);

	if (subMat.mRoiPoints.size() != 4)
		assert(0);
	return ret;
}

// 挑选出2D-3D点对用于计算单应矩阵H
int Cal2D::LibapiSelect2DTo3DPoints(CMeasureInfo& measureInfo, CSubRoi& subRoi,
	std::vector<cv::Point2f>& vec2D, std::vector<cv::Point3f>& vec3D)
{
	// 该函数弃用
	assert(0);
	if (measureInfo.GetIm()->empty() || subRoi.mInnerConners.size() != 4)
		assert(0);

	int ret = CAL_OK;

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();
	// 待修改，在roi中遍历
	float* hptr;
	cv::Mat *im_2D3D = measureInfo.GetIm().get();
	int ch = im_2D3D->channels();
	for (int h = 0; h < im_2D3D->rows; ++h)
	{
		hptr = (float*)im_2D3D->ptr<float>(h);
		for (int w = 0; w < im_2D3D->cols; ++w)
		{
			// 待优化
			if (hptr[w * ch + 2] >= 3.5)
			{
				cv::Point2f p2 = { (float)w, (float)h };
				if (cv::pointPolygonTest(subRoi.mInnerConners, p2, false))
				{
					cv::Point3f v3 = { hptr[w * ch], hptr[w * ch + 1], hptr[w * ch + 2] };
					vec2D.push_back(p2);
					vec3D.push_back(v3);
				}
			}
		}
	}

	if (vec2D.size() < 2000)
		assert(0);

	std::cout << vec2D.size() << std::endl;
	std::cout << vec3D.size() << std::endl;
	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal2D::LibapiSelect2DTo3DPoints time is : " << elapsex.count() << std::endl;
	return ret;
}

// 计算单应矩阵
int Cal2D::LibapiCalH(const std::vector<cv::Point2f>& vec2D, const std::vector<cv::Point3f>& vec3D, cv::Mat& H)
{
	if (vec2D.size() == 0 || vec3D.size() == 0)
	{
		Loger(LEVEL_INFOR, "vec2D.size() == 0 || vec3D.size() == 0");
		return CAL_PRAMA_EMPUTY;
		//assert(0);
	}

	int ret = CAL_OK;

	std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();

	float ransacReprojThreshold = 5.0;
	std::vector<cv::Point3f> vp3;
	for (int i = 0; i < vec2D.size(); ++i)
	{
		cv::Point3f p3 = { vec2D[i].x, vec2D[i].y, 1 };
		vp3.push_back(p3);
	}

	H = cv::findHomography(vec3D, vp3, cv::RANSAC, ransacReprojThreshold);
	std::cout << H << std::endl;
	std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsex = m_end - m_start;
	std::cout << "Cal2D::LibapiCalH time is : " << elapsex.count() << std::endl;

	return ret;
}

// 计算预埋件角点和中心位置
int Cal2D::LibapiCalScale(const CSubRoi& subConner, int H)
{
	int ret = CAL_OK;



	return ret;
}

static CMeasureInfo* fake_create2D3D()
{
	int ret = CAL_OK;

	utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);

	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 = Cal3D::LibapiCapCloudPoints(cloud_ptr);
	if (ret != CAL_OK) assert(0);

	ret = Cal3D::LibapiRotAndTrans(cloud_ptr);

	ret = Cal3D::LibapiFilterCloudPointsByDist(cloud_ptr, cloud_ptr_by_dist,
		G(dminx), G(dmaxx), G(dminy), G(dmaxy), G(dminz), G(dmaxz));
	if (ret != CAL_OK) assert(0);
	if (cloud_ptr_by_dist->IsEmpty()) assert(0);

	ret = Cal3D::LibapiCorrectCloudPoint(cloud_ptr_by_dist, cloud_ptr_by_correct, 0);

	std::vector<cv::Point3d> vec_point_3d;
	ret = Cal3D::LibapiCloudPointToVec(cloud_ptr_by_correct, vec_point_3d);

	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;

	//cv::Mat test_src_im;
	std::shared_ptr<cv::Mat> im_2D3D = std::make_shared<cv::Mat>(cv::Size(G(w), G(h)), 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);

	// for test
	ret = Cal3D::LibapiCloudPointProjectTest(*im_2D3D);
	//cv::FileStorage file("d:\\im_2D3D.yml", cv::FileStorage::WRITE);
	//std::string tag = "im_2D3D";
	//file << tag << *im_2D3D;
	//file.release();

	// 创建CMeasureInfo数据结构
	long id;
	ret = CalUtils::LibapiGenRandomId(id);
	std::string info = "abc";

	//std::shared_ptr<CMeasureInfo> measureInfoPtr = std::make_shared<CMeasureInfo>(id, info, im_2D3D);
	CMeasureInfo* measureInfoPtr = CMeasureInfo::Init(id, info);
	measureInfoPtr->SetIm2D3D(im_2D3D);

 	return measureInfoPtr;
}

//int main()
//{
//	G(fx);
//	int ret = CAL_OK;
//
//	std::shared_ptr<cv::Mat> hqImPtr = std::make_shared<cv::Mat>();
//
//	*hqImPtr = cv::imread("./caowei/fe57022fa2c58888.jpg");
//
//	std::vector<std::vector<float>> sort_lines4;
//	std::vector<std::vector<float>> sort_conners4;
//
//	cv::Mat hq_im_cp = hqImPtr->clone();
//	GetSortConnersAndEdgess(
//		hq_im_cp,
//		sort_lines4,
//		sort_conners4,
//		80,
//		100);
//
//	float x1, y1, x2, y2;
//	float x_p, y_p;
//
//	for (int i = 0; i < 4; ++i)
//	{
//		std::vector<float> line = sort_lines4[i];
//		x1 = line[0], y1 = line[1], x2 = line[2], y2 = line[3];
//		std::vector<float> conners = sort_conners4[i];
//		x_p = conners[0], y_p = conners[1];
//		
//		cv::circle(*hqImPtr, cv::Point(int(x_p), int(y_p)), 2, cv::Scalar(0, 0, 255), -1);
//		cv::line(*hqImPtr, cv::Point(int(x1), int(y1)), cv::Point(int(x2), int(y2)), cv::Scalar(0,255, 0), 1);
//	}
//
//	cv::imshow("im_edge", *hqImPtr);
//	cv::waitKey(0);
//
//	// 拍摄照片
//	CMeasureInfo* measureInfoPtr = CMeasureInfo::Init(1, "");
//	ret = Cal2D::LibapiCapHQImage(measureInfoPtr, hqImPtr);
//	if (ret != CAL_OK || hqImPtr->empty()) 
//		assert(0);
//	
//	
//	// AI识别划分ROI
//	//std::vector<cv::Rect2f> rois;
//	//ret = Cal2D::LibapiDetectObj(hq_im, rois, true);
//	//if (ret != CAL_OK || rois.size()==0) 
//	//	assert(0);
//	measureInfoPtr = fake_create2D3D();
//	std::shared_ptr<CSubRoi> subRoi = std::make_shared<CSubRoi>();
//	measureInfoPtr->GetSubRois().push_back(subRoi);
//
//	subRoi->mInnerConners.push_back({ 465,436 });
//	subRoi->mInnerConners.push_back( { 615, 260 } );
//	subRoi->mInnerConners.push_back({ 783, 405 } );
//	subRoi->mInnerConners.push_back( { 630, 581 } );
//	for (int i = 0; i < subRoi->mInnerConners.size(); ++i)
//		subRoi->mInnerConners[i] = {
//		float(subRoi->mInnerConners[i].x * 9344 / 934 / 1.5),
//		float(subRoi->mInnerConners[i].y * 7000 / 700 / 1.5)};
//	std::vector<cv::Point2f> vec2D;
//	std::vector<cv::Point3f> vec3D;
//	
//	//ret = Cal2D::LibapiSelect2DTo3DPoints(*measureInfoPtr, *subRoi, vec2D, vec3D);
//
//	cv::Mat H;
//	//ret = Cal2D::LibapiCalH(vec2D, vec3D, H);
//
//	// 开多线程分发ROI进行识别
//
//
//	return 0;
//}