/*
  砧木切割点识别

  1）压左侧叶
  2）识别左侧叶柄分割点，
  3）切割角度：-45度

*/
#include <opencv2\imgproc\imgproc.hpp>
#include <math.h>
#include <algorithm>

#include "cut_point_rs.h"
#include "utils.h"
#include "data_def.h"
#include "logger.h"
//using namespace cv;

namespace graft_cv{

CRootStockCutPoint::CRootStockCutPoint(ConfigParam&cp,CGcvLogger*pLog/*=0*/)
:m_cparam(cp),
m_pLogger(pLog),
m_pImginfoBinFork(0),
m_pImgCorners(0),
m_pImgCutPoint(0),
m_imgId(""),
m_ppImgSaver(0)
{	
}
CRootStockCutPoint::~CRootStockCutPoint()
{
	this->clear_imginfo();	
}

void CRootStockCutPoint::clear_imginfo(){
	if (m_pImginfoBinFork){
		imginfo_release(&m_pImginfoBinFork);
		m_pImginfoBinFork=0;
	}
	if (m_pImgCorners){
		imginfo_release(&m_pImgCorners);
		m_pImgCorners=0;
	}
	if (m_pImgCutPoint){
		imginfo_release(&m_pImgCutPoint);
		m_pImgCutPoint=0;
	}
}

int CRootStockCutPoint::up_point_detect(
	ImgInfo* imginfo, 
	cv::Mat&cimg,
	PositionInfo& posinfo,
	map<string, cv::Mat>& img_cache
	)
{
	// cimg --- color image, bgr
	/*
	# 找到竖直的径，并对径两侧兴趣区域进行分析：茎位置，分叉位置，用于协助确定上切割点位置，或下切割点位置
    #     1）如果找到切割位置的角点，用角点作为上切割点，确定下切割点
    #     2）如果没有找到上切割点，通过分叉位置确定下切割点，根据径粗推导出上切割点
    h_ratio = 0.7 # 垂直方向hist，最高点的70%作为阈值，判断茎的位置
    v_ration = 1.2 # 水平方向hist，茎粗确定后，茎粗的1.2倍断定为分叉点    
    */
    m_imgId = getImgId(img_type::rs);
	//1 image segment
	if(m_pLogger){				
		m_pLogger->INFO(m_imgId +" rootstock cut_pt detect begin");				
	}

	clock_t t;
	clock_t t0 = clock();
	
	

	cv::Mat img;
	if(imginfo){
		if(m_pLogger){		
			stringstream buff;
			buff<<m_imgId<<" rootstock image, width="<<imginfo->width
				<<"\theight="<<imginfo->height;
			m_pLogger->INFO(buff.str());
		}
		if(!isvalid(imginfo)){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" rootstock input image invalid.");				
			}
			throw_msg(m_imgId+" invalid input image");	
			
		}	
		img = imginfo2mat(imginfo);
	}
	else{
		if(m_pLogger){		
			stringstream buff;
			buff<<m_imgId<<"rootstock image, width="<<cimg.cols
				<<"\theight="<<cimg.rows;
			m_pLogger->INFO(buff.str());
		}
		if(cimg.empty()){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" rootstock input image invalid.");				
			}
			throw_msg(m_imgId +" invalid input image");
			
		}
		img = cimg;
	}
	
	if(m_cparam.self_camera){
		image_set_bottom(img,20,8);
        if(m_pLogger){				
			m_pLogger->DEBUG(m_imgId+" image set bottom with pixel value 20.");				
		}
	}
	if(m_cparam.rs_y_flip){
		flip(img,img,0);
        if(m_pLogger){				
			m_pLogger->DEBUG(m_imgId+" image y fliped.");				
		}
	}
	
	//image saver
	if(m_ppImgSaver && *m_ppImgSaver){
		(*m_ppImgSaver)->saveImage(img, m_imgId);
	}
    if(m_pLogger){				
		m_pLogger->DEBUG(m_imgId+" before image segment.");				
	}	
	///////////////////////////////////////////////////////
	// image segment
	this->img_segment(img);
	// image save to cache, clear first
	img_cache.clear();
	img_cache.insert(pair<string, cv::Mat>(m_imgId, m_grayImg.clone()));

    if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after image segment.");				
	}

	if(m_cparam.image_show){
		cv::destroyAllWindows();
		imshow_wait("rs_bin",m_binImg);		
	}
	else{
		t = clock();
		if(1000.0*((float)(t-t0))/CLOCKS_PER_SEC>(float)m_cparam.timeout_proc){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" rootstock timeout.");				
			}
			throw_msg(m_imgId+" time out");
		}
	}
    if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after m_binImg image show.");				
	}
	
	//2 茎位置：x方向位置范围
	// 2.1 计算砧木x方向位置，中心
	vector<int> hist_col_pre;
	mat_histogram(m_binImg, hist_col_pre, 0);		

	int min_idx, max_idx;
	min_idx = hist_col_pre.size();
	max_idx = 0;
	vector<int>::const_iterator it0 = hist_col_pre.begin();
	for(vector<int>::const_iterator it = hist_col_pre.begin();it!=hist_col_pre.end();++it){
		if(*it>=m_cparam.rs_min_hist_value){
			int idx = it - it0;
			if(idx<min_idx){min_idx = idx;}
			if(idx > max_idx) {max_idx = idx;}
		}
	}
	if(max_idx<=min_idx){
		throw_msg(m_imgId+" invalid binary image, not exist valid objects");		
	}

	if(m_cparam.image_show){			
		cv::Mat tmp = m_binImg.clone();
		cv::line(tmp, cv::Point(min_idx,0), cv::Point(min_idx,tmp.rows), cv::Scalar(156),3);
		cv::line(tmp, cv::Point(max_idx,0), cv::Point(max_idx,tmp.rows), cv::Scalar(156),3);

		imshow_wait("rs_bin_x_range", tmp);		
	}
    int width = max_idx-min_idx+1;	
	int cent_x = (int)(0.5*(double) (max_idx-min_idx));

    
	vector<int> hist_col;
	mat_histogram_w(m_binImg,hist_col);

	if(m_cparam.image_show){		
		cv::Mat hist_col_img;
		hist2image(hist_col,hist_col_img,1,m_cparam.image_col_grid,m_cparam.image_row_grid);		
		imshow_wait("rs_hist_col", hist_col_img);		
	}

    if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after histogram col.");				
	}
	int x0,x1,stem_x0, stem_x1;
	get_stem_x_range_rscut(
		hist_col,
		m_cparam.rs_col_th_ratio,
		m_cparam.rs_stem_x_padding,
		cent_x,
		width,
		x0,
		x1,
		stem_x0,
		stem_x1);

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" rootstock image, x0="<<x0
			<<"\tx1="<<x1
			<<"\tstem_x0="<<stem_x0
			<<"\tstem_x1="<<stem_x1;
		m_pLogger->INFO(buff.str());
	}

	if(m_cparam.image_show){		
		cv::Mat tmp_img = m_binImg.clone();

		cv::line(tmp_img, cv::Point(x0,0), cv::Point(x0,m_binImg.rows-1), cv::Scalar(100),2);
		cv::line(tmp_img, cv::Point(x1,0), cv::Point(x1,m_binImg.rows-1), cv::Scalar(100),2);
		//fork right point
		imshow_wait("rs_x_field", tmp_img);				
	}
	
	//3 茎分叉位置：y方向分叉位置识别
	vector<int> hist_row;
	mat_histogram(m_binImg,hist_row,1,-1,-1,x0,x1+1);
	hist_filter(hist_row,0,5);
	if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after histogram row.");				
	}

	if(m_cparam.image_show){		
		cv::Mat hist_row_img;
		hist2image(hist_row,hist_row_img, 0,m_cparam.image_col_grid,m_cparam.image_row_grid);		
		imshow_wait("rs_hist_row", hist_row_img);		
	}
	else{
		t = clock();
		if(1000.0*((float)(t-t0))/CLOCKS_PER_SEC>(float)m_cparam.timeout_proc){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" rootstock timeout.");				
			}
			throw_msg(m_imgId+" time out");
		}
	}

	int stem_fork_y_pre=-1,stem_fork_y=-1,stem_end_y=-1,stem_dia=-1;
	
	int roi_max_y=-1;
	get_stem_y_fork_rs(
		hist_row,
		m_cparam.rs_row_th_ratio,
		m_cparam.rs_stem_dia_min,
		m_cparam.rs_stem_fork_y_min,
		m_cparam.rs_stem_dia_mp,
		stem_fork_y_pre,
		stem_end_y,
		stem_dia,
		roi_max_y);

	
	cv::Point fork_cent;
	double max_radius;
	double stem_angle;
	get_stem_y_fork_rs_update(
		m_binImg,
		m_cparam.rs_stem_x_padding,
		x0,
		x1,
		roi_max_y,
		stem_end_y,
		stem_dia,
		m_cparam.image_show,
		m_cparam.rs_cut_point_offset_ratio,
		fork_cent,
		max_radius,
		stem_angle
	);
	stem_fork_y = fork_cent.y;

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" rootstock image, stem_fork_y="<<stem_fork_y
			<<"\tstem_end_y="<<stem_end_y
			<<"\tstem_dia="<<stem_dia;
		m_pLogger->INFO(buff.str());
	}

	//4 茎分叉位置，茎左侧边缘点识别
	int stem_fork_left_x, stem_fork_right_x;
	get_stem_fork_xs(
		m_binImg,
		stem_fork_y,
		stem_x0,
		stem_x1,
		x0,
		x1,		
		stem_fork_left_x,
		stem_fork_right_x);
	int stem_fork_dia = stem_fork_right_x - stem_fork_left_x;

	gcv_point<int> cut_pt = gcv_point<int>((int)(stem_fork_left_x + 0.5*(stem_fork_right_x - stem_fork_left_x)),stem_fork_y);

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" rootstock image, stem_fork_x0="<<stem_fork_left_x
			<<"\tstem_fork_x1="<<stem_fork_right_x
			<<"\tstem_fork_diameter="<<stem_fork_dia;
		m_pLogger->INFO(buff.str());
	}

	
	//lower cut point	
	gcv_point<int> lower_cut_pt = gcv_point<int>(-1,-1);	
	find_lower_cut_point(m_binImg,cut_pt, lower_cut_pt, m_cparam.rs_cut_angle, stem_dia);
	if(m_cparam.image_show){
		cv::Mat stem_img = m_binImg.clone();
		cv::line(stem_img, cv::Point(stem_x0,0), cv::Point(stem_x0,stem_img.rows-1), cv::Scalar(100),2);
		cv::line(stem_img, cv::Point(stem_x1,0), cv::Point(stem_x1,stem_img.rows-1), cv::Scalar(100),2);
		//fork left point
		circle(stem_img, cv::Point(stem_fork_left_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		circle(stem_img, cv::Point(stem_fork_right_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		
		cv::Mat gray_img = m_grayImg.clone();
		cv::circle(gray_img, cv::Point(stem_fork_left_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		cv::circle(gray_img, cv::Point(stem_fork_right_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		cv::circle(gray_img, cv::Point((int)cut_pt.x,cut_pt.y),5, cv::Scalar(200,0,200), -1, 8,0);
		cv::circle(gray_img, cv::Point(lower_cut_pt.x,lower_cut_pt.y),5, cv::Scalar(200,0,200), -1, 8,0);
		image_draw_line(gray_img,cut_pt.x,cut_pt.y,lower_cut_pt.x,lower_cut_pt.y);
		imshow_wait("rs_fork_left_right_point", stem_img);		
		imshow_wait("rs_fork_left_right_point", gray_img);
	}
	else{
		t = clock();
		if(1000.0*((float)(t-t0))/CLOCKS_PER_SEC>(float)m_cparam.timeout_proc){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" rootstock timeout.");				
			}
			throw_msg(m_imgId+" time out");
		}
	}

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" rootstock image, upper_cut_point("<<cut_pt.x
			<<","<<cut_pt.y<<")"<<", lower_cut_pt( "<<lower_cut_pt.x
			<<","<<lower_cut_pt.y<<")";
		m_pLogger->INFO(buff.str());
	}

	//////////////////////////////////////start
	//v0.5.9.3 更改(2022-01-02)
	//cut_pt.x = stem_fork_left_x + 0.5*(stem_fork_right_x - stem_fork_left_x);
	//cut_pt.y = stem_fork_y;
	///////////////////////////////////////end

	double rs_cut_upoint_x = (double)cut_pt.x;
	//rs_cut_upoint_x -= (double)(img.cols/2.0);
	//rs_cut_upoint_x *= m_cparam.rs_cut_pixel_ratio;

	double rs_cut_upoint_y = (double)cut_pt.y;
	//rs_cut_upoint_y = (double)(img.rows/2.0) - rs_cut_upoint_y;
	//rs_cut_upoint_y *= m_cparam.rs_cut_pixel_ratio;

	//double rs_stem_diameter = stem_dia * m_cparam.rs_cut_pixel_ratio;
	double rs_stem_diameter = stem_dia;

	double rs_cut_lpoint_x = lower_cut_pt.x;
	//rs_cut_lpoint_x -= (double)(img.cols/2.0);
	//rs_cut_lpoint_x *= m_cparam.rs_cut_pixel_ratio;

	double rs_cut_lpoint_y = lower_cut_pt.y;
	//rs_cut_lpoint_y = (double)(img.rows/2.0) - rs_cut_lpoint_y;
	//rs_cut_lpoint_y *= m_cparam.rs_cut_pixel_ratio;

	//posinfo.rs_cut_edge_length = 0.0;
	posinfo.rs_cut_upoint_x = rs_cut_upoint_x;
	posinfo.rs_cut_upoint_y = rs_cut_upoint_y;
    posinfo.rs_stem_diameter = rs_stem_diameter;
	posinfo.rs_cut_lpoint_x = rs_cut_lpoint_x;
	posinfo.rs_cut_lpoint_y = rs_cut_lpoint_y;

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" rootstock image, rs_cut_upoint(mm)("<<rs_cut_upoint_x
			<<","<<rs_cut_upoint_y<<")"
			<<", rs_stem_diameter(mm)="<<rs_stem_diameter
			<<", lower_cut_pt(mm)("<<rs_cut_lpoint_x
			<<","<<rs_cut_lpoint_y<<")";
		m_pLogger->INFO(buff.str());
	}

	//  return images:	posinfo.pp_images
	if(m_cparam.image_return){
		this->clear_imginfo();
		//0) image id
		strcpy_s(posinfo.rs_img_id,m_imgId.c_str());

		//1) 		
		//stem x-range
		cv::line(m_binImg, cv::Point(stem_x0,0), cv::Point(stem_x0,m_binImg.cols-1), cv::Scalar(100),2);
		cv::line(m_binImg, cv::Point(stem_x1,0), cv::Point(stem_x1,m_binImg.cols-1), cv::Scalar(100),2);
		//fork right point
		cv::circle(m_binImg, cv::Point(stem_fork_left_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		m_pImginfoBinFork=mat2imginfo(m_binImg);	

		//3 cut point int gray image	
		cv::circle(m_grayImg, cv::Point(stem_fork_left_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);
		cv::circle(m_grayImg, cv::Point(stem_fork_right_x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);//v0.5.9.3 reference point
		cv::circle(m_grayImg, cv::Point(cut_pt.x,stem_fork_y),5, cv::Scalar(128,0,128), -1, 8,0);//v0.5.9.3 reference point
		cv::circle(m_grayImg, cv::Point(cut_pt.x,stem_fork_y),2, cv::Scalar(255,0,255), -1, 8,0);
		cv::circle(m_grayImg, cv::Point(lower_cut_pt.x,lower_cut_pt.y),5, cv::Scalar(200,0,200), -1, 8,0);
		image_draw_line(m_grayImg,cut_pt.x,cut_pt.y,lower_cut_pt.x,lower_cut_pt.y);
	
		m_pImgCutPoint = mat2imginfo(m_grayImg);			
		posinfo.pp_images[0]=m_pImginfoBinFork;
		posinfo.pp_images[1]=m_pImgCutPoint;

		if(m_ppImgSaver && *m_ppImgSaver){
			(*m_ppImgSaver)->saveImage(m_binImg, m_imgId+"_rst_0");
			(*m_ppImgSaver)->saveImage(m_grayImg, m_imgId+"_rst_1");
		}		
	}
	if(m_pLogger){				
		m_pLogger->INFO(m_imgId +" rootstock cut_pt detect finished.");				
	}
	return 0;
}

void CRootStockCutPoint::get_stem_root_y(
		vector<int>&hist,		
		double stem_dia_min,			
		int & end_y)
{
	end_y = -1;

	int start_idx, max_start_idx, max_end_idx, max_len;
	start_idx= max_start_idx= max_end_idx=-1;
	max_len=0;
    
	for(size_t i = 0; i<hist.size(); i++){
		if(i==0 && hist[i]>=stem_dia_min){
			start_idx=0;
		}
		if(i==0){continue;}

		if(hist[i]>=stem_dia_min && hist[i-1]<stem_dia_min){
			start_idx = i;
			continue;
		}
		if(hist[i]<stem_dia_min && hist[i-1]>=stem_dia_min){
			int lenth = i - start_idx;
			if(lenth>max_len){
				max_start_idx= start_idx;
				max_end_idx = i;
				max_len = lenth;
			}
			continue;
		}
		if(i==hist.size()-1){
			if(hist[i]>=stem_dia_min){
				int lenth = i - start_idx;
				if(lenth>max_len){
					max_start_idx= start_idx;
					max_end_idx = i;
					max_len = lenth;
				}
			}
		}
		
	}
	end_y = max_end_idx;	
}

void CRootStockCutPoint::get_default_cutpoint(
		int fork_cent_x,
		int fork_cent_y,
		int fork_stem_dia,
	    cv::Point2f& cut_pt)
{
	gcv_point<int> start_pt(fork_cent_x,fork_cent_y);
	gcv_point<int> lower_cut_pt = gcv_point<int>(-1,-1);

	find_lower_cut_point(m_binImg,start_pt,lower_cut_pt,m_cparam.rs_cut_angle+180.0, fork_stem_dia);
	if(lower_cut_pt.x<0 || lower_cut_pt.y<0){
		throw_msg(m_imgId+" invalid end points");	
	}
	double dist = start_pt.distance(lower_cut_pt);
	if(dist>fork_stem_dia){
		double ca = (m_cparam.rs_cut_angle+180.0)*0.0174532925199433;
		cut_pt.x = start_pt.x+fork_stem_dia*cos(ca);
		cut_pt.y = start_pt.y-fork_stem_dia*sin(ca);
	}
	else{
		cut_pt.x = lower_cut_pt.x;
		cut_pt.y = lower_cut_pt.y;
	}
}
void CRootStockCutPoint::img_segment(cv::Mat&img)
{
	cv::Mat b_img;
	if(img.channels()!=1){
		//color image ,bgr, for testing		
		cvtColor(img,m_grayImg, cv::COLOR_BGR2GRAY);
	}
	else{
		m_grayImg = img.clone();
	}

	cv::Mat kernel = cv::getStructuringElement(
		cv::MORPH_ELLIPSE,
		cv::Size( 2*m_cparam.rs_morph_radius + 1, 2*m_cparam.rs_morph_radius+1 ),
		cv::Point( m_cparam.rs_morph_radius, m_cparam.rs_morph_radius )
		);    
	/*
	morphologyEx(
		g_img, 
		m_grayImg, 
		MORPH_CLOSE,
		kernel,
		Point(-1,-1),
		m_cparam.rs_morph_iteration_gray
		);*/

	double th = threshold(m_grayImg, b_img, 255, 255, cv::THRESH_OTSU);

	morphologyEx(
		b_img, 
		m_binImg, 
		cv::MORPH_CLOSE,
		kernel,
		cv::Point(-1,-1),
		m_cparam.rs_morph_iteration
		);

	//Canny(m_binImg, m_edgeImg,30,100);
	
}
int CRootStockCutPoint::get_optimal_corner( 
	int ref_x, 
	int ref_y,
	std::vector<cv::Point2f>& corners,
	int stem_dia,
	double cand_corner_box_width_ratio,
	double cand_corner_box_xoffset_ratio,
	double opt_corner_xoffset_ratio,
	double opt_corner_yoffset_ratio,	
	double corner_mask_radius_ratio,
	cv::Point2f& cut_pt)
{

	/*"""
    通过茎左侧分叉点坐标（ ref_x, ref_y）判别最优角点
    stem_img, 二值图像，茎部分的局部图像
    ref_x, ref_y, 检测到的茎分叉的左侧x,y坐标
    corners, 图像中的角点

    方法：
    1 reference点上区域的角点，定义区域
    2 评价角点
       角点与直线ref_x的距离
       角点与参考点间联通性（是否都是前景）
       角点与参考点的距离（1.5倍茎粗）

    """
	*/
	cut_pt.x = -1;
	cut_pt.y = -1;

    int cand_corner_box_window = (int)(cand_corner_box_width_ratio*stem_dia);
	int bx = ref_x + (int)(cand_corner_box_xoffset_ratio*stem_dia) - cand_corner_box_window;
    int by = ref_y - cand_corner_box_window;

    roi_box<int> candidate_box = roi_box<int>(bx,by,cand_corner_box_window,cand_corner_box_window);
    vector<cv::Point2f>cand_pts;
    for(size_t i=0; i<corners.size(); ++i){
        int pt_x = corners[i].x;
		int pt_y = corners[i].y;
        if (candidate_box.isInBox(pt_x,pt_y)){
			cv::Point2f pt;
			pt.x=corners[i].x;
			pt.y=corners[i].y;
            cand_pts.push_back(pt);
		}
	}

	if(cand_pts.size()==0){
		return 1;
	}
	//calculate corner pts' corner score of binary image
	vector<double> cor_mag_scores;
	corner_magnificence(
		cand_pts,
		stem_dia,
		m_cparam.rs_corner_mask_rad_ratio,
		cor_mag_scores
		);
    //# estimate candidate points' score
    vector<double>cand_pts_score;
	int optx = ref_x + (int)(opt_corner_xoffset_ratio * stem_dia);
	int opty = ref_y + (int)(opt_corner_yoffset_ratio * stem_dia);
    for(size_t i=0; i<cand_pts.size(); ++i){
		int ptx = cand_pts[i].x;
		int pty = cand_pts[i].y;        
        double score = (double)(abs(ptx - optx));
        double dist =sqrt((double)((ptx - optx)*(ptx - optx)) +
			              (double)((pty - opty)* (pty - opty)));
		double mag_score = stem_dia * (1.0 - cor_mag_scores[i]);
        score += dist;
		score += mag_score;
        cand_pts_score.push_back(score);
	}

	auto smallest = min_element(begin(cand_pts_score),end(cand_pts_score));
	size_t min_idx = distance(begin(cand_pts_score), smallest);
    
	cut_pt.x = cand_pts[min_idx].x;
	cut_pt.y = cand_pts[min_idx].y;   
	return 0;
}

void CRootStockCutPoint::corner_magnificence(
		const vector<cv::Point2f>& cand_pts,
		int stem_dia,
		double corner_mask_radius_ratio,
		vector<double>& scores
		)
{
	int r = int(stem_dia*corner_mask_radius_ratio);
	if (r<=0){r = 1;}
	int ptx,pty,x,y;
	double cnt,cnt_pos,score;
	for(size_t i=0; i<cand_pts.size();++i){
		ptx = (int)(cand_pts[i].x);
		pty = (int)(cand_pts[i].y); 
		cnt = 0.0;
		cnt_pos = 0.0;
		for(int dx=-r;dx<=r;++dx){
			x = ptx+dx;
			if(x<0 || x>=m_binImg.cols){continue;}
			for(int dy = -r;dy<=r;++dy){
				y = pty+dy;
				if(y<0 || y>=m_binImg.rows){continue;}
				cnt+=1.0;
				if(m_binImg.at<unsigned char>(y,x)>0 ){
					cnt_pos+=1.0;
				}
			}
		}
		if(cnt>0){score = cnt_pos/cnt;}
		else{score=0.0;}
		scores.push_back(score);
	}

}

double CRootStockCutPoint::get_cut_edge_length
		(
			cv::Mat& bin_img,
			cv::Mat& edge_img,
		gcv_point<int>& start_pt, //up cut point
		gcv_point<int>& lower_cut_pt,//output
		gcv_point<int>& root_pt,//output
		double cut_angle,
		int y_max,
		int stem_dia,
	    clock_t t0
		)
	{
		double curve_lenth = 0.0;
		gcv_point<int> pt_tmp = gcv_point<int>(-1,-1);
		lower_cut_pt = pt_tmp;
		
		find_lower_cut_point(bin_img,start_pt, lower_cut_pt, m_cparam.rs_cut_angle, stem_dia);
		curve_lenth = start_pt.distance(lower_cut_pt);

		if(m_pLogger){		
			stringstream buff;
			buff<<m_imgId<<" rootstock image, found lower_cut_point("<<lower_cut_pt.x
				<<", "<<lower_cut_pt.y<<"), upper_cut_point("<<start_pt.x<<","<<start_pt.y<<") to lower_cut_point length: "<<curve_lenth;
			m_pLogger->INFO(buff.str());
		}
		//Mat edge_img;		
		root_pt = pt_tmp;
		//Canny(bin_img, edge_img,30,100);
		double stem_curve_len = calculate_edge_length(edge_img,lower_cut_pt,root_pt,y_max,t0);
		curve_lenth+=stem_curve_len;
		return curve_lenth;
	}
	
	void CRootStockCutPoint::find_lower_cut_point(
		cv::Mat& bin_img,
		gcv_point<int>&start_pt,
		gcv_point<int>&end_pt,//output
		double cut_angle,
		int stem_dia)
	{
		end_pt.x = -1;
		end_pt.y = -1;

		double step = 5.0;    
		double polar_radius = step;
		int height,width;
		height = bin_img.rows;
		width = bin_img.cols;

		//ca = cut_angle*math.pi/180.0
		double ca = cut_angle*0.0174532925199433;
		int state = 0;
		int nxt_x,nxt_y;
		while(true) {
			nxt_x = int(start_pt.x+polar_radius*cos(ca));
			nxt_y = int(start_pt.y-polar_radius*sin(ca));
			if (nxt_x<0 || nxt_x >= width){
				state = 1;
				break;
			}
			if (nxt_y < 0 || nxt_y >= height){
				state = 1;
				break;
			}
			if (bin_img.at<unsigned char>(nxt_y,nxt_x)==0){
				//#在cur_pt和 nxt_pt两点间找到边缘点
				int small_step=1;
				double pr = polar_radius;
				int pre_x, pre_y;
				while (pr>0){
					pr-=small_step;
					pre_x = int(start_pt.x + pr * cos(ca));
					pre_y = int(start_pt.y - pr * sin(ca));
					if (bin_img.at<unsigned char>(pre_y, pre_x) > 0){						
						end_pt.x = pre_x;
						end_pt.y = pre_y;
						double dd = start_pt.distance(end_pt);
						if (dd <(double)(stem_dia *0.66)){
							break;
						}
						else{
							state = 2;
							break;
						}
					}
				}
            
			}
			if(state==2){
				break;
			}

			polar_radius += step;
		}//while
		if(state==1){//超出图像范围，直接计算
			end_pt.x = start_pt.x + stem_dia/2;
			end_pt.y = start_pt.y + (int)(0.5*stem_dia*fabs(tan(ca))+0.5);
		}
	}
	double CRootStockCutPoint::calculate_edge_length(
		cv::Mat& edge_img,
		gcv_point<int>&lower_cut_pt,
		gcv_point<int>&root_pt,//output
		int y_max,
	    clock_t t0
		)
	{
		double sum_len = 0.0;

		//cur_x, cur_y = int(start_pt[0]), int(start_pt[1])
		root_pt.x = -1;
		root_pt.y = -1;

		gcv_point<int> cur_pt = gcv_point<int>(lower_cut_pt);
		gcv_point<int> pre_pt = gcv_point<int>(-1,-1);
		gcv_point<int> nxt_pt = gcv_point<int>(-1,-1);
		int state = 0;
		double root2 = sqrt(2.0);
	    clock_t t;	
		int cnt = 0;
		if(m_pLogger){				
			stringstream buff;
			buff<<m_imgId<<"  before curve crawling, finishing conditions: 1) ymax="<<y_max<<" 2) crawling to start_point("<<lower_cut_pt.x
				<<","<<lower_cut_pt.y<<")";
			m_pLogger->DEBUG(buff.str());
		}
		while (true){
			cnt +=1;			
			if (!m_cparam.image_show && cnt%50 == 0){
				t = clock();
				if(1000.0*((float)(t-t0))/CLOCKS_PER_SEC>(float)m_cparam.timeout_proc){
					throw_msg(m_imgId+" time out");
				}
			}
			get_next_pt(edge_img, cur_pt, pre_pt,nxt_pt,false);
			if( nxt_pt.x == cur_pt.x || nxt_pt.y == cur_pt.y){
				sum_len += 1.0;
			}
			if( nxt_pt.x != cur_pt.x && nxt_pt.y != cur_pt.y){
				sum_len += root2;
			}
			pre_pt = cur_pt;			
			cur_pt = nxt_pt;
            
			if(m_pLogger){		
				stringstream buff;
				buff<<m_imgId<<" index: "<<cnt<<"\tcurrent point ("<<pre_pt.x
					<<", "<<pre_pt.y<<")";
				m_pLogger->DEBUG(buff.str());
			}
			if( nxt_pt.y == y_max){
				state = 1;
				root_pt = nxt_pt;
				break;
			}

			if (nxt_pt == lower_cut_pt){
				state = 2;
				break;
			}
		}
		if (m_pLogger){
			m_pLogger->DEBUG(m_imgId + " finished curve crawling!");
		}
		if (state == 2){
			return 0.0;
		}
		if (state == 1){			
			return sum_len;
		}
		else{
			return 0.0;
		}

	}

	int CRootStockCutPoint::get_root_point_reid(
		int x0,
		int x1,
		int stem_end_y,		
		bool is_right  //是否右侧根部点， true-右侧，false-左侧
		)
	{
		int stem_end_x=-1;
		int start_x,end_x,max_start_x,max_end_x, max_len;
		start_x = end_x = -1;
		max_start_x = max_end_x = -1;
		max_len = -1;

		

		for(int i =x0; i<x1;++i){
			if(i==x0 && m_binImg.at<unsigned char>(stem_end_y,i)>0){
				start_x = i;
				continue;
			}
			if(i==x1-1 && m_binImg.at<unsigned char>(stem_end_y,i)>0){
				end_x = i;
				int length = end_x - start_x+1;
				if (length >max_len){
					max_len = length;
					max_start_x = start_x;
					max_end_x = end_x;
				}
			}
			if(m_binImg.at<unsigned char>(stem_end_y,i)>0 && 
				m_binImg.at<unsigned char>(stem_end_y,i-1)==0){
				start_x = i;
				continue;
			}
			if(m_binImg.at<unsigned char>(stem_end_y,i)==0 && 
				m_binImg.at<unsigned char>(stem_end_y,i-1)>0){
				end_x = i;
				int length = end_x - start_x+1;
				if (length >max_len){
					max_len = length;
					max_start_x = start_x;
					max_end_x = end_x;
				}
			}

		}
		if (is_right){
			return max_end_x;
		}
		return max_start_x;    
	}

	void  CRootStockCutPoint::find_upper_cut_point_reid(
		cv::Mat& edge_img,
		gcv_point<int>&start_pt,
		gcv_point<int>&end_pt,//output
		double curve_length
		)
		{
			end_pt.x  =-1;
			end_pt.y = -1;
		gcv_point<int> cur_pt = gcv_point<int>(start_pt);
		gcv_point<int> pre_pt = gcv_point<int>(-1,-1);
		gcv_point<int> nxt_pt = gcv_point<int>(-1,-1);
		//pre_x=-1
		//pre_y=-1
		double root2 = sqrt(2.0);
		double sum_len =0.0;
		int state = 0;

		if(m_pLogger){				
			stringstream buff;
			buff<<m_imgId<<"  reid,  before curve crawling, finishing conditions: curve length="<<curve_length
			<<", start_point(root)("<<start_pt.x
				<<","<<start_pt.y<<")";
			m_pLogger->DEBUG(buff.str());

		}

		while (true){
			get_next_pt(edge_img,cur_pt,pre_pt,nxt_pt,true);
			if (nxt_pt.x==cur_pt.x || nxt_pt.y==cur_pt.y){
				sum_len+=1.0;
			}
			if ( nxt_pt.x!=cur_pt.x && nxt_pt.y!=cur_pt.y){
				sum_len += root2;
			}			
			pre_pt = cur_pt;
			cur_pt = nxt_pt;
			if(m_pLogger){		
				stringstream buff;
				buff<<m_imgId<<"current point ("<<pre_pt.x
					<<", "<<pre_pt.y<<")";
				m_pLogger->DEBUG(buff.str());
			}

			if (sum_len>=curve_length){
				state = 1;
				break;
			}
			if (nxt_pt == start_pt){
				state = 2;
				break;
			}
		}

		if (state==2){
			end_pt.x  =-1;
			end_pt.y = -1;
		}
		if (state == 1){
			end_pt = cur_pt;
		}			
		else{
			end_pt.x  =-1;
			end_pt.y = -1;
		}
	}

};