#include "Inc/MobileFaceFeatureDnn.h"
#include "Inc/DeepCamDef.h"

#define FEATURE_IMG_SIZE 112

extern unsigned char mobile_face_model0[];
extern unsigned char mobile_face_model1[];
extern unsigned char mobile_face_model2[];

extern int mobile_face_model0_len;
extern int mobile_face_model1_len;
extern int mobile_face_model2_len;

MobileFaceFeatureDnn* MobileFaceFeatureDnn::m_instance = nullptr;

MobileFaceFeatureDnn* MobileFaceFeatureDnn::GetInstance()
{
	if (m_instance == nullptr)
	{
		m_instance = new MobileFaceFeatureDnn();
	}
	return m_instance;
}

MobileFaceFeatureDnn::MobileFaceFeatureDnn()
{
	const unsigned char pw[] = "~12321@AAA";
	std::vector<unsigned char> model;
	model.insert(model.end(), mobile_face_model0, mobile_face_model0 + mobile_face_model0_len);
	model.insert(model.end(), mobile_face_model1, mobile_face_model1 + mobile_face_model1_len);
	model.insert(model.end(), mobile_face_model2, mobile_face_model2 + mobile_face_model2_len);
	
	int len = mobile_face_model0_len + mobile_face_model1_len + mobile_face_model2_len;
	for (int i = 0; i < len; i++) {
		model[i] = model[i] ^ pw[i % sizeof(pw)];
	}

	//mobile_onnx = cv::dnn::readNetFromONNX(R"(K:\workspace\DeepCamFace-WinSdk_bak\models\MobileFaceNet_v6.6.4.onnx)");
	mobile_onnx = cv::dnn::readNetFromONNX(model);
	
#if USE_CUDA
	mobile_onnx.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA);
	mobile_onnx.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
#else
	mobile_onnx.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
	mobile_onnx.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
#endif
}

MobileFaceFeatureDnn::~MobileFaceFeatureDnn()
{
}

cv::Mat MobileFaceFeatureDnn::estimateTrans(std::vector<cv::Point2f>& srcLmks, std::vector<cv::Point2f>& dstLmks)
{
	cv::Mat A(2 * srcLmks.size(), 4, CV_32FC1);
	cv::Mat B(2 * srcLmks.size(), 1, CV_32FC1);
	for (int i = 0; i < srcLmks.size(); i++)
	{
		A.at<float>(2 * i, 0) = srcLmks[i].x;
		A.at<float>(2 * i, 1) = -srcLmks[i].y;
		A.at<float>(2 * i, 2) = 1;
		A.at<float>(2 * i, 3) = 0;
		B.at<float>(2 * i, 0) = dstLmks[i].x;

		A.at<float>(2 * i + 1, 0) = srcLmks[i].y;
		A.at<float>(2 * i + 1, 1) = srcLmks[i].x;
		A.at<float>(2 * i + 1, 2) = 0;
		A.at<float>(2 * i + 1, 3) = 1;
		B.at<float>(2 * i + 1, 0) = dstLmks[i].y;
	}

	cv::Mat C, X;
	C = (A.t() * A);
	X = C.inv() * A.t() * B;
	cv::Mat transformMatrix(2, 3, CV_32FC1);
	transformMatrix.at<float>(0, 0) = X.at<float>(0, 0);
	transformMatrix.at<float>(0, 1) = -X.at<float>(1, 0);
	transformMatrix.at<float>(0, 2) = X.at<float>(2, 0);
	transformMatrix.at<float>(1, 0) = X.at<float>(1, 0);
	transformMatrix.at<float>(1, 1) = X.at<float>(0, 0);
	transformMatrix.at<float>(1, 2) = X.at<float>(3, 0);
	return transformMatrix;
}

cv::Mat MobileFaceFeatureDnn::FaceAlign(const cv::Mat& inputImage, const float* landmarks)
{
	std::vector<cv::Point2f> templateLmks;
	std::vector<cv::Point> salientLmks;
	for (int l = 0; l < 5; l++) {
		salientLmks.push_back(cv::Point(landmarks[l * 2], landmarks[l * 2 + 1]));
	}
	static double const template_points[] = { 30.2946, 65.5318, 48.0252, 33.5493, 62.7299, 51.6963, 51.5014, 71.7366, 92.3655, 92.2041 };
	static double shift_x = 8.0;
	static size_t n = sizeof(template_points) / (2 * sizeof(template_points[0]));
	for (int i = 0; i < n; i++)
	{
		templateLmks.push_back(cv::Point2f(template_points[i] + shift_x, template_points[i + n]));
	}

	cv::Mat normalizedImage;
	cv::Mat noFace(0, 0, CV_8UC1);

	cv::Point2f leftEye;
	cv::Point2f rightEye;

	leftEye = salientLmks.at(0);
	rightEye = salientLmks.at(1);

	cv::Point2f nose;
	cv::Point2f leftMouth;
	cv::Point2f rightMouth;

	nose = salientLmks.at(2);
	leftMouth = salientLmks.at(3);
	rightMouth = salientLmks.at(4);

	std::vector<cv::Point2f> srcLmks;
	srcLmks.push_back(leftEye);
	srcLmks.push_back(rightEye);
	srcLmks.push_back(nose);
	srcLmks.push_back(leftMouth);
	srcLmks.push_back(rightMouth);

	cv::Mat matTransform = estimateTrans(srcLmks, templateLmks);

	if (matTransform.empty())
	{
		normalizedImage = noFace;
	}
	else {
		cv::warpAffine(inputImage, normalizedImage, matTransform, cv::Size(FEATURE_IMG_SIZE, FEATURE_IMG_SIZE));
	}
	return normalizedImage;
}

int MobileFaceFeatureDnn::GetFaceFeature(cv::Mat& img, const float* landmarks, float* feature)
{
	std::lock_guard<std::mutex> lock(v6_mt);

	cv::Mat& alignFace = FaceAlign(img, landmarks);
	//cv::imshow("FaceAlign-dnn", alignFace);
	//cv::waitKey(1);
	if (alignFace.cols != FEATURE_IMG_SIZE || alignFace.rows != FEATURE_IMG_SIZE)
	{
		cv::resize(alignFace, alignFace, cv::Size(FEATURE_IMG_SIZE, FEATURE_IMG_SIZE));
	}

	std::vector<cv::Mat> out_blobs;
	try
	{
		cv::Mat& inputBlob = cv::dnn::blobFromImage(alignFace, 1.0 / 127.5, cv::Size(FEATURE_IMG_SIZE, FEATURE_IMG_SIZE), cv::Scalar(127.5, 127.5, 127.5), true);
		mobile_onnx.setInput(inputBlob);
		static std::vector<cv::String> output_names = { "feature" };
		{
			//mobile_onnx.forward(out_blobs, "feature");
			out_blobs.push_back(mobile_onnx.forward("feature"));
		}
		
		if (out_blobs.size() > 0)
		{
			memcpy(feature, (float*)out_blobs[0].data, out_blobs[0].cols);
			//float* _feature = (float*)out_blobs[0].data;
			//for (int i = 0; i < out_blobs[0].cols; i++)
			//{
			//	feature.push_back(_feature[i]);
			//}
			return ERR_OK;
		}
		else
		{
			memset(feature, 0.f, 512 * sizeof(float));
		}
	}
	catch (cv::Exception* e)
	{
		std::cout << "get fece feature error: " << e->what() << std::endl;
	}

	return ERR_OK;
}


//外部调用接口

/*
float deepcam_face_compare(std::vector<float> &feature1, std::vector<float> &feature2)
{
	if (feature1.size() < 1 || feature1.size() != feature2.size())
	{
		return 0;
	}
	
	int size = feature1.size();
	float tmp0 = 0, tmp1 = 0, tmp2 = 0;
	for (int i = 0; i < size; i++)
	{
		tmp0 += feature1[i] * feature2[i];
		tmp1 += feature1[i] * feature1[i];
		tmp2 += feature2[i] * feature2[i];
	}
	return (float(tmp0 / (sqrt(tmp1)* sqrt(tmp2))));
}

float mappingnScore(float score) {
	float max_score = log10f(1.01);
	float min_score = log10f(0.02);
	float ret, temp;
	if (score <= 0) {
		temp = fabs(score);
		if (temp > 0.3) {
			ret = 0.0001;
		}
		else {
			ret = temp;
		}
	}
	else {
		temp = log10f(score + 0.01);
		ret = fabsf(temp - min_score) / (max_score - min_score);
	}
	return ret;
}
*/