#include "fork_rs.h"

namespace graft_cv{

	CForkDetectOptimal::CForkDetectOptimal()
		:m_roi_width(0),m_roi_height(0)
	{
	}
	CForkDetectOptimal::CForkDetectOptimal(int w,int h)
		:m_roi_width(w),m_roi_height(h)
	{
	}
	CForkDetectOptimal::~CForkDetectOptimal(){}
	void CForkDetectOptimal::set_roi_size(int w,int h){
		m_roi_width = w;
		m_roi_height = h;
	}
	void CForkDetectOptimal::get_roi_size(int &w,int &h){
		w = m_roi_width;
		h = m_roi_height;
	}

	double CForkDetectOptimal::center_pt_index(
		const cv::Mat& binimg,
		cv::Point&cent,
		double cut_angle //刀具切割角度
		)
	{
		double ratio = 0.0;
		cv::RotatedRect centers_rect = cv::RotatedRect(cv::Point2f(0,0), cv::Size2f(m_roi_width,m_roi_height), -cut_angle);//换算成顺时针角度
		cv::Point2f vertices[4];
		 centers_rect.points(vertices);

		 cv::Point2f left_cent  = cv::Point2f(cent.x + vertices[0].x, cent.y + vertices[0].y);
		 cv::Point2f right_cent = cv::Point2f(cent.x + vertices[1].x, cent.y + vertices[1].y);
		 cv::RotatedRect left_rect = cv::RotatedRect(left_cent, cv::Size2f(m_roi_width,m_roi_height), -cut_angle);
		 cv::RotatedRect right_rect = cv::RotatedRect(right_cent, cv::Size2f(m_roi_width,m_roi_height), -cut_angle);

		 double left_cnt = roi_object_size(binimg, left_rect);
		 double right_cnt = roi_object_size(binimg, right_rect);
		 ratio = left_cnt* left_cnt/right_cnt/(m_roi_width*m_roi_height);

		 //imshow
		 cv::Mat tmp_img = binimg.clone();
		 cv::Point2f vertices_left[4];
		 cv::Point2f vertices_right[4];
		 left_rect.points(vertices_left);
		 right_rect.points(vertices_right);
		 for (int i = 0; i < 4; i++){
			 cv::line(tmp_img, vertices_left[i], vertices_left[(i+1)%4], cv::Scalar(128,255,0), 1);
			 cv::line(tmp_img, vertices_right[i], vertices_right[(i+1)%4], cv::Scalar(200,255,0), 1);
		 }
		 circle(tmp_img,cent,5, cv::Scalar(128,0,0));

		 return ratio;
	}

	double CForkDetectOptimal::roi_object_size(
		const cv::Mat& binimg,
		cv::RotatedRect &rrect)
	{
		double cnt = 1.0;
		cv::Rect brect = rrect.boundingRect();
		cv::Point2f vertices[4];
		rrect.points(vertices);

		//Mat tmp_img = Mat::zeros(binimg.rows,binimg.cols,CV_8UC1);
		for(int r = brect.y; r<brect.y+brect.height;++r){
			for(int c=brect.x; c<brect.x+brect.width;++c){
				cv::Point2f ptf = cv::Point2f(c,r);
				bool isin = is_in_rrect(vertices,ptf, rrect.angle);
				if(isin){
					//tmp_img.at<unsigned char>(r,c) = 255;
					if(r<0||r>binimg.rows-1){continue;}
					if(c<0||c>binimg.cols-1){continue;}
					if(binimg.at<unsigned char>(r,c) >0){cnt+=1.0;}
				}
			}
		}
		return cnt;		 
	}
	bool CForkDetectOptimal::is_in_rrect(
		cv::Point2f* ptf4,
		cv::Point2f ptf,
		float fAngle)
	{
		cv::Point2f ptf4Vector[4];
		int nQuadrant[4] = {0};
		fAngle *= CV_PI/180 *(-1);

		for(int idx=0;idx<4;idx++){
			float fDifx = float(ptf.x - ptf4[idx].x);
			float fDify = float(ptf.y - ptf4[idx].y);
			int nDifx = fDifx * cos(fAngle) - fDify * sin(fAngle);
			int nDify = fDifx * sin(fAngle) + fDify * cos(fAngle);

			if(nDifx >=0 && nDify >=0) nQuadrant[0]++;
			if(nDifx < 0 && nDify >=0) nQuadrant[1]++;
			if(nDifx < 0 && nDify < 0) nQuadrant[2]++;
			if(nDifx > 0 && nDify < 0) nQuadrant[3]++;

		}
		int firstIdx = -1;
		int secIdx = -1;
		int countNum = 0;
		for(int idx=0;idx<4;idx++){
			if(nQuadrant[idx] !=0){
				if(firstIdx == -1){
					firstIdx = idx;
				}
				else{
					if(secIdx == -1 && firstIdx != -1){
						secIdx = idx;
					}
				}
				countNum++;
			}
		}
		if(countNum <= 2){
			if(abs(firstIdx - secIdx)==1 || abs(firstIdx - secIdx)==3 ||
				(countNum==1 && (firstIdx==-1 || secIdx==-1)))
				{
					return false;
			}
		}
		return true;
	}



};