#include <stdio.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include "time_helper.h"
#include "cv_face.h"
#include <windows.h>

#define DEFAULT_THRESHOLD (0.5)
using namespace std;
using namespace cv;

// @»ñÈ¡ÈËÁ³ÌØÕ÷Öµ
// @param bgr_image ÐèÒªÌáÈ¡ÌØÕ÷ÖµµÄͼƬ
// @param handle_verify ÒѾ­³õʼ»¯µÄÈËÁ³ÑéÖ¤¾ä±ú
cv_feature_t * getFeature(Mat bgr_image,cv_handle_t handle_verify) {

	cv_feature_t *p_feature = NULL;
	Mat image_color;
	image_color = bgr_image;

	//µ÷Óþä±ú¡£ÓÃÓÚ±£´æº¯Êý¼°Êý¾ÝµÄ¾ä±ú
	cv_handle_t handle_detect = NULL;

	//ÈËÁ³ÐÅÏ¢½á¹¹Ìå
	cv_face_t *p_face = NULL;

	int face_count = 0;

	//º¯Êý·µ»ØµÄ´íÎó´úÂëÀàÐÍ
	cv_result_t cv_result = CV_OK;
	char *string_feature;
	do 
	{
		// ´´½¨¾²Ì¬Í¼Æ¬ÈËÁ³¼ì²â¾ä±ú
		cv_result = cv_face_create_detector(&handle_detect, NULL, CV_DETECT_ENABLE_ALIGN_21);
		if (cv_result != CV_OK) {
			fprintf(stderr, "fail to init detect handle, error code %d\n", cv_result);
			break;
		}

		// ÈËÁ³¼ì²â1.³É¹¦·µ»ØCV_OK£¬·ñÔò·µ»Ø´íÎóÀàÐÍ
		cv_result = cv_face_detect(handle_detect, image_color.data, CV_PIX_FMT_BGR888,
			image_color.cols, image_color.rows, image_color.step,
			CV_FACE_UP, &p_face, &face_count);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect failed, error code : %d\n", cv_result);
			break;
		}

		if (face_count > 0) {
			//ÓÃÓÚ±íʾÈËÁ³ÌØÕ÷ÐÅÏ¢
			float score;
			unsigned int feature_length = 0;

			// get feature
			__TIC__();
			//ÌáÈ¡ÈËÁ³ÌØÕ÷1£¬¿ÉÒÔ°Ñ·µ»ØÊý×é±àÂë³É×Ö·û´®ºó´æ´¢ÆðÀ´ÒÔ±ãÒÔºóʹÓÃ
			cv_result = cv_verify_get_feature(handle_verify, image_color.data, CV_PIX_FMT_BGR888,
				image_color.cols, image_color.rows, image_color.step, p_face,
				&p_feature, &feature_length);
			__TOC__();
			if (cv_result != CV_OK) {
				fprintf(stderr, "cv_verify_get_feature failed, error code %d\n", cv_result);
				break;
			}

			if (feature_length > 0 ) {

				// test serial and deserial
				string_feature = new char[CV_ENCODE_FEATURE_SIZE(p_feature)];
				cv_verify_serialize_feature(p_feature, string_feature);

				//ÊÍ·ÅÌáÈ¡ÈËÁ³ÌØÕ÷ʱ·ÖÅäµÄ¿Õ¼ä
				cv_verify_release_feature(p_feature);
				break;
			} else {
				fprintf(stderr, "error, the feature length is 0!\n");
			}
			// ÊÍ·ÅÌáÈ¡ÈËÁ³ÌØÕ÷ʱ·ÖÅäµÄ¿Õ¼ä
			cv_verify_release_feature(p_feature);

		} else {
			if (face_count == 0) {
				fprintf(stderr, "can't find face in \n");
			}
		}
	} while (0);
	cv_face_release_detector_result(p_face, face_count);
	return string_feature;
}


//²âÊÔÈ·¶¨ÈËÁ³Î»ÖÃ
int testface_detect(Mat bgr_image_color,char* output_image_path ){

	int points_size = 106;
	int config;
	if (points_size == 21) {
		config = CV_DETECT_ENABLE_ALIGN_21;
	}
	else if (points_size == 106) {
		config = CV_DETECT_ENABLE_ALIGN_106;
	}
	else {
		fprintf(stderr, "alignment point size error, must be 21 or 106\n");
		return -1;
	}

	// load image
	Mat image_color;


	//	cvtColor(bgr_image_color, image_color, CV_BGR2BGRA);    // CV_PIX_FMT_BGRA8888
	image_color = bgr_image_color;                          // CV_PIX_FMT_BGR888

	// init detect handle
	cv_handle_t handle_detect = NULL;
	cv_result_t cv_result = CV_OK;
	cv_face_t* p_face = NULL;
	int face_count = 0;
	do 
	{
		cv_result = cv_face_create_detector(&handle_detect, NULL, config);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_create_detector failed, error code %d\n", cv_result);
			break;
		}

		/*
		* test get and set threshold
		*/
		float default_threshold;
		cv_result = cv_face_detect_get_threshold(handle_detect, &default_threshold);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect_get_threshold failed, error code %d\n", cv_result);
			break;
		}
		fprintf(stderr, "default threshold : %f\n", default_threshold);

		cv_result = cv_face_detect_set_threshold(handle_detect, default_threshold);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect_set_threshold failed, error code %d\n", cv_result);
			break;
		}
		fprintf(stderr, "threshold set : %f\n", default_threshold);


		// detect
		__TIC__();
		cv_result = cv_face_detect(handle_detect, image_color.data, CV_PIX_FMT_BGR888,
			image_color.cols, image_color.rows, image_color.step,
			CV_FACE_UP, &p_face, &face_count);
		__TOC__();
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect error %d\n", cv_result);
			break;
		}

		if (face_count > 0) {
			// draw result
			for (int i = 0; i < face_count; i++) {
				rectangle(image_color, Point(p_face[i].rect.left, p_face[i].rect.top),
					Point(p_face[i].rect.right, p_face[i].rect.bottom),
					Scalar(0, 255, 0), 2, 8, 0);
				fprintf(stderr, "face number: %d\n", i + 1);
				fprintf(stderr, "face rect: [%d, %d, %d, %d]\n", p_face[i].rect.top,
					p_face[i].rect.left,
					p_face[i].rect.right, p_face[i].rect.bottom);
				fprintf(stderr, "score: %f\n", p_face[i].score);
				fprintf(stderr, "face pose: [yaw: %f, pitch: %f, roll: %f, eye distance: %f]\n",
					p_face[i].yaw,
					p_face[i].pitch, p_face[i].roll, p_face[i].eye_dist);
				fprintf(stderr, "face algin:\n");
				for (unsigned int j = 0; j < p_face[i].points_count; j++) {
					float x = p_face[i].points_array[j].x;
					float y = p_face[i].points_array[j].y;
					fprintf(stderr, "(%.2f, %.2f)\n", x, y);
					circle(image_color, Point2f(x, y), 2, Scalar(0, 0, 255), -1);
				}
				fprintf(stderr, "\n");
			}
			// save image
			imwrite(output_image_path, image_color);
		}
		else {
			fprintf(stderr, "can't find face in ");
		}

	} while (0);


	// release the memory of face
	cv_face_release_detector_result(p_face, face_count);
	// destroy detect handle
	cv_face_destroy_detector(handle_detect);


	fprintf(stderr, "test finish!\n");
	return 0;
}

// @brief ²âÊÔÈËÁ³¶Ô±È
// @param bgr_image_1 ͼÏñ1
// @param bgr_image_1 ͼÏñ2
int testface_verify(Mat bgr_image_1,Mat bgr_image_2){

	// ±£´æÁ½¸öͼƬµÄԭʼÊý¾Ý
	Mat image_color_1,image_color_2;
	//	cvtColor(bgr_image_1, image_color_color_1, CV_BGR2BGRA);    // CV_PIX_FMT_BGRA8888
	image_color_1 = bgr_image_1;                                // CV_PIX_FMT_BGR888
	//	cvtColor(bgr_image_2, image_color_2, CV_BGR2BGRA);
	image_color_2 = bgr_image_2;

	int main_return = -1;

	//µ÷Óþä±ú¡£ÓÃÓÚ±£´æº¯Êý¼°Êý¾ÝµÄ¾ä±ú
	cv_handle_t handle_detect = NULL;
	cv_handle_t handle_verify = NULL;

	//ÈËÁ³ÐÅÏ¢½á¹¹Ìå
	cv_face_t *p_face_1 = NULL;
	cv_face_t *p_face_2 = NULL;

	int face_count_1 = 0;
	int face_count_2 = 0;

	//º¯Êý·µ»ØµÄ´íÎó´úÂëÀàÐÍ
	cv_result_t cv_result = CV_OK;

	do {
		cout<<"a"<<endl;
		// ´´½¨¾²Ì¬Í¼Æ¬ÈËÁ³¼ì²â¾ä±ú
		cv_result = cv_face_create_detector(&handle_detect, NULL, CV_DETECT_ENABLE_ALIGN_21);

		if (cv_result != CV_OK) {
			fprintf(stderr, "fail to init detect handle, error code %d\n", cv_result);
			break;
		}

		// ÈËÁ³¼ì²â1.³É¹¦·µ»ØCV_OK£¬·ñÔò·µ»Ø´íÎóÀàÐÍ
		cv_result = cv_face_detect(handle_detect, image_color_1.data, CV_PIX_FMT_BGR888,
			image_color_1.cols, image_color_1.rows, image_color_1.step,
			CV_FACE_UP, &p_face_1, &face_count_1);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect failed, error code : %d\n", cv_result);
			break;
		}

		// ÈËÁ³¼ì²â2.³É¹¦·µ»ØCV_OK£¬·ñÔò·µ»Ø´íÎóÀàÐÍ
		cv_result = cv_face_detect(handle_detect, image_color_2.data, CV_PIX_FMT_BGR888,
			image_color_2.cols, image_color_2.rows, image_color_2.step,
			CV_FACE_UP, &p_face_2, &face_count_2);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_detect failed, error code : %d\n", cv_result);
			break;
		}

		// ÈËÁ³ÑéÖ¤
		if (face_count_1 > 0 && face_count_2 > 0) {
			// ´´½¨ÈËÁ³ÑéÖ¤¾ä±ú
			cv_result = cv_verify_create_handle(&handle_verify, "../../../models/verify.model");
			if (cv_result != CV_OK)
			{
				fprintf(stderr, "fail to init verify handle, error code %d\n", cv_result);
				break;
			}

			if (handle_verify) {
				int model_version = cv_verify_get_version(handle_verify);
				fprintf(stderr, "verify model version : %d\n", model_version);
				int feature_length = cv_verify_get_feature_length(handle_verify);
				fprintf(stderr, "verify model feature length : %d\n", feature_length);

				//ÓÃÓÚ±íʾÈËÁ³ÌØÕ÷ÐÅÏ¢
				cv_feature_t *p_feature_1 = NULL, *p_feature_2 = NULL;
				float score;
				unsigned int feature_length_1 = 0, feature_length_2 = 0;

				// get feature
				__TIC__();
				//ÌáÈ¡ÈËÁ³ÌØÕ÷1£¬¿ÉÒÔ°Ñ·µ»ØÊý×é±àÂë³É×Ö·û´®ºó´æ´¢ÆðÀ´ÒÔ±ãÒÔºóʹÓÃ
				cv_result = cv_verify_get_feature(handle_verify, image_color_1.data, CV_PIX_FMT_BGR888,
					image_color_1.cols, image_color_1.rows, image_color_1.step, p_face_1,
					&p_feature_1, &feature_length_1);
				__TOC__();
				if (cv_result != CV_OK) {
					fprintf(stderr, "cv_verify_get_feature failed, error code %d\n", cv_result);
					break;
				}
				//ÌáÈ¡ÈËÁ³ÌØÕ÷1£¬¿ÉÒÔ°Ñ·µ»ØÊý×é±àÂë³É×Ö·û´®ºó´æ´¢ÆðÀ´ÒÔ±ãÒÔºóʹÓÃ
				cv_result = cv_verify_get_feature(handle_verify, image_color_2.data, CV_PIX_FMT_BGR888,
					image_color_2.cols, image_color_2.rows, image_color_2.step, p_face_2, 
					&p_feature_2, &feature_length_2);
				if (cv_result != CV_OK) {
					fprintf(stderr, "cv_verify_get_feature failed, error code %d\n", cv_result);
					break;
				}

				if (feature_length_1 > 0 && feature_length_2 > 0) {
					cv_feature_header_t *p_feature_header = CV_FEATURE_HEADER(p_feature_1);
					fprintf(stderr, "Feature information:\n");
					fprintf(stderr, "    ver:\t0x%08x\n", p_feature_header->ver);
					fprintf(stderr, "    length:\t%d bytes\n", p_feature_header->len);

					// ÈËÁ³ÑéÖ¤
					cv_result = cv_verify_compare_feature(handle_verify, p_feature_1,
						p_feature_2, &score);
					if (cv_result == CV_OK) {
						fprintf(stderr, "score: %f\n", score);
						// comapre score with DEFAULT_THRESHOLD
						// > DEFAULT_THRESHOLD => the same person
						// < DEFAULT_THRESHOLD => different people
						if (score > DEFAULT_THRESHOLD)
							fprintf(stderr, "the same person.\n");
						else
							fprintf(stderr, "different people.\n");

						main_return = 0;  // success
					}
					else {
						fprintf(stderr, "cv_verify_compare_feature failed, error code : %d\n", cv_result);
					}

					// test serial and deserial
					char *string_feature_1 = new char[CV_ENCODE_FEATURE_SIZE(p_feature_1)];
					cv_verify_serialize_feature(p_feature_1, string_feature_1);
					cout<<string_feature_1<<endl;
					cv_feature_t *p_feature_new_1 = cv_verify_deserialize_feature(string_feature_1);
					delete[]string_feature_1;
					char *string_feature_2;
					string_feature_2 = new char[CV_ENCODE_FEATURE_SIZE(p_feature_2)];
					cv_verify_serialize_feature(p_feature_2, string_feature_2);
					cout<<string_feature_2<<endl;
					cv_feature_t *p_feature_new_2 = cv_verify_deserialize_feature(string_feature_2);
					delete[]string_feature_2;

					score = 0.0;
					cv_result = cv_verify_compare_feature(handle_verify, p_feature_1,
						p_feature_2, &score);
					fprintf(stderr, "after serial and deserial the feature compare score is %f  \n", score);
					cin>>string_feature_2;
					//ÊÍ·ÅÌáÈ¡ÈËÁ³ÌØÕ÷ʱ·ÖÅäµÄ¿Õ¼ä
					cv_verify_release_feature(p_feature_new_1);
					cv_verify_release_feature(p_feature_new_2);
				}
				else {
					fprintf(stderr, "error, the feature length is 0!\n");
				}
				// ÊÍ·ÅÌáÈ¡ÈËÁ³ÌØÕ÷ʱ·ÖÅäµÄ¿Õ¼ä
				cv_verify_release_feature(p_feature_1);
				cv_verify_release_feature(p_feature_2);

			}
		}
		else {
			if (face_count_1 == 0) {
				fprintf(stderr, "can't find face in \n");
			}
			if (face_count_2 == 0) {
				fprintf(stderr, "can't find face in \n");
			}
		}
	} while (0);


	// release the memory of face
	cv_face_release_detector_result(p_face_1, face_count_1);
	cv_face_release_detector_result(p_face_2, face_count_2);
	// destroy detect handle
	cv_face_destroy_detector(handle_detect);
	// destroy verify handle
	cv_verify_destroy_handle(handle_verify);
	fprintf(stderr, "test finish!\n");
	return 0;
}

int main() {

	//ͼƬÈë¿Ú£¬ÏÖÐè¾­¹ýOPENCV´¦Àíºó±»sdkʹÓá£
	char* input_image_path = "../../test_image/face_06.jpg";
	char* output_image_path = "../../test_image/face_06out.jpg";

	// ´´½¨ÈËÁ³ÑéÖ¤¾ä±ú²¢³õʼ»¯
	//º¯Êý·µ»ØµÄ´íÎó´úÂëÀàÐÍ
	cv_result_t cv_result = CV_OK;
	cv_handle_t handle_verify =NULL;
	cv_result = cv_verify_create_handle(&handle_verify, "../../../models/verify.model");
	if (cv_result != CV_OK)
	{
		fprintf(stderr, "fail to init verify handle, error code %d\n", cv_result);

	}

	
	//-------²âÊÔÈËÁ³ ʶ±ðλÖà start-------
	Mat bgr_image_1 = imread(input_image_path);
	if (!bgr_image_1.data) {
	fprintf(stderr, "fail to read %s\n", input_image_path);
	return -1;
	}else
	{
	testface_detect(bgr_image_1,output_image_path);
	}
	//-------²âÊÔÈËÁ³ ʶ±ðλÖà end-------
/*
	//-------²âÊÔÈËÁ³ ÑéÖ¤ start-------
	output_image_path = "../../test_image/face_04.jpg";
	Mat bgr_image_2 = imread(output_image_path);
	if (!bgr_image_2.data) {
	fprintf(stderr, "fail to read %s\n", output_image_path);
	return -1;
	}else
	{
	testface_verify(bgr_image_color,bgr_image_2);
	}
	//-------²âÊÔÈËÁ³ ÑéÖ¤ end-------
	

	//-------²âÊÔÌáÈ¡ÈËÁ³ÌØÕ÷Öµ start-------
	Mat bgr_image_1 = imread(input_image_path);
	char *string_feature;

	if (!bgr_image_1.data) {
		fprintf(stderr, "fail to read %s\n", input_image_path);
		return -1;
	}else
	{
		string_feature=getFeature(bgr_image_1,handle_verify);
	}
	cout<<string_feature<<endl;
	//-------²âÊÔÌáÈ¡ÈËÁ³ÌØÕ÷Öµ end-------
*/
	//




	cin>>cv_result;
	Sleep(100);
	return 0;
}