#include <opencv2\imgproc\imgproc.hpp>
#include "cut_point_sc.h"
#include "utils.h"
using namespace cv;

namespace graft_cv{

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

void CScionCutPoint::clear_imginfo(){
	if (m_pImginfoBinFork){
		imginfo_release(&m_pImginfoBinFork);
		m_pImginfoBinFork=0;
	}	
}

int CScionCutPoint::up_point_detect(
	ImgInfo* imginfo, 
	Mat&cimg, 
	PositionInfo& posinfo
	)
{
	clock_t t;
	clock_t t0 = clock();
	m_imgId = getImgId(img_type::sc);

	if(m_pLogger){				
		m_pLogger->INFO(m_imgId +" scion cut_pt detect begin.");				
	}
	
	// cimg --- color image, bgr
	Mat img;
	if(imginfo){
		if(m_pLogger){		
			stringstream buff;
			buff<<m_imgId<<" scion image, width="<<imginfo->width
				<<"\theight="<<imginfo->height;
			m_pLogger->INFO(buff.str());
		}

		if(!isvalid(imginfo)){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" scion input image invalid.");				
			}
			throw_msg( m_imgId+" invalid input image");
		}
		img = imginfo2mat(imginfo);
	}
	else{
		if(m_pLogger){		
			stringstream buff;
			buff<<m_imgId<<" scion image, width="<<cimg.cols
				<<"\theight="<<cimg.rows;
			m_pLogger->INFO(buff.str());
		}
		if(cimg.empty()){
			if(m_pLogger){				
				m_pLogger->ERRORINFO(m_imgId+" scion 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+" scion image set bottom with pixel value 20.");				
		}
	}
	if(m_cparam.sc_y_flip){
		flip(img,img,0);
		if(m_pLogger){				
			m_pLogger->DEBUG(m_imgId+" scion image y fliped.");				
		}
	}
	//image saver
	if(m_ppImgSaver && *m_ppImgSaver){
		(*m_ppImgSaver)->saveImage(img, m_imgId);
		if(m_pLogger){				
			m_pLogger->DEBUG(m_imgId+" scion after image save.");				
		}
	}
	if(m_pLogger){				
		m_pLogger->DEBUG(m_imgId+" scion before image segment.");				
	}

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

	if(m_cparam.image_show){
		destroyAllWindows();	
		imshow_wait("sc_gray", m_grayImg);	
		imshow_wait("sc_bin", m_binImg);		
	}
	
	if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" scion after m_binImg\m_grayImg image show.");				
	}

	//////////////////////////////////////////////////////////////////
	//stem x-range
	vector<int> hist_col;
	mat_histogram(m_binImg,hist_col);

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

	if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" scion after histogram col.");				
	}

	int x0,x1,stem_x0, stem_x1;
	get_stem_x_range_scion(hist_col,
		m_cparam.sc_col_th_ratio,
		m_cparam.sc_stem_x_padding,
		x0,
		x1,
		stem_x0,
		stem_x1
		);

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

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

		line(tmp_img,Point(x0,0),Point(x0,m_binImg.rows-1),Scalar(100),2);
		line(tmp_img,Point(x1,0),Point(x1,m_binImg.rows-1),Scalar(100),2);
		//fork right point
		imshow_wait("sc_x_field", tmp_img);				
	}	
	/////////////////////////////////////////////////////
	// 茎分离，通过高亮夹子分割
	vector<int> hist_row;
	mat_histogram(m_binImg,hist_row,1,-1,-1,x0,x1+1);

	if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" scion after histogram row.");				
	}

	if(m_cparam.image_show){		
		Mat hist_row_img;
		hist2image(hist_row,hist_row_img, 0,m_cparam.image_col_grid,m_cparam.image_row_grid);		
		imshow_wait("sc_hist_row", hist_row_img);		
	}
	else{
		t = clock();
		if(1000.0*((float)(t-t0))/CLOCKS_PER_SEC>(float)m_cparam.timeout_proc){
			throw_msg(m_imgId+" time out");
		}
		
	}
	int stem_y_min=-1;//,stem_dia=-1,stem_root_y=-1;
	vector<int> sub_hist_col;
	vector<int> sub_hist_row;
	int sub_stem_x0,sub_stem_x1,sub_x0,sub_x1;
	sub_x0 = sub_x1=sub_stem_x0=sub_stem_x1=-1;
	Mat scionBinImg;	
	get_stem_local_img(
		hist_row,		
		m_cparam.sc_stem_dia_min,
		x0,
		x1,
		stem_y_min,
		sub_hist_col,
		sub_hist_row,
		scionBinImg);	

	get_stem_x_range_scion(
		sub_hist_col,
		m_cparam.sc_col_th_ratio,
		m_cparam.sc_stem_x_padding,
		sub_x0,
		sub_x1,
		sub_stem_x0,
		sub_stem_x1);
	
	int ymin=-1,ymax=-1;
	get_cut_up_point(
		sub_hist_row,
		stem_y_min,		
		m_cparam.sc_r2_th,
		m_cparam.sc_average_window,
		m_cparam.sc_r2_window,
		ymin,
		ymax);		

    int slop_fold_x = get_stem_fork_left(
		scionBinImg,
		ymin,
		sub_stem_x0, 
		sub_stem_x1,
		m_cparam.sc_stem_edge_detect_window);

	int slop_cent_y = (int)((float)(ymin+ymax)/2.0);
	int slop_cent_x = get_stem_fork_left(
		scionBinImg,
		slop_cent_y,
		sub_stem_x0, 
		sub_stem_x1,
		m_cparam.sc_stem_edge_detect_window);

	int slop_lower_x = get_stem_fork_left(
		scionBinImg,
		ymax,
		sub_stem_x0, 
		sub_stem_x1,
		m_cparam.sc_stem_edge_detect_window);

	//update sub_stem_x0, sub_stem_x1
	stem_x0 = x0 + sub_stem_x0;
	stem_x1	= x0 + sub_stem_x1;	
	slop_fold_x += x0;
	slop_cent_x += x0;
	slop_lower_x += x0;

	//update ymin,ymax
	ymin += stem_y_min;
	ymax += stem_y_min;
	slop_cent_y += stem_y_min;

	Mat binRoi = m_binImg(Rect(x0,stem_y_min,scionBinImg.cols,scionBinImg.rows));
	scionBinImg.copyTo(binRoi);
	//imshow_wait("m_binImg", m_binImg);

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" scion image result(pixel) x0="<<x0
			<<", x1="<<x1
			<<", stem_x0="<<stem_x0
			<<", stem_x1="<<stem_x1
			<<", cut_pt=("<<slop_fold_x<<","<<ymin<<")"
			<<", cut_cent_pt=("<<slop_cent_x<<","<<slop_cent_y<<")"
			<<", cut_lower_pt=("<<slop_lower_x<<","<<ymax<<")"
			<<", stem_y_max="<<ymax;
		m_pLogger->INFO(buff.str());
	}

	if(m_cparam.image_show){		
		Mat result_img = m_binImg.clone();
		//stem x-range
		line(result_img,Point(stem_x0,0),Point(stem_x0,m_binImg.rows-1),Scalar(100),2);
		line(result_img,Point(stem_x1,0),Point(stem_x1,m_binImg.rows-1),Scalar(100),2);

		//fork y line
		line(result_img,Point(stem_x0,stem_y_min),Point(stem_x1,stem_y_min),Scalar(100),2);
		line(result_img,Point(stem_x0,ymax),Point(stem_x1,ymax),Scalar(100),2);
		//fold right point
		circle(result_img, Point(slop_fold_x,ymin),2, Scalar(150,0,128), -1, 8,0);
		circle(result_img, Point(slop_cent_x,slop_cent_y),2, Scalar(150,0,128), -1, 8,0);
		circle(result_img, Point(slop_lower_x,ymax),2, Scalar(150,0,128), -1, 8,0);
		imshow_wait("sc_result", result_img);		
	}

	

	double sc_cut_upoint_x, sc_cut_upoint_y,sc_cut_cpoint_x, sc_cut_cpoint_y, sc_cut_lpoint_x, sc_cut_lpoint_y;
	sc_cut_upoint_x = (double)slop_fold_x;
	sc_cut_upoint_x	-= (double)(img.cols/2.0);
	sc_cut_upoint_x *= m_cparam.sc_cut_pixel_ratio;

	sc_cut_upoint_y = (double)ymin;
	sc_cut_upoint_y = (double)(img.rows/2.0) - sc_cut_upoint_y;
	sc_cut_upoint_y *= m_cparam.sc_cut_pixel_ratio;


	sc_cut_cpoint_x = (double)slop_cent_x;
	sc_cut_cpoint_x	-= (double)(img.cols/2.0);
	sc_cut_cpoint_x *= m_cparam.sc_cut_pixel_ratio;

	sc_cut_cpoint_y = (double)slop_cent_y;
	sc_cut_cpoint_y = (double)(img.rows/2.0) - sc_cut_cpoint_y;
	sc_cut_cpoint_y *= m_cparam.sc_cut_pixel_ratio;

	
	sc_cut_lpoint_x = (double)slop_lower_x;
	sc_cut_lpoint_x	-= (double)(img.cols/2.0);
	sc_cut_lpoint_x *= m_cparam.sc_cut_pixel_ratio;

	sc_cut_lpoint_y = (double)ymax;
	sc_cut_lpoint_y = (double)(img.rows/2.0) - sc_cut_lpoint_y;
	sc_cut_lpoint_y *= m_cparam.sc_cut_pixel_ratio;


	posinfo.sc_cut_upoint_x = sc_cut_upoint_x;
	posinfo.sc_cut_upoint_y = sc_cut_upoint_y;
	posinfo.sc_cut_cpoint_x = sc_cut_cpoint_x;
	posinfo.sc_cut_cpoint_y = sc_cut_cpoint_y;

	posinfo.sc_cut_lpoint_x = sc_cut_lpoint_x;
	posinfo.sc_cut_lpoint_y = sc_cut_lpoint_y;


	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" scion image result(mm)"
			<<", cut_pt=("<<sc_cut_upoint_x<<","<<sc_cut_upoint_y<<")"
			<<", cut_cent_pt=("<<sc_cut_cpoint_x<<","<<sc_cut_cpoint_y<<")"
			<<", cut_lower_pt=("<<sc_cut_lpoint_x<<","<<sc_cut_lpoint_y<<")";
		m_pLogger->INFO(buff.str());
	}

	// posinfo.pp_images  	
	if(m_cparam.image_return){
		this->clear_imginfo();
		//1) 		
		//stem x-range
		line(m_binImg,Point(stem_x0,0),Point(stem_x0,m_binImg.cols-1),Scalar(100),2);
		line(m_binImg,Point(stem_x1,0),Point(stem_x1,m_binImg.cols-1),Scalar(100),2);

		//fork y line
		line(m_binImg,Point(stem_x0,stem_y_min),Point(stem_x1,stem_y_min),Scalar(100),2);
		line(m_binImg,Point(stem_x0,ymax),Point(stem_x1,ymax),Scalar(100),2);
		//fold right point
		circle(m_binImg, Point(slop_fold_x,ymin),5, Scalar(128,0,128), -1, 8,0);
		circle(m_binImg, Point(slop_cent_x,slop_cent_y),5, Scalar(128,0,128), -1, 8,0);
		circle(m_binImg, Point(slop_lower_x,ymax),5, Scalar(128,0,128), -1, 8,0);
		m_pImginfoBinFork=mat2imginfo(m_binImg);		
		//
		posinfo.pp_images[0]=m_pImginfoBinFork;		
		if(m_ppImgSaver && *m_ppImgSaver){
			(*m_ppImgSaver)->saveImage(m_binImg, m_imgId+"_rst_0");			
		}
	}
	if(m_pLogger){				
		m_pLogger->INFO(m_imgId +" scion cut_pt detect finished.");				
	}
	return ymin;
}
void CScionCutPoint::img_segment(Mat&img)
{
	if(img.channels()!=1){
		//color image ,bgr, for testing
		Mat b_img;
		cvtColor(img,m_grayImg,COLOR_BGR2GRAY);		
		double th = threshold(
			m_grayImg,
			b_img,
			255,
			255,
			THRESH_OTSU
			);
	   
		Mat kernel = getStructuringElement(
			MORPH_RECT,
			Size( 2*m_cparam.sc_morph_radius + 1, 2*m_cparam.sc_morph_radius+1),
			Point( m_cparam.sc_morph_radius, m_cparam.sc_morph_radius)
			);  
		morphologyEx(
			b_img,
			m_binImg,
			MORPH_CLOSE,
			kernel,
			Point(-1,-1),
			m_cparam.sc_morph_iteration);
		
	}
	else{
		//from imginfo image, gray image 
		//int morph_size = 1;	
		m_grayImg = img.clone();
		Mat b_img;
		double th = threshold(img, b_img, 255, 255,THRESH_OTSU);
		Mat kernel = getStructuringElement( 
			MORPH_RECT, 
			Size( 2*m_cparam.sc_morph_radius + 1, 2*m_cparam.sc_morph_radius+1 ), 
			Point( m_cparam.sc_morph_radius, m_cparam.sc_morph_radius)
			);    
		morphologyEx(
			b_img, 
			m_binImg, 
			MORPH_OPEN,
			kernel,
			Point(-1,-1),
			m_cparam.sc_morph_iteration
			);
	}	
}
void CScionCutPoint::get_stem_local_img(
	const std::vector<int>& hist,	
	int stem_dia_min,
	int x0, 
	int x1,
	int& stem_y_min,
	vector<int>& hist_col,
	vector<int>& hist_row,
	Mat& scionBinImg
	)
{
	//1 夹子被罩上背景板，不能以高亮夹子定位
	//2 直接找x范围内下方的大目标作为识别对象，目标像素面积
    //3 截取部分图像单独做二值化得到二值图像
    //4 局部图的histogram

	//0 设定目标面积最小值
	int min_obj_area = 100;

    //1 reverse histogram
	std::vector<int>reversed_hist;	
	for(int i=hist.size()-1; i>=0;i--){		
		reversed_hist.push_back(hist[i]);
	}    	
	stem_y_min = -1;
	int stem_y_max = -1;
	int start_idx, end_idx;
	start_idx = end_idx = -1;
	for(size_t i=0; i < reversed_hist.size(); ++i){
		if(i==0){
			if(reversed_hist[i]>=stem_dia_min){start_idx=i;}
			continue;
		}
		if(reversed_hist[i]>=stem_dia_min && reversed_hist[i-1]<stem_dia_min){
			start_idx=i;
			continue;
		}
		if(reversed_hist[i]<stem_dia_min && reversed_hist[i-1]>=stem_dia_min){
			//计算面积
			int area = 0;
			for(int oi=start_idx;oi<i;++oi){
				area +=reversed_hist[oi];
			}
			if(area >= min_obj_area){
				end_idx = i-1;
				break;
			}
		}
		if(i==reversed_hist.size()-1){
			if(reversed_hist[i]>=stem_dia_min){
				int area = 0;
				for(int oi=start_idx;oi<=i;++oi){
					area +=reversed_hist[oi];
				}
				if(area >= min_obj_area){
					end_idx = i;
					break;
				}
			}	
		}
	}
	if(start_idx<0 || end_idx<0 || end_idx<=start_idx){		
		throw_msg( m_imgId+string(" scion stem NOT exists valid sub-image"));
	}

	stem_y_min = end_idx + m_cparam.sc_clip_padding;
	stem_y_max = start_idx - 10* m_cparam.sc_clip_padding;

	if(stem_y_min>=m_grayImg.rows){stem_y_min = m_grayImg.rows;}
	if(stem_y_max<0){stem_y_max=0;}
	
	//2	
	if(m_cparam.image_show){
		Mat grayimg = m_grayImg.clone();
		rectangle(grayimg,
			Point(x0,grayimg.rows-stem_y_min),
			Point(x1,grayimg.rows-stem_y_max),
			Scalar(128));
		imshow_wait("gray_rect",grayimg);
	}

	Mat scionImg = m_grayImg(Rect(x0,
		m_grayImg.rows-stem_y_min,
		x1-x0,
		stem_y_min-stem_y_max));

	if(m_pLogger){		
		stringstream buff;
		buff<<m_imgId<<" scion image, sub image(x0,y0,width,height), x0="<<x0
			<<", y0="<<m_grayImg.rows-stem_y_min
			<<", width="<<x1-x0
			<<", height="<<stem_y_min-stem_y_max;
		m_pLogger->INFO(buff.str());
	}
	
	//Mat scionBinImg;
	double th = threshold(scionImg, scionBinImg, 255, 255,THRESH_OTSU);	
	if(m_cparam.image_show){
	   imshow_wait("scion_sub_bin",scionBinImg);
	   vector<int>hist;
	   mat_histogram(scionBinImg,hist,1);
	   Mat hist_col_img;
		hist2image(hist,hist_col_img,1,m_cparam.image_col_grid,m_cparam.image_row_grid);		
		imshow_wait("sc_sub_hist_col", hist_col_img);
	}
	//3
	hist_row.clear();
	mat_histogram(scionBinImg,hist_row,1,-1,-1,-1,-1);
	hist_col.clear();
	mat_histogram(scionBinImg,hist_col,0,-1,-1,-1,-1);
	stem_y_min = m_grayImg.rows-stem_y_min;
};
void CScionCutPoint::get_cut_up_point(
	const std::vector<int>& sub_hist,
	int stem_y_min,	
	double r2_ratio_th, 
	int average_window,
	int r2_window,
	int& ymin, 
	int& ymax)
{

	int start_idx=-1;
	int max_len=0;
	int max_start_idx = -1;
	int max_end_idx = -1;
	for(size_t i=0;i<sub_hist.size();++i){		
		if(i==0 && sub_hist[i]>0){
			start_idx=i;
			continue;
		}
		if(i==0){continue;}
		if(sub_hist[i]>0 && sub_hist[i-1]==0){
			start_idx=i;
			continue;
		}
		if((sub_hist[i]==0 && sub_hist[i-1]>0) ||
			(i==sub_hist.size()-1 && sub_hist[i]>0)){
			if((i-start_idx) >max_len ){
				max_end_idx=i;
				max_start_idx = start_idx;
				max_len = i-start_idx;
			}
			continue;
		}
	}
	if(max_end_idx<0){
		throw_msg(m_imgId+" scion stem sub-image histogram is all zero");
	}
   
    ymin = max_start_idx;
    ymax = max_end_idx-1;   
	
	vector<int>ys;   
    for (size_t i = ymin; i < ymax+1; ++i){       
	    ys.insert(ys.begin(),sub_hist[i]);        
	}	
    int fold_point = -1;
	vector<int> ys_tmp;
	for(size_t i=0;i<ys.size();++i){ys_tmp.push_back(ys[i]);}
	int mid_idx = (int)((float)ys.size() * 0.5);	
	sort(ys_tmp.begin(), ys_tmp.end());
	int mid_stem_dia = ys_tmp[mid_idx];
	int th_stem_dia = (int)((double)mid_stem_dia*0.95 +0.5);

	for(size_t i=0;i<ys.size();++i){
		if(ys[i]>=th_stem_dia){
			fold_point = i;
			break;
		}
	}
	int offset=10;
	if(m_folder_th<0){
		double ca = (75.+m_cparam.rs_cut_angle)*0.0174532925199433;
		m_folder_th = (int)(offset * fabs(tan(ca)) + 0.5);
	}
	while(true){
		int pre_idx = fold_point-offset;
		if(pre_idx<=0){break;}
		if(ys[fold_point] - ys[pre_idx] >=m_folder_th){break;}
		fold_point--;
	}
	ymin = ymax-fold_point;
	
}

};