#include "YoloDetector.h"
#include <glog/logging.h>

void YoloDetector::free_img(image_t m)
{
    if (m.data)
    {
        free(m.data);
    }
}

Error_manager YoloDetector::set_region(float minx, float maxx, float miny, float maxy,float freq)
{
    if(m_maxx<=m_minx || m_maxy<=m_miny || m_freq<=0)
    {
        char description[255]={0};
        sprintf(description,"set yolo parameter l:%.3f  r:%.3f  t:%.3f  b:%.3f",minx,maxx,miny,maxy);
        return Error_manager(LOCATER_YOLO_PARAMETER_INVALID,NORMAL,description);
    }
    m_minx = minx;
    m_maxx = maxx;
    m_miny = miny;
    m_maxy = maxy;
    m_freq = freq;
    return SUCCESS;
}

YoloDetector::YoloDetector(std::string cfg, std::string weights, float minx, float maxx, float miny, float maxy,float freq)
{
#if YOLO_API_USED
	m_pDetector = new Detector(cfg, weights);
	m_minx = minx;
	m_maxx = maxx;
	m_miny = miny;
	m_maxy = maxy;
	m_freq = freq;
	//LOG(INFO) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
#endif
}
YoloDetector::~YoloDetector()
{
#if YOLO_API_USED
	if (m_pDetector)
	{
		delete m_pDetector;
		m_pDetector = NULL;
	}
#endif
}

Error_manager YoloDetector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir)
{
	boxes.clear();
#if YOLO_API_USED
	/*int w = int((m_maxx - m_minx) / m_freq);
	int h = int((m_maxy - m_miny) / m_freq);*/
	int w = 416;
	int h = 208;
	

	cv::Mat img = cv::Mat::zeros(cv::Size(w, h * 2), CV_8UC1);
	for (int i = 0; i < cloud_in->size(); ++i)
	{
		pcl::PointXYZ point = cloud_in->points[i];

		float x = point.x;
		float y = point.y;
		float z = point.z;
		if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.&&z>50.)
		{
			int c = int((x - m_minx) / (m_maxx-m_minx)*w);
			int r = int((y - m_miny) / (m_maxy-m_miny)*h);
			int gray = int(z / 2000. * 256);
			if (gray > img.at<uchar>(r, c))
				img.at<uchar>(r, c) = gray;
		}

	}

	if (m_pDetector)
	{
		boxes.clear();
		cv::cvtColor(img,img,CV_GRAY2RGB);
        std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
        std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
        //free_img(*img_t);
		cv::Mat rgb=img.clone();
		float ext = 0.0;
		
		for (int i = 0; i < rects.size(); ++i)
		{
			/*bbox_t box = rects[i];
			bbox_t box_t = box;
			float len = std::min(box.w*m_freq, box.h*m_freq);
			box_t.x = int(box.x*m_freq + m_minx - ext*len);
			box_t.y = int(box.y*m_freq + m_miny - ext*len);
			box_t.w = int(box.w*m_freq*(1.0+2*ext));
			box_t.h = int(box.h*m_freq*(1.0+2*ext));*/

			bbox_t box = rects[i];
			YoloBox box_y;
            box_y.x = (float(box.x) / float(w)*(m_maxx - m_minx) + m_minx);
            box_y.y = (float(box.y) /float(h)*(m_maxy - m_miny) + m_miny);
            box_y.w = (float(box.w)/float(w)*(m_maxx - m_minx));
            box_y.h = (float(box.h)/float(h)*(m_maxy - m_miny));

			//LOG(WARNING) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
			//LOG(WARNING) << "box " << i << " x:" << box.x << " ,y:" << box.y << " ,w:" << box.w << " ,h:" << box.h;
			//LOG(INFO) << "yolo box " << i << " x:" << box_y.x << " ,y:" << box_y.y << " ,w:" << box_y.w << " ,h:" << box_y.h;

			boxes.push_back(box_y);
			//����boxes

			cv::rectangle(rgb, cv::Point(box.x, box.y),
				cv::Point(box.x + box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);

		}

		char num[32] = { 0 };
		sprintf(num, " size=%d", rects.size());
		cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
		cv::imwrite(save_dir + "/yolo.jpg", rgb);
		if (boxes.size() > 0)
			return SUCCESS;
		else
			return Error_manager(LOCATER_YOLO_DETECT_NO_TARGET,NORMAL,"yolo detect no car");

	}
#endif
	return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"YOLO detect failed");
}


Error_manager YoloDetector::detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir)
{
#if YOLO_API_USED
	int w = int((m_maxx - m_minx) / m_freq);
	int h = int((m_maxy - m_miny) / m_freq);

	cv::Mat img = cv::Mat::zeros(cv::Size(w, h*2), CV_8UC1);
	for (int i = 0; i < l.rows; ++i)
	{
		for (int j = 0; j < l.cols; ++j)
		{
			cv::Vec3f vec_l = l.at<cv::Vec3f>(i, j);
			cv::Vec3f vec_r = r.at<cv::Vec3f>(i, j);
	
			float x = vec_l[0];
			float y = vec_l[1];
			float z = vec_l[2];
			if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
			{
				int c = int((x - m_minx) / m_freq);
				int r = int((y - m_miny) / m_freq);
				int gray = int(z / 2000. * 256);
				if (gray > img.at<uchar>(r, c))
					img.at<uchar>(r, c) = gray;
			}

			x = vec_r[0];
			y = vec_r[1];
			z = vec_r[2];
			if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
			{
				int c = int((x - m_minx) / m_freq);
				int r = int((y - m_miny) / m_freq);
				int gray = int(z / 2000. * 256);
				if (gray > img.at<uchar>(r, c))
					img.at<uchar>(r, c) = gray;
			}
		}
	}

	if (m_pDetector)
	{
		boxes.clear();
		std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
		std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
		free_img(*img_t);
		cv::Mat rgb;
		cv::cvtColor(img, rgb, CV_GRAY2RGB);
		for (int i = 0; i < rects.size(); ++i)
		{
			bbox_t box = rects[i];
            YoloBox box_t;
			box_t.x = (box.x*m_freq+m_minx);
			box_t.y = (box.y*m_freq+m_miny);
			box_t.w = (box.w*m_freq);
			box_t.h = (box.h*m_freq);
			boxes.push_back(box_t);
			//����boxes
			
			cv::rectangle(rgb, cv::Point(box.x, box.y ),
				cv::Point(box.x+box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);
			
		}
		char num[32] = { 0 };
		sprintf(num, " size=%d", rects.size());
		cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
		cv::imwrite(save_dir + "/yolo.jpg", rgb(cv::Rect(0,0,rgb.cols,rgb.rows/2)));
		return SUCCESS;

	}
#endif
	return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"yolo api not define,(no code)");

}