#include "PL_OpenCV_HOG_SVM_Detector.h"
#include "MaterialBuffer.h"
#include "logger.h"

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/objdetect/objdetect.hpp>

//#include <libyuv.h>

#include <vector>

struct PL_OpenCV_HOG_SVM_Detector_Internal
{
	//uint8_t buffer[1920*1080*4];
	//size_t buffSize;
	//size_t buffSizeMax;
	MB_Frame lastFrame;
	PL_OpenCV_HOG_SVM_Detector_Config config;

	bool payError;
	
	cv::HOGDescriptor hogDescriptor;

	PL_OpenCV_HOG_SVM_Detector_Internal() : 
		//buffSize(0), buffSizeMax(sizeof(buffer)), 
		lastFrame(), config(), payError(true), 
		hogDescriptor()
	{
	}
	
	~PL_OpenCV_HOG_SVM_Detector_Internal()
	{
	}
	
	void reset()
	{
		//buffSize = 0;
		payError = true;
		
		MB_Frame _lastFrame;
		lastFrame = _lastFrame;
		PL_OpenCV_HOG_SVM_Detector_Config _config;
		config = _config;
		
		cv::HOGDescriptor _hogDescriptor;
		hogDescriptor = _hogDescriptor;
	}
};

PipeLineElem* create_PL_OpenCV_HOG_SVM_Detector()
{
	return new PL_OpenCV_HOG_SVM_Detector;
}

PL_OpenCV_HOG_SVM_Detector::PL_OpenCV_HOG_SVM_Detector() : internal(new PL_OpenCV_HOG_SVM_Detector_Internal)
{
}

PL_OpenCV_HOG_SVM_Detector::~PL_OpenCV_HOG_SVM_Detector()
{
	delete (PL_OpenCV_HOG_SVM_Detector_Internal*)internal;
	internal= nullptr;
}

bool PL_OpenCV_HOG_SVM_Detector::init(void* args)
{
	PL_OpenCV_HOG_SVM_Detector_Internal* in = (PL_OpenCV_HOG_SVM_Detector_Internal*)internal;
	in->reset();
	
	PL_OpenCV_HOG_SVM_Detector_Config* config = (PL_OpenCV_HOG_SVM_Detector_Config*)args;
	if (config != nullptr)
		in->config = *config;

    in->hogDescriptor.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
	
	return true;
}

void PL_OpenCV_HOG_SVM_Detector::finit()
{
	PL_OpenCV_HOG_SVM_Detector_Internal* in = (PL_OpenCV_HOG_SVM_Detector_Internal*)internal;

}

uint64_t time_msec()
{
	timeval tv;
	gettimeofday(&tv, nullptr);
	
	return (tv.tv_sec * 1000 * 1000 + tv.tv_usec) / 1000;
}

int doFaceTrack(PL_OpenCV_HOG_SVM_Detector_Internal* in, 
				uint8_t* buffer, size_t width, size_t height, MB_Frame::MBFType pixFmt)
{
	using namespace cv;

	Mat img(cv::Size(width,height), CV_8UC1, buffer);
	
	std::vector<Rect> found, found_filtered;
	double t = (double)getTickCount();
	// run the detector with default parameters. to get a higher hit-rate
	// (and more false alarms, respectively), decrease the hitThreshold and
	// groupThreshold (set groupThreshold to 0 to turn off the grouping completely).
	in->hogDescriptor.detectMultiScale(img, found, 0, Size(8,8), Size(32,32), 1.05, 2);
	t = (double)getTickCount() - t;
	printf("tdetection time = %gms\n", t*1000./cv::getTickFrequency());
	size_t i, j;
	for( i = 0; i < found.size(); i++ )
	{
		Rect r = found[i];
		for( j = 0; j < found.size(); j++ )
		{
			if( j != i && (r & found[j]) == r)
				break;
		}
		if( j == found.size() )
			found_filtered.push_back(r);
	}
	for( i = 0; i < found_filtered.size(); i++ )
	{
		Rect r = found_filtered[i];
		// the HOG detector returns slightly larger rectangles than the real objects.
		// so we slightly shrink the rectangles to get a nicer output.
		r.x += cvRound(r.width*0.1);
		r.width = cvRound(r.width*0.8);
		r.y += cvRound(r.height*0.07);
		r.height = cvRound(r.height*0.8);
		rectangle(img, r.tl(), r.br(), cv::Scalar(0,255,0), 3);
	}
	LOG_ERROR << "found_filtered=" << found_filtered.size()<< LOG_ENDL;
return 1;
}

bool PL_OpenCV_HOG_SVM_Detector::pay(const PipeMaterial& pm)
{
	PL_OpenCV_HOG_SVM_Detector_Internal* in = (PL_OpenCV_HOG_SVM_Detector_Internal*)internal;

	if (pm.type != PipeMaterial::PMT_FRAME)
	{
		LOG_ERROR << "Only support PMT_FRAME" << LOG_ENDL;
		return false;
	}
	
	if (pm.buffer == nullptr)
		return false;
	
	MB_Frame* frame = (MB_Frame*)pm.buffer;
	if (frame->type != MB_Frame::MBFT_YUV420)
	{
		LOG_ERROR << "Only support MBFT_YUV420" << LOG_ENDL;
		return false;
	}

	int face_count = doFaceTrack(
						in, (uint8_t*)frame->buffer, frame->width, frame->height, MB_Frame::MBFT_YUV420);
	if (face_count < 0)
	{
		in->payError = true;
		return false;
	}
	else
		in->payError = false;
	
	//in->buffer readly
	in->lastFrame.type = MB_Frame::MBFT_YUV420;
	in->lastFrame.buffer = frame->buffer;//#todo should copy
	in->lastFrame.buffSize = frame->buffSize;
	in->lastFrame.width = frame->width;
	in->lastFrame.height = frame->height;
	in->lastFrame.pts = frame->pts;
	gettimeofday(&(in->lastFrame.pts),NULL);

	return true;
}

bool PL_OpenCV_HOG_SVM_Detector::gain(PipeMaterial& pm)
{
	PL_OpenCV_HOG_SVM_Detector_Internal* in = (PL_OpenCV_HOG_SVM_Detector_Internal*)internal;

	if (!in->payError)
	{
		pm.type = PipeMaterial::PMT_FRAME;
		pm.buffer = &(in->lastFrame);
		pm.buffSize = 0;
		pm.former = this;
	}
	pm.former = this;
	return !in->payError;
}