#include <opencv.hpp>
#include <math.h>
#include <io.h>
#include "tea_sorter.h"
#include "utils.h"



using namespace cv;

namespace graft_cv{

CTeaSort::CTeaSort(
	ConfigParam& cp,	
	img_type dtpye,
	CGcvLogger*pLog)
:
m_cp(cp),  
m_dtype(dtpye),
m_pLogger(pLog),
m_ppImgSaver(0),
m_pImginfoRaw(0),
m_pImginfoDetected(0)
{
	m_drop_detector = RetinaDrop(m_pLogger, 0.5, 0.5);
}

CTeaSort::~CTeaSort()
{
	clear_imginfo();
}

int CTeaSort::detect(
	ImgInfo*imginfo,
	PositionInfo& posinfo, 
	const char* fn
	)
{	
	//1 model status
	if (!m_drop_detector.IsModelLoaded()) {
		m_pLogger->ERRORINFO(
			string("tea detect model NOT loaded"));
		return 1;
	}
	//2 update recognize threshold
	if (m_dtype == img_type::tea_grab) {
		m_drop_detector.SetThreshold(m_cp.object_threshold_grab, m_cp.nms_threshold_grab);
	}
	else {
		m_drop_detector.SetThreshold(m_cp.object_threshold_cut, m_cp.nms_threshold_cut);
	}


	//3 load data
	load_data(imginfo, fn);
	if (m_cp.image_show) {
		cv::destroyAllWindows();
		imshow_wait("input_img", m_raw_img);
	}

	//4 generate_detect_windows(vector<Rect>&boxes)
	vector<Rect> drop_regions;
	int region_cnt = generate_detect_windows(drop_regions);
	if (region_cnt == 0) {
		stringstream buff_;
		buff_ << m_imgId << m_dtype_str << "tea detect image regions' size == 0";
		m_pLogger->ERRORINFO(buff_.str());
		return 1;
	}
	else {
		stringstream bufftmp;
		bufftmp << m_imgId << m_dtype_str << "tea detect image regions' size = "<<region_cnt;
		m_pLogger->INFO(bufftmp.str());
	}
	if (m_cp.image_show) {
		cv::Mat rects_img = m_raw_img.clone();
		int step_c = int(255 / (float)region_cnt);
		int step_cc = step_c / 2;
		int step_ccc = step_cc / 2;
		int cnt = 0;
		for (auto&r : drop_regions) {
			cv::rectangle(rects_img, r, cv::Scalar(step_cc*cnt, step_c*cnt, step_ccc*cnt), 3);
			cnt += 1;
		}
		imshow_wait("regions_img", rects_img);
	}

	//5 detect	
	vector<Bbox> droplets_raw;
	int dn = detect_impl(m_raw_img, drop_regions, droplets_raw);
	if (dn < 2 && m_dtype == img_type::tea_grab) {
		//up-down flip
		cv::Mat flip_img;
		cv::flip(m_raw_img, flip_img, 0);
		if (m_cp.image_show) {			
			imshow_wait("flip_img", flip_img);
		}

		vector<Bbox> droplets_flip;
		int dn_flip = detect_impl(flip_img, drop_regions, droplets_flip);
		for (auto&b: droplets_flip) {			
			int y2 = flip_img.rows - b.y1;
			int y1 = flip_img.rows - b.y2;
			b.y1 = y1;
			b.y2 = y2;
			
			for (int i = 0; i < 5; ++i) {				
				b.ppoint[2 * i + 1] = flip_img.rows - b.ppoint[2 * i + 1];
			}
		}
		if (dn_flip > 0) {
			droplets_raw.insert(
				droplets_raw.end(),
				droplets_flip.begin(),
				droplets_flip.end());
		}
	}
	/*for (auto rect : drop_regions) {
		Mat roi = m_raw_img(rect);
		vector<Bbox> head_droplets = m_drop_detector.RunModel(roi, m_pLogger);
		if (m_pLogger) {
			stringstream buff_;
			buff_ << m_imgId << m_dtype_str << "-------crop_rect["<< rect.x<<","<<rect.y<<","<<rect.width
				<<","<<rect.height<<"],"
				<<" roi image detect over. tea number is " << head_droplets.size();
			m_pLogger->INFO(buff_.str());
		}
		for (Bbox& b : head_droplets) {
			b.x1 += rect.x;
			b.x2 += rect.x;
			b.y1 += rect.y;
			b.y2 += rect.y;
			for (int i = 0; i < 5; ++i) {
				b.ppoint[2 * i] += rect.x;
				b.ppoint[2 * i + 1] += rect.y;
			}			
		}

		if (head_droplets.size()) {
			droplets_raw.insert(
				droplets_raw.end(),
				head_droplets.begin(),
				head_droplets.end());
		}
	}*/
	if (m_pLogger) {		
		stringstream buff_;
		buff_ << m_imgId<<m_dtype_str << "image detect over. tea number is " << droplets_raw.size();
		m_pLogger->INFO(buff_.str());
	}
	//6 nms, width(height) filt and area calculation
	vector<Bbox> droplets;
	vector<int> keep;
	nms_bbox(droplets_raw, m_drop_detector.GetNmsThreshold(), keep);
	//nms keep and area filter
	double min_area_th = m_cp.min_area_ratio_grab;
	double max_area_th = m_cp.max_area_ratio_grab;
	if (m_dtype == img_type::tea_cut) {
		min_area_th = m_cp.min_area_ratio_cut;
		max_area_th = m_cp.max_area_ratio_cut;
	}
	for (int i : keep) {
		Bbox&tbox = droplets_raw[i];
		double area_ratio = static_cast<double>(tbox.y2 - tbox.y1) * static_cast<double>(tbox.x2 - tbox.x1);
		area_ratio = fabs(area_ratio);
		area_ratio /= static_cast<double>(m_raw_img.rows);
		area_ratio /= static_cast<double>(m_raw_img.cols);
		tbox.area = area_ratio;
		if (area_ratio < min_area_th || area_ratio > max_area_th) { continue; }
		//检查box边界是否在图像内，如果没有，修改之
		if (tbox.x1 < 0) { tbox.x1 = 0; }
		if (tbox.y1 < 0) { tbox.y1 = 0; }
		if (tbox.x2 >= m_raw_img.cols) { tbox.x2 = m_raw_img.cols - 1; }
		if (tbox.y2 >= m_raw_img.rows) { tbox.y2 = m_raw_img.rows - 1; }

		droplets.push_back(tbox);
	}
	if (m_pLogger) {
		stringstream buff_;
		buff_ << m_imgId << m_dtype_str << "after nms, keep tea number is " << droplets.size();
		for (auto&tbox : droplets) {
			buff_ << "\nscore:" << tbox.score << ", area_ratio:" << tbox.area << ", left_top:(" << tbox.x1 << "," << tbox.y1 << "), bottom_rigt:(" << tbox.x2 << "," << tbox.y2 << ")";
		}
		m_pLogger->INFO(buff_.str());		
	}
	
	
	int valid_cnt = 0;
	if (m_dtype == img_type::tea_grab) {
		//grab
		double pre_cx, pre_cy;
		double min_dist_grab = m_cp.min_distance_grab;
		pre_cx = -min_dist_grab;
		pre_cy = -min_dist_grab;

		for (int i = 0; i < droplets.size(); ++i) {
			if (valid_cnt > 1) { break; }
			Bbox&b = droplets.at(i);
			double cx = 0.5*(b.x1 + b.x2);
			double cy = 0.5*(b.y1 + b.y2);
			double dist = sqrt((cx - pre_cx)*(cx - pre_cx) + (cy - pre_cy)*(cy - pre_cy));
			if (dist < min_dist_grab) {
				continue;
			}
			double grab_x, grab_y;
			double angle = calculate_angle(b,/* true, */grab_x, grab_y);
			
			//grab point 
			if (valid_cnt == 0) {
				posinfo.tea_grab_x1 = grab_x;
				posinfo.tea_grab_y1 = grab_y;
				posinfo.tea_grab_angle1 = angle;				
			}
			else {
				posinfo.tea_grab_x2 = grab_x;
				posinfo.tea_grab_y2 = grab_y;
				posinfo.tea_grab_angle2 = angle;
			}
			pre_cx = cx;
			pre_cy = cy;
			b.status = 1;
			valid_cnt += 1;
		}
	}
	else {
		//cut
		for (int i = 0; i < droplets.size();++i) {
			if (i > 1) { break; }
			Bbox&b = droplets.at(i);
			b.status = 1; // selected
			double grab_x, grab_y;
			double angle = calculate_angle(b,/* true,*/ grab_x, grab_y);
			valid_cnt += 1;			
			if (i == 0) {				
				// 切割点是3、4的中间的点
				posinfo.tea_cut_x1 = 0.5 * (b.ppoint[4] + b.ppoint[6]);
				posinfo.tea_cut_y1 = 0.5 * (b.ppoint[5] + b.ppoint[7]);
				posinfo.tea_cut_angle1 = angle;						
			}
			else {				
				// 切割点是3、4的中间的点
				posinfo.tea_cut_x2 = 0.5 * (b.ppoint[4] + b.ppoint[6]);
				posinfo.tea_cut_y2 = 0.5 * (b.ppoint[5] + b.ppoint[7]);
				posinfo.tea_cut_angle2 = angle;
			}		
		}
	}
	

	//6 draw
	if (m_cp.image_return) {
		this->clear_imginfo();
		cv::Mat img_rst = m_raw_img.clone();		
		for (auto& b : droplets) {			
			//rectangle
			cv::Rect r = cv::Rect(cv::Point2i(b.x1, b.y1), cv::Point2i(b.x2, b.y2));
			if (b.status > 0) {
				cv::rectangle(img_rst, r, cv::Scalar(0, 0, 255),2);
			}
			else {
				cv::rectangle(img_rst, r, cv::Scalar(0, 255, 0),2);
			}
			//score
			char name[256];
			cv::Scalar color(120, 120, 0);//bgr
			
			sprintf_s(name, "%.2f", b.score);
			cv::putText(img_rst, name,
				cv::Point(b.x1, b.y1),
				cv::FONT_HERSHEY_COMPLEX, 0.7, color, 2);

			//points
			cv::circle(img_rst, cv::Point(int(b.ppoint[0]), int(b.ppoint[1])), 4, cv::Scalar(255, 0, 255), -1, 3, 0);
			cv::circle(img_rst, cv::Point(int(b.ppoint[2]), int(b.ppoint[3])), 4, cv::Scalar(0, 255, 255), -1, 3, 0);
			cv::circle(img_rst, cv::Point(int(b.ppoint[4]), int(b.ppoint[5])), 4, cv::Scalar(255, 0, 0), -1, 3, 0);
			cv::circle(img_rst, cv::Point(int(b.ppoint[6]), int(b.ppoint[7])), 4, cv::Scalar(0, 255, 0), -1, 3, 0);
			cv::circle(img_rst, cv::Point(int(b.ppoint[8]), int(b.ppoint[9])), 4, cv::Scalar(0, 0, 255), -1, 3, 0);
			
			//grab points
			if (m_dtype == img_type::tea_grab) {
				double grab_x, grab_y;
				//bool need_precise = b.status == 1;
				double grab_angle = calculate_angle(b, /*need_precise,*/ grab_x, grab_y);
				//cv::circle(img_rst, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(0, 215, 255), -1, 3, 0);
				//lines, p4-p5, p5-grab
				cv::line(img_rst, 
					cv::Point(int(b.ppoint[6]), int(b.ppoint[7])), 
					cv::Point(int(b.ppoint[8]), int(b.ppoint[9])),
					cv::Scalar(0, 215, 255), 2);
				cv::line(img_rst, 
					cv::Point(int(b.ppoint[8]), int(b.ppoint[9])), 
					cv::Point(int(grab_x), int(grab_y)),
					cv::Scalar(0, 215, 255), 2);
				//line x
				int radius = 20;
				int cx = int(grab_x);
				int cy = int(grab_y);
				cv::line(img_rst, cv::Point(cx - radius, cy - radius), cv::Point(cx + radius, cy + radius), cv::Scalar(0, 215, 255), 2);
				cv::line(img_rst, cv::Point(cx - radius, cy + radius), cv::Point(cx + radius, cy - radius), cv::Scalar(0, 215, 255), 2);

				//grab point angle
				int radius_dir = m_cp.offset_grab / 2;
				grab_angle *= (CV_PI / 180.0);
				double dx = radius_dir * sin(grab_angle);
				double dy = radius_dir * cos(grab_angle);
				int dir_x = int(grab_x + dx);
				int dir_y = int(grab_y + dy);
				cv::line(img_rst, cv::Point(cx, cy), cv::Point(dir_x, dir_y), cv::Scalar(20, 255, 20), 2);
			}
			//cut points
			if (m_dtype == img_type::tea_cut) {	
				//lines, p3-p4
				cv::line(img_rst,
					cv::Point(int(b.ppoint[4]), int(b.ppoint[5])),
					cv::Point(int(b.ppoint[6]), int(b.ppoint[7])),					
					cv::Scalar(0, 215, 255), 2);
				//line x
				int cx = int(0.5 * (b.ppoint[4] + b.ppoint[6]));
				int cy = int(0.5 * (b.ppoint[5] + b.ppoint[7]));
				int radius = 20;
				cv::line(img_rst, cv::Point(cx - radius, cy - radius), cv::Point(cx + radius, cy + radius), cv::Scalar(0, 215, 255),2);
				cv::line(img_rst, cv::Point(cx - radius, cy + radius), cv::Point(cx + radius, cy - radius), cv::Scalar(0, 215, 255),2);
			}
		}

		if (m_cp.image_show) {
			imshow_wait("result_img", img_rst);
		}
		m_pImginfoRaw = mat2imginfo(m_raw_img);
		m_pImginfoDetected = mat2imginfo(img_rst);
		posinfo.pp_images[0] = m_pImginfoRaw;
		posinfo.pp_images[1] = m_pImginfoDetected;

		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveImage(img_rst, m_imgId + "_rst_0");			
		}
	}
	//拍照无苗, 返回识别结果-1
	if (valid_cnt != 2) { return -1; }
	return 0;
}

int CTeaSort::detect_impl(
	cv::Mat& raw_img,				//input, image
	vector<Rect>&drop_regions,	//input, detect regions
	vector<Bbox> &droplets_raw	//output, detect result
)
{
	//return number of detect result
	droplets_raw.clear();
	for (auto rect : drop_regions) {
		Mat roi = raw_img(rect);
		vector<Bbox> head_droplets = m_drop_detector.RunModel(roi, m_pLogger);
		if (m_pLogger) {
			stringstream buff_;
			buff_ << m_imgId << m_dtype_str << "-------crop_rect[" << rect.x << "," << rect.y << "," << rect.width
				<< "," << rect.height << "],"
				<< " roi image detect over. tea number is " << head_droplets.size();
			m_pLogger->INFO(buff_.str());
		}
		for (Bbox& b : head_droplets) {
			b.x1 += rect.x;
			b.x2 += rect.x;
			b.y1 += rect.y;
			b.y2 += rect.y;
			for (int i = 0; i < 5; ++i) {
				b.ppoint[2 * i] += rect.x;
				b.ppoint[2 * i + 1] += rect.y;
			}
		}

		if (head_droplets.size()) {
			droplets_raw.insert(
				droplets_raw.end(),
				head_droplets.begin(),
				head_droplets.end());
		}
	}

	return droplets_raw.size();
}
double CTeaSort::calculate_angle(
	Bbox&b,				//input
	//bool need_precise_angle,//input
	double& grab_x,		//output
	double& grab_y		//output
)
{
	grab_x = grab_y = 0.0;
	double angle = 0.0;
	float x3,y3,x4,y4,x5,y5;
	x3 = b.ppoint[4];
	y3 = b.ppoint[5];
	x4 = b.ppoint[6];
	y4 = b.ppoint[7];
	x5 = b.ppoint[8];
	y5 = b.ppoint[9];
	if (m_dtype == img_type::tea_grab) {
		angle = atan2(x5 - x3, y5 - y3);
		//if (need_precise_angle) {
			calculate_stem_grab_position_opt(b, grab_x, grab_y, angle);
			//计算抓取点
			if (grab_x < 0 && grab_y < 0) {
				double pr = (double)m_cp.offset_grab;
				double dx = pr * sin(angle);
				double dy = pr * cos(angle);
				grab_x = x5 + dx;
				grab_y = y5 + dy;
			}
		/*}
		else {
			double pr = (double)m_cp.offset_grab;
			double dx = pr * sin(angle);
			double dy = pr * cos(angle);
			grab_x = x5 + dx;
			grab_y = y5 + dy;
		}*/
	}
	else {
		//tea cut, calculate line of p3 ans p4
		angle = atan2(x3 - x4, y3 - y4);
	}
	
	angle *= (180.0 / 3.1415926);
	return angle;

}

int CTeaSort::load_data(
	ImgInfo*imginfo,
	const char* fn/* = 0*/)
{
	//数据加载功能实现，并生成imageid，保存原始数据到文件
	int rst = 0;
	//generate image id
	if (m_dtype == img_type::tea_grab) {
		m_imgId = getImgId(img_type::tea_grab);
		m_dtype_str = string(" tea_grab ");
	}
	else {
		m_imgId = getImgId(img_type::tea_cut);
		m_dtype_str = string(" tea_cut ");
	}

	if (imginfo) {
		if (m_pLogger) {
			stringstream buff;
			buff << m_imgId << m_dtype_str << "image, width=" << imginfo->width
				<< "\theight=" << imginfo->height;
			m_pLogger->INFO(buff.str());
		}
		if (!isvalid(imginfo) || (imginfo->channel!=1 && imginfo->channel!=3)) {
			if (m_pLogger) {
				m_pLogger->ERRORINFO(m_imgId + m_dtype_str + "input image invalid.");
			}
			throw_msg(m_imgId + " invalid input image");

		}
		if (imginfo->channel == 1) {
			cv::Mat tmp_img = imginfo2mat(imginfo);

			vector<Mat> channels;
			for (size_t i = 0; i < 3; ++i) { channels.push_back(tmp_img); }			
			cv::merge(channels, m_raw_img);
		}
		else {
			m_raw_img = imginfo2mat(imginfo);
		}
		
	}
	else {
		cv::Mat img = imread(fn, cv::IMREAD_COLOR);
		if (img.empty()) {
			if (m_pLogger) {
				m_pLogger->ERRORINFO(m_imgId + m_dtype_str + "input image invalid：" + string(fn));
			}
			throw_msg(m_imgId + m_dtype_str + "invalid input image: " + string(fn));

		}
		if (m_pLogger) {
			stringstream buff;
			buff << m_imgId << m_dtype_str << "image, width=" << img.cols
				<< "\theight=" << img.rows;
			m_pLogger->INFO(buff.str());
		}

		m_raw_img = img.clone();
	}
	//image saver
	if (m_ppImgSaver && *m_ppImgSaver) {
		(*m_ppImgSaver)->saveImage(m_raw_img, m_imgId);
	}
	return rst;
}


int CTeaSort::load_model()
{
	bool b = false;
	if (!m_drop_detector.IsModelLoaded()) {
		if (m_dtype == img_type::tea_grab) {
			b = m_drop_detector.LoadModel(m_cp.model_path_grab);
		}
		else {
			b = m_drop_detector.LoadModel(m_cp.model_path_cut);
		}
		
	}
	else {
		b = true;
	}
	return b ? 0 : 1;
}

void CTeaSort::clear_imginfo() {
	if (m_pImginfoDetected) {
		imginfo_release(&m_pImginfoDetected);
		m_pImginfoDetected = 0;
	}	
	if (m_pImginfoRaw) {
		imginfo_release(&m_pImginfoRaw);
		m_pImginfoRaw = 0;
	}
}
int CTeaSort::generate_detect_windows(vector<Rect>&boxes)
{
	boxes.clear();
	int grid_row = m_cp.grid_row_cut;
	int grid_col = m_cp.grid_col_cut;
	int grid_padding = m_cp.grid_padding_cut;
	if (m_dtype == img_type::tea_grab) {
		grid_row = m_cp.grid_row_grab;
		grid_col = m_cp.grid_col_grab;
		grid_padding = m_cp.grid_padding_grab;
	}
	if (grid_row < 1) {		grid_row = 1;	}
	if (grid_col < 1) {		grid_col = 1;	}
	if (grid_padding < 0) { grid_padding = 0; }

	int block_height = int(m_raw_img.rows / (float)grid_row + 0.5);
	int block_width = int(m_raw_img.cols / (float)grid_col + 0.5);
	for (int r = 0; r < grid_row; ++r) {
		for (int c = 0; c < grid_col; ++c) {
			int x0 = c*block_width - grid_padding;
			int y0 = r*block_height - grid_padding;
			int x1 = (c+1)*block_width + grid_padding;
			int y1 = (r+1)*block_height + grid_padding;

			if (x0 < 0) { x0 = 0; }
			if (y0 < 0) { y0 = 0; }
			if (x1 > m_raw_img.cols) { x1 = m_raw_img.cols; }
			if (y1 > m_raw_img.rows) { y1 = m_raw_img.rows; }
			Rect r(x0, y0, x1-x0, y1-y0);
			boxes.push_back(r);
		}
	}
	return boxes.size();
}

void CTeaSort::calculate_stem_grab_position(
	Bbox&b,
	double& grab_x,		//output
	double& grab_y,		//output
	double& grab_angle //output
)
{

	grab_x = grab_y = -1.0;
	//crop image
	int padding = 2 * m_cp.offset_grab;
	int y3 = int(b.ppoint[5]);	
	int y5 = int(b.ppoint[9]);
	cv::Point p3(int(b.ppoint[4] - b.x1), int(b.ppoint[5] - b.y1));
	cv::Point p4(int(b.ppoint[6] - b.x1), int(b.ppoint[7] - b.y1));
	cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
	cv::Mat crop_img;
	if (y5 > y3) {
		// Y position	
		int ymax = b.y2 + padding;
		if (ymax > m_raw_img.rows) { 
			ymax = m_raw_img.rows; 
		}
		crop_img = m_raw_img(cv::Range(b.y1, ymax), cv::Range(b.x1, b.x2)).clone();
	}
	else {
		// ^ position
		if (b.y1 - padding < 0) {
			padding = b.y1;
		}
		p5.y = int(b.ppoint[9] - b.y1 + padding);
		p4.y = int(b.ppoint[7] - b.y1 + padding);
		p3.y = int(b.ppoint[5] - b.y1 + padding);
		crop_img = m_raw_img(cv::Range(b.y1 - padding, b.y2), cv::Range(b.x1, b.x2)).clone();
		
	}
	if (m_cp.image_show) {
		cv::Mat crop_img_tmp = crop_img.clone();
		cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
		cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
		cv::circle(crop_img_tmp, p5, 4, cv::Scalar(0, 0, 255), -1, 3, 0);

		imshow_wait("cropped box", crop_img_tmp);
	}

	//to gray
	cv::Mat gray_img;
	if (crop_img.channels() == 1) { gray_img = crop_img; }
	else {
		cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
	}
	//binary
	cv::Mat bin_img;
	double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
	cv::bitwise_not(bin_img, bin_img);
	if (m_cp.image_show) {
		imshow_wait("cropped binary img", bin_img);
	}

	// skeletonize() or medial_axis()	
	cv::Mat ske_img;
	thinning(bin_img, ske_img);	
	/*if (m_cp.image_show) {
		imshow_wait("skeleton img", ske_img);
	}*/

	//遍历所有点，找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
	std::vector<cv::Point> candidate_pts;
	cv::Point p5_nearst;
	double dist_th = 5;
	double dist_min = 1.0e6;
	for (int r = 0; r < ske_img.rows; ++r) {
		for (int c = 0; c < ske_img.cols; ++c) {
			if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
			double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r),2);
			dist = std::sqrtf(dist);
			if (dist < dist_min) {
				dist_min = dist;
				p5_nearst.x = c;
				p5_nearst.y = r;
			}
			if (std::fabs(dist - m_cp.offset_grab) < dist_th) {
				candidate_pts.push_back(cv::Point(c, r));
			}
		}
	}

	//按与参考角度的差，找到有效的候选点集合
	std::vector<cv::Point> valid_candidate_pts;
	double ref_angle = atan2(p5.x - p3.x, p5.y - p3.y);
	cv::Point p_min_angle(-1,-1);
	double min_angle = CV_PI;
	for (auto&p : candidate_pts) {
		double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
		//计算夹角
		double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
		/*if (ref_angle > 0.5 * CV_PI) {
			if (angle_to_p3 < 0) {
				angle_to_p3 += 2 * CV_PI;				
			}
			fabs_angle = std::fabs(angle_to_p3 - ref_angle);
		}
		else {
			if (ref_angle < -0.5 * CV_PI) {
				if (angle_to_p3 > 0) {
					angle_to_p3 -= 2 * CV_PI;
				}
				fabs_angle = std::fabs(angle_to_p3 - ref_angle);
			}
			else {
				fabs_angle = std::fabs(angle_to_p3 - ref_angle);
			}
		}*/
		if (fabs_angle > CV_PI / 4.0) { continue; }
		if (fabs_angle < min_angle) {
			min_angle = fabs_angle;
			p_min_angle.x = p.x;
			p_min_angle.y = p.y;
		}
		valid_candidate_pts.push_back(p);
	}	
	if (p_min_angle.x>0 && p_min_angle.y>0) {
		grab_x = p_min_angle.x;
		grab_y = p_min_angle.y;
	}

	if (m_cp.image_show) {
		cv::Mat ske_img_tmp = ske_img.clone();
		for (auto&p : valid_candidate_pts) {
			ske_img_tmp.at<unsigned char>(p) = 100;
		}
		cv::circle(ske_img_tmp, p5, 4, cv::Scalar(255, 0, 255), 1, 3, 0);
		if (grab_x > 0 && grab_y > 0) {
			cv::circle(ske_img_tmp, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		}
		imshow_wait("skeleton img label", ske_img_tmp);
	}
	
	//计算grab点的抓取角度
	if (p_min_angle.x > 0 && p_min_angle.y > 0) {
		grab_angle = get_grab_position(ske_img, p_min_angle, ref_angle);
	}

	//重新得到grab_x,grab_y的坐标
	if (grab_x > 0 && grab_y > 0) {
		int real_padding_y = p5.y - int(b.ppoint[9] - b.y1);
		grab_y -= real_padding_y;
		grab_y += b.y1;
		grab_x += b.x1;
	}

}

/**
* Code for thinning a binary image using Zhang-Suen algorithm.
*
* Author:  Nash (nash [at] opencv-code [dot] com)
* Website: http://opencv-code.com
*/

/**
* Perform one thinning iteration.
* Normally you wouldn't call this function directly from your code.
*
* Parameters:
* 		im    Binary image with range = [0,1]
* 		iter  0=even, 1=odd
*/
void CTeaSort::thinningIteration(cv::Mat& img, int iter)
{
	CV_Assert(img.channels() == 1);
	CV_Assert(img.depth() != sizeof(uchar));
	CV_Assert(img.rows > 3 && img.cols > 3);

	cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1);

	int nRows = img.rows;
	int nCols = img.cols;

	if (img.isContinuous()) {
		nCols *= nRows;
		nRows = 1;
	}

	int x, y;
	uchar *pAbove;
	uchar *pCurr;
	uchar *pBelow;
	uchar *nw, *no, *ne;    // north (pAbove)
	uchar *we, *me, *ea;
	uchar *sw, *so, *se;    // south (pBelow)

	uchar *pDst;

	// initialize row pointers
	pAbove = NULL;
	pCurr = img.ptr<uchar>(0);
	pBelow = img.ptr<uchar>(1);

	for (y = 1; y < img.rows - 1; ++y) {
		// shift the rows up by one
		pAbove = pCurr;
		pCurr = pBelow;
		pBelow = img.ptr<uchar>(y + 1);

		pDst = marker.ptr<uchar>(y);

		// initialize col pointers
		no = &(pAbove[0]);
		ne = &(pAbove[1]);
		me = &(pCurr[0]);
		ea = &(pCurr[1]);
		so = &(pBelow[0]);
		se = &(pBelow[1]);

		for (x = 1; x < img.cols - 1; ++x) {
			// shift col pointers left by one (scan left to right)
			nw = no;
			no = ne;
			ne = &(pAbove[x + 1]);
			we = me;
			me = ea;
			ea = &(pCurr[x + 1]);
			sw = so;
			so = se;
			se = &(pBelow[x + 1]);

			int A = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) +
				(*ea == 0 && *se == 1) + (*se == 0 && *so == 1) +
				(*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) +
				(*we == 0 && *nw == 1) + (*nw == 0 && *no == 1);
			int B = *no + *ne + *ea + *se + *so + *sw + *we + *nw;
			int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we);
			int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we);

			if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0)
				pDst[x] = 1;
		}
	}

	img &= ~marker;
}

/**
* Function for thinning the given binary image
*
* Parameters:
* 		src  The source image, binary with range = [0,255]
* 		dst  The destination image
*/
void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
{
	dst = src.clone();
	dst /= 255;         // convert to binary image

	cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1);
	cv::Mat diff;

	do {
		thinningIteration(dst, 0);
		thinningIteration(dst, 1);
		cv::absdiff(dst, prev, diff);
		dst.copyTo(prev);
	} while (cv::countNonZero(diff) > 0);

	dst *= 255;
}

/**
计算 [-pi,pi]间的两个角间的夹角
*/
double CTeaSort::intersection_angle(
	double ref_angle, 
	double angle_to_p3
)
{
	//计算夹角
	double fabs_angle = 0;
	if (ref_angle > 0.5 * CV_PI) {
		if (angle_to_p3 < 0) {
			angle_to_p3 += 2 * CV_PI;
		}
		fabs_angle = std::fabs(angle_to_p3 - ref_angle);
	}
	else {
		if (ref_angle < -0.5 * CV_PI) {
			if (angle_to_p3 > 0) {
				angle_to_p3 -= 2 * CV_PI;
			}
			fabs_angle = std::fabs(angle_to_p3 - ref_angle);
		}
		else {
			fabs_angle = std::fabs(angle_to_p3 - ref_angle);
		}
	}
	return fabs_angle;
}
/**
* 
*/
double CTeaSort::get_grab_position(
	const cv::Mat& skele_img, 
	cv::Point&vertex, 
	double ref_angle
)
{
	double grab_point_angle = CV_2PI;
	cv::Point pt0, pt1, pt2, pt3;
	double radius = static_cast<double>(m_cp.offset_grab) * 0.5;
	calc_bottom_vertex(vertex, ref_angle, CV_PI / 8.0, radius, pt0, pt1);
	calc_bottom_vertex(vertex, ref_angle+CV_PI, CV_PI / 8.0, radius, pt2, pt3);

	std::vector<cv::Point> triangle_region;	
	triangle_region.push_back(pt0);
	triangle_region.push_back(pt1);
	triangle_region.push_back(pt2);
	triangle_region.push_back(pt3);	
	
	//构建多边形，然后判别骨架图中在多边形内的骨架像素
	std::vector<cv::Point> curve_pts;
	cv::Mat roi_img = skele_img.clone();
	for(int r=0;r<roi_img.rows;++r){
		for(int c=0;c<roi_img.cols;++c){
			if(roi_img.at<unsigned char>(r,c)==0){continue;}
			double d = cv::pointPolygonTest(triangle_region,cv::Point2f(c,r),false);
			// d 1-内部点， 0-边缘点 -1-外部点
			if(d<=0){
				roi_img.at<unsigned char>(r,c)==0;
			}
			else{
				curve_pts.push_back(cv::Point(c,r));
			}
		}
	}
	//根据curve_pts进行曲线拟合，得到茎的曲线
	//cv::Mat curve_model;
	//poly_fit_cv(curve_pts, 1, curve_model);
	cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
	line_fit(curve_pts, line_model);
		//double k = curve_model.at<double>(1, 0);
		//double y_angle = 0.5 * CV_PI - atan(k); // y_angle in range [0, pi]
	double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
	double fabs_angle = intersection_angle(ref_angle, y_angle);
	double y_angle_inv = atan2(-line_model[0], -line_model[1]);;	//y_angle_inv in range [-pi, pi]
	double fabs_angle_inv = intersection_angle(ref_angle, y_angle_inv);

	grab_point_angle = y_angle;
	if (fabs_angle_inv < fabs_angle) {
		grab_point_angle = y_angle_inv;
	}


	//可视化
	if (m_cp.image_show) {
		cv::Mat ske_img_tmp = skele_img.clone();
		for (auto&p : curve_pts) {
			ske_img_tmp.at<unsigned char>(p) = 100;
		}
		cv::circle(ske_img_tmp, vertex, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(ske_img_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(ske_img_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(ske_img_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(ske_img_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);

		cv::line(ske_img_tmp, pt0, pt1, cv::Scalar(255, 215, 255), 2);
		cv::line(ske_img_tmp, pt0, pt3, cv::Scalar(255, 215, 255), 2);
		cv::line(ske_img_tmp, pt1, pt2, cv::Scalar(255, 215, 255), 2);
		cv::line(ske_img_tmp, pt2, pt3, cv::Scalar(255, 215, 255), 2);
		
		double dcx = radius * sin(grab_point_angle);
		double dcy = radius * cos(grab_point_angle);

		cv::Point dir_o;
		cv::Point dir_p;
		dir_o.x = vertex.x + 10;
		dir_o.y = vertex.y;

		dir_p.x = int(vertex.x + 10 + dcx);
		dir_p.y = int(vertex.y + dcy);
		cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);

		
		imshow_wait("grab angle", ske_img_tmp);
	}
	return grab_point_angle;

}

/**
* calc_bottom_vertex
* 找到三角形两个底角点
*/
void CTeaSort::calc_bottom_vertex(
	cv::Point&vertex,	//input
	double ref_angle,	//input, rad
	double delta_angle, //input, rad
	double radius,		//input
	cv::Point&bpt0,		//output
	cv::Point&bpt1		//output
) 
{
	//double delta_angle = CV_PI / 8.0; // 22.5 degree
	//double radius = static_cast<double>(m_cp.offset_grab) * 1.5;

	double angle = ref_angle - delta_angle;
	int x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
	int y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;

	bpt0.x = x;
	bpt0.y = y;

	angle = ref_angle + delta_angle;
	x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
	y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;

	bpt1.x = x;
	bpt1.y = y;
}
//cv::Mat CTeaSort::poly_fit(
//	std::vector<cv::Point2f>& chain,
//	int n
//)
//{
//	//https://blog.csdn.net/jpc20144055069/article/details/103232641
//	cv::Mat y(chain.size(), 1, CV_32F, cv::Scalar::all(0));
//	cv::Mat phy(chain.size(), n, CV_32F, cv::Scalar::all(0));
//	for(int i=0;i<phy.rows;++i){
//		float* pr = phy.ptr<float>(i);
//		for(int j=0; j<phy.cols;++j){
//			pr[j] = pow(chain[i].x,j);
//		}
//		y.at<float>(i) = chain[i].y;
//	}
//	
//	cv::Mat phy_t = phy.t();
//	cv::Mat phyMULphy_t = phy.t() * phy;
//	cv::Mat phyMphyInv = phyMULphy_t.inv();
//	cv::Mat a = phyMphyInv * phy_t;
//	a = a*y;
//	return a;	
//}

void CTeaSort::line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines)
{
	std::vector<cv::Point2f> pts;
	for (auto&p : key_point) {
		pts.push_back(cv::Point2f(p.x, p.y));
	}
	double param = 0;
	double reps = 0.01;
	double aeps = 0.01;
	//cv::Vec4f lines;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
	cv::fitLine(pts, lines, DIST_L1, param, reps, aeps);	
}
//bool CTeaSort::poly_fit_cv(
//std::vector<cv::Point>& key_point,
//int n,
//cv::Mat& A
//)
//{
//	//https://blog.csdn.net/KYJL888/article/details/103073956
//	int N = key_point.size();
//	
//	//构造矩阵X
//	cv::Mat X = cv::Mat::zeros(n+1, n+1, CV_64FC1);
//	for(int i=0;i<n+1; ++i){
//		for(int j=0;j<n+1;++j){
//			for(int k=0;k<N;++k){
//				X.at<double>(i,j) = X.at<double>(i,j) +
//  				  std::pow(key_point[k].x, i+j);
//			}
//		}
//	}
//	
//	//构造矩阵Y
//	cv::Mat Y = cv::Mat::zeros(n+1, 1, CV_64FC1);
//	for(int i=0;i<n+1;++i){
//		for(int k=0;k<N;++k){
//			Y.at<double>(i,0) = Y.at<double>(i,0) +
//			std::pow(key_point[k].x, i) + key_point[k].y;
//		}
//	}
//	
//	A = cv::Mat::zeros(n+1, 1, CV_64FC1);
//	cv::solve(X,Y,A,cv::DECOMP_LU);	
//	return true;
//}
//double CTeaSort::calc_fit_y(
//double x,	//input
//cv::Mat& A	//input
//)
//{	
//	//double y = A.at<double>(0,0) + A.at<double>(1,0) * x +
//	//	A.at<double>(2,0) * std::pow(x,2) + A.at<double>(3,0) * std::pow(x,3);
//	//return y;	
//	
//	double y = 0.0;
//	for(int i=0; i<A.rows;++i){
//		y += A.at<double>(i,0) * std::pow(x,i);
//	}
//	return y;	
//}
//}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// calculate_stem_grab_position_opt（）替代calculate_stem_grab_position函数
// 1）采用局部thinning方法提高效率
// 2) 重新用局部线性拟合的方向替代ref_angle(原始是p5和p3点连线与y正方向的夹角)

void CTeaSort::calculate_stem_grab_position_opt(
	Bbox&b,
	double& grab_x,		//output
	double& grab_y,		//output
	double& grab_angle //input-output
)
{

	grab_x = grab_y = -1.0;
	//crop image
	int padding = 2 * m_cp.offset_grab;
	int y3 = int(b.ppoint[5]);
	int y5 = int(b.ppoint[9]);
	cv::Point p3(int(b.ppoint[4] - b.x1), int(b.ppoint[5] - b.y1));
	cv::Point p4(int(b.ppoint[6] - b.x1), int(b.ppoint[7] - b.y1));
	cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
	cv::Mat crop_img;
	if (y5 > y3) {
		// Y position	
		int ymax = b.y2 + padding;
		if (ymax > m_raw_img.rows) {
			ymax = m_raw_img.rows;
		}
		crop_img = m_raw_img(cv::Range(b.y1, ymax), cv::Range(b.x1, b.x2)).clone();
	}
	else {
		// ^ position
		if (b.y1 - padding < 0) {
			padding = b.y1;
		}
		p5.y = int(b.ppoint[9] - b.y1 + padding);
		p4.y = int(b.ppoint[7] - b.y1 + padding);
		p3.y = int(b.ppoint[5] - b.y1 + padding);
		crop_img = m_raw_img(cv::Range(b.y1 - padding, b.y2), cv::Range(b.x1, b.x2)).clone();

	}
	if (m_cp.image_show) {
		cv::Mat crop_img_tmp = crop_img.clone();
		cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
		cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
		cv::circle(crop_img_tmp, p5, 4, cv::Scalar(0, 0, 255), -1, 3, 0);

		imshow_wait("cropped box", crop_img_tmp);
	}

	//to gray
	cv::Mat gray_img;
	if (crop_img.channels() == 1) { gray_img = crop_img; }
	else {
		cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
	}
	//binary
	cv::Mat bin_img;
	double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
	cv::bitwise_not(bin_img, bin_img);
	if (m_cp.image_show) {
		imshow_wait("cropped binary img", bin_img);
	}

	vector<vector<cv::Point>> contours;
	vector<cv::Vec4i> hierarchy;
	contours.clear();
	hierarchy.clear();
	findContours(bin_img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
	cv::Mat con_img = cv::Mat::zeros(bin_img.size(), CV_8UC1);
	for (auto&c : contours) {
		for (auto&p : c) {
			con_img.at<unsigned char>(p.y, p.x) = 255;
		}
	}
	if (m_cp.image_show) {	
		imshow_wait("findContours", con_img);
	}
	//在grab_angle的指导下找到最优方向，截图，进行局部thinning
	double ref_angle_init = grab_angle;
	double delta_angle = CV_PI / 24.0;
	double radius = static_cast<double>(m_cp.offset_grab);
	cv::Point pt0, pt1, pt2, pt3;
	double step_angle = CV_PI / 36.0; // 5 degree
	int max_pixels = 0;
	cv::Point pt0_opt, pt1_opt, pt2_opt, pt3_opt, center_opt;
	int minx_opt, maxx_opt, miny_opt, maxy_opt;
	double target_angle_opt;
	for (int i = -8; i <= 8; ++i) { //-30 degree ----  30 degree
		//在指定方向的矩形框内，找到内部点最多的方向，作为主方向
		double target_angle = ref_angle_init + i*step_angle;
		cv::Point center_pt;
		center_pt.x = p5.x + static_cast<int>(radius * sin(target_angle));
		center_pt.y = p5.y + static_cast<int>(radius * cos(target_angle));
		calc_bottom_vertex(center_pt, target_angle, delta_angle, radius, pt0, pt1);
		calc_bottom_vertex(center_pt, target_angle + CV_PI, delta_angle, radius, pt2, pt3);

		std::vector<cv::Point> triangle_region;
		triangle_region.push_back(pt0);
		triangle_region.push_back(pt1);
		triangle_region.push_back(pt2);
		triangle_region.push_back(pt3);

		//外接4边形
		int minx, maxx, miny, maxy;
		minx = maxx = pt0.x;
		miny = maxy = pt0.y;
		for (auto& pt : triangle_region) {
			minx = minx > pt.x ? pt.x : minx;
			maxx = maxx > pt.x ? maxx : pt.x;
			miny = miny > pt.y ? pt.y : miny;
			maxy = maxy > pt.y ? maxy : pt.y;
		}

		//counting
		int pixel_num = 0;
		for (int r = miny; r <= maxy; ++r) {
			if (r < 0) { continue; }
			if (r >= bin_img.rows) { continue; }
			for (int c = minx; c <= maxx; ++c) {
				if (c < 0) { continue; }
				if (c >= bin_img.cols) { continue; }
				if (con_img.at<unsigned char>(r, c) == 0) { continue; }

				double d = cv::pointPolygonTest(triangle_region, cv::Point2f(c, r), false);
				// d 1-内部点， 0-边缘点 -1-外部点
				if (d >= 0) {
					pixel_num++;
				}				
			}
		}
		if (pixel_num > max_pixels) {
			max_pixels = pixel_num;
			pt0_opt = pt0; 
			pt1_opt = pt1;
			pt2_opt = pt2;
			pt3_opt = pt3;
			center_opt = center_pt;
			minx_opt = minx;
			maxx_opt = maxx;
			miny_opt = miny;
			maxy_opt = maxy;
			target_angle_opt = target_angle;
		}
		/*if (m_cp.image_show) {
			cv::Mat bin_tmp = bin_img.clone();

			cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
			cv::circle(bin_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
			cv::circle(bin_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
			cv::circle(bin_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
			cv::circle(bin_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);

			cv::line(bin_tmp, pt0, pt1, cv::Scalar(180, 215, 255), 2);
			cv::line(bin_tmp, pt0, pt3, cv::Scalar(180, 215, 255), 2);
			cv::line(bin_tmp, pt1, pt2, cv::Scalar(180, 215, 255), 2);
			cv::line(bin_tmp, pt2, pt3, cv::Scalar(180, 215, 255), 2);
			
			imshow_wait("binary img box", bin_tmp);
		}*/
	}

	//opt box process
	if (m_cp.image_show) {
		cv::Mat bin_tmp = bin_img.clone();

		cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(bin_tmp, pt0_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(bin_tmp, pt1_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(bin_tmp, pt2_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		cv::circle(bin_tmp, pt3_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);

		cv::line(bin_tmp, pt0_opt, pt1_opt, cv::Scalar(180, 215, 255), 2);
		cv::line(bin_tmp, pt0_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);
		cv::line(bin_tmp, pt1_opt, pt2_opt, cv::Scalar(180, 215, 255), 2);
		cv::line(bin_tmp, pt2_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);

		imshow_wait("binary img box opt", bin_tmp);
	}


	// skeletonize() or medial_axis()	
	cv::Mat ske_img;
	cv::Mat roi_bin_img = cv::Mat::zeros(bin_img.size(), CV_8UC1);	
	for (int r = miny_opt; r <= maxy_opt; ++r) {
		if (r < 0) { continue; }
		if (r >= bin_img.rows) { continue; }
		for (int c = minx_opt; c <= maxx_opt; ++c) {
			if (c < 0) { continue; }
			if (c >= bin_img.cols) { continue; }
			if (bin_img.at<unsigned char>(r, c) == 0) { continue; }
			roi_bin_img.at<unsigned char>(r, c) = bin_img.at<unsigned char>(r, c);			
		}
	}

	thinning(roi_bin_img, ske_img);
	if (m_cp.image_show) {
		imshow_wait("skeleton img", ske_img);
	}

	//通过区域内的骨架点计算ref_angle
	std::vector<cv::Point> triangle_region;
	triangle_region.push_back(pt0_opt);
	triangle_region.push_back(pt1_opt);
	triangle_region.push_back(pt2_opt);
	triangle_region.push_back(pt3_opt);
	std::vector<cv::Point> in_region_pts;	
	for (int r = miny_opt; r <= maxy_opt; ++r) {
		if (r < 0) { continue; }
		if (r >= ske_img.rows) { continue; }
		for (int c = minx_opt; c <= maxx_opt; ++c) {
			if (c < 0) { continue; }
			if (c >= ske_img.cols) { continue; }
			if (ske_img.at<unsigned char>(r, c) == 0) { continue; }

			double d = cv::pointPolygonTest(triangle_region, cv::Point2f(c, r), false);
			// d 1-内部点， 0-边缘点 -1-外部点
			if (d > 0) {
				in_region_pts.push_back(cv::Point(c, r));
			}			
		}
	}
	//计算ref_angle
	cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
	line_fit(in_region_pts, line_model);	
	double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
	double fabs_angle = intersection_angle(target_angle_opt, y_angle);
	double y_angle_inv = atan2(-line_model[0], -line_model[1]);;	//y_angle_inv in range [-pi, pi]
	double fabs_angle_inv = intersection_angle(target_angle_opt, y_angle_inv);

	double ref_angle = y_angle;
	if (fabs_angle_inv < fabs_angle) {
		ref_angle = y_angle_inv;
	}

	//可视化
	/*if (m_cp.image_show) {
		cv::Mat ske_img_tmp = ske_img.clone();
		for (auto&p : in_region_pts) {
			ske_img_tmp.at<unsigned char>(p) = 100;
		}	

		double dcx = radius * sin(ref_angle);
		double dcy = radius * cos(ref_angle);

		cv::Point dir_o;
		cv::Point dir_p;
		dir_o.x = center_opt.x + 10;
		dir_o.y = center_opt.y;

		dir_p.x = int(center_opt.x + 10 + dcx);
		dir_p.y = int(center_opt.y + dcy);
		cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);


		imshow_wait("ref angle", ske_img_tmp);
	}*/


	//遍历所有点，找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
	std::vector<cv::Point> candidate_pts;
	cv::Point p5_nearst;
	double dist_th = 5;
	double dist_min = 1.0e6;
	for (auto& pt : in_region_pts) {
		int c = pt.x;
		int r = pt.y;
		double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r), 2);
		dist = std::sqrtf(dist);
		if (dist < dist_min) {
			dist_min = dist;
			p5_nearst.x = c;
			p5_nearst.y = r;
		}
		if (std::fabs(dist - m_cp.offset_grab) < dist_th) {
			candidate_pts.push_back(cv::Point(c, r));
		}
	}	

	//按与参考角度的差，找到有效的候选点集合
	std::vector<cv::Point> valid_candidate_pts;	
	cv::Point p_min_angle(-1, -1);
	double min_angle = CV_PI;
	for (auto&p : candidate_pts) {
		double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
		//计算夹角
		double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
		
		if (fabs_angle > CV_PI / 4.0) { continue; }
		if (fabs_angle < min_angle) {
			min_angle = fabs_angle;
			p_min_angle.x = p.x;
			p_min_angle.y = p.y;
		}
		valid_candidate_pts.push_back(p);
	}
	if (p_min_angle.x>0 && p_min_angle.y>0) {
		grab_x = p_min_angle.x;
		grab_y = p_min_angle.y;
	}

	if (m_cp.image_show) {
		cv::Mat ske_img_tmp = ske_img.clone();
		for (auto&p : valid_candidate_pts) {
			ske_img_tmp.at<unsigned char>(p) = 100;
		}
		cv::circle(ske_img_tmp, p5, 4, cv::Scalar(255, 0, 255), 1, 3, 0);
		if (grab_x > 0 && grab_y > 0) {
			cv::circle(ske_img_tmp, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(156, 0, 255), 1, 3, 0);
		}
		imshow_wait("skeleton img label", ske_img_tmp);
	}

	//计算grab点的抓取角度
	if (p_min_angle.x > 0 && p_min_angle.y > 0) {
		grab_angle = get_grab_position(ske_img, p_min_angle, ref_angle);
	}

	//重新得到grab_x,grab_y的坐标
	if (grab_x > 0 && grab_y > 0) {
		int real_padding_y = p5.y - int(b.ppoint[9] - b.y1);
		grab_y -= real_padding_y;
		grab_y += b.y1;
		grab_x += b.x1;
	}
}

}