#include <vector>
#include <stdio.h>
#include <cv_face.h>

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
	if (argc < 2) {
		fprintf(stderr, "test_sample_face_track [alignment point size(21 or 106)] [detect face cont limit]\n");
		fprintf(stderr, "for example: \"test_sample_face_track 21 1\"\n");
		return -1;
	}

	VideoCapture capture;
	capture.open(0);         // open the camera
	if (!capture.isOpened()) {
		fprintf(stderr, "can not open camera!\n");
		return -1;
	}
	namedWindow("TrackingTest");
	int frame_width = capture.get(CV_CAP_PROP_FRAME_WIDTH);
	int frame_height = capture.get(CV_CAP_PROP_FRAME_HEIGHT);

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

	cv_handle_t handle_track = NULL;
	cv_result_t cv_result = CV_OK;
	do {
		// init handle
		cv_result = cv_face_create_tracker(&handle_track, NULL, config | CV_FACE_TRACKING_TWO_THREAD);
		if (cv_result != CV_OK) {
			fprintf(stderr, "cv_face_create_tracker failed, error code %d\n", cv_result);
			break;
		}

		if (argc == 3) {
			int detect_face_cnt_limit = atoi(argv[2]);
			if (detect_face_cnt_limit < -1) {
				detect_face_cnt_limit = -1;
			}
			int val = 0;
			cv_result = cv_face_track_set_detect_face_cnt_limit(handle_track, detect_face_cnt_limit, &val);
			if (cv_result != CV_OK) {
				fprintf(stderr, "cv_face_track_set_detect_face_cnt_limit failed, error : %d\n", cv_result);
				break;
			} else {
				fprintf(stderr, "detect face count limit : %d\n", val);
			}
		}

		Mat bgr_frame;
		cv_face_t *p_face = NULL;
		int face_count = 0;
		while (capture.read(bgr_frame)) {       // CV_PIX_FMT_BGR888
			resize(bgr_frame, bgr_frame, Size(frame_width, frame_height), 0, 0,
			       INTER_LINEAR);
			// realtime track
			face_count = 0;
			cv_result = cv_face_track(handle_track, bgr_frame.data, CV_PIX_FMT_BGR888,
			                          bgr_frame.cols, bgr_frame.rows, bgr_frame.step,
			                          CV_FACE_UP, &p_face, &face_count);
			if (cv_result != CV_OK) {
				fprintf(stderr, "cv_face_track failed, error : %d\n", cv_result);
				cv_face_release_tracker_result(p_face, face_count);
				break;
			}

			for (int i = 0; i < face_count; i++) {
				fprintf(stderr, "face: %d-----[%d, %d, %d, %d]-----id: %d\n", i,
				        p_face[i].rect.left, p_face[i].rect.top,
				        p_face[i].rect.right, p_face[i].rect.bottom, p_face[i].ID);
				fprintf(stderr, "face pose: [yaw: %.2f, pitch: %.2f, roll: %.2f, eye distance: %.2f]\n",
				        p_face[i].yaw,
				        p_face[i].pitch, p_face[i].roll, p_face[i].eye_dist);

				// draw the video
				Scalar scalar_color = CV_RGB(p_face[i].ID * 53 % 256,
				                             p_face[i].ID * 93 % 256,
				                             p_face[i].ID * 143 % 256);
				rectangle(bgr_frame, Point2f(static_cast<float>(p_face[i].rect.left),
				                             static_cast<float>(p_face[i].rect.top)),
				          Point2f(static_cast<float>(p_face[i].rect.right),
				                  static_cast<float>(p_face[i].rect.bottom)), scalar_color, 2);
				for (int j = 0; j < p_face[i].points_count; j++) {
					circle(bgr_frame, Point2f(p_face[i].points_array[j].x,
					                          p_face[i].points_array[j].y), 1, Scalar(0, 255, 0));
				}
			}

			// release the memory of face
			cv_face_release_tracker_result(p_face, face_count);
			imshow("TrackingTest", bgr_frame);
			if (waitKey(1) != -1)
				break;
		}

	} while (0);

	// destroy track handle
	cv_face_destroy_tracker(handle_track);

	return 0;
}