/*
  砧木切割点后识别

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

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

namespace graft_cv{

CRootStockCutPointReid::CRootStockCutPointReid(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)
{	
}
CRootStockCutPointReid::~CRootStockCutPointReid()
{
	this->clear_imginfo();	
}

void CRootStockCutPointReid::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 CRootStockCutPointReid::cut_point_reid(
	ImgInfo* imginfo, 
	Mat&cimg, 
	const char * pre_img_id,
	PositionInfo& posinfo,
	map<string, Mat>& img_cache
	)
{
	// cimg --- color image, bgr
	
	if(m_pLogger){				
			m_pLogger->INFO(m_imgId +" rootstock cut_pt reid begin");
	}
	if(!pre_img_id){
		if(m_pLogger){				
			m_pLogger->ERRORINFO(m_imgId +" pre-image id is NULL");
		}	
		return 1;
	} 
	string pre_imgid(pre_img_id);
	map<string, Mat>::iterator iter = img_cache.find(pre_imgid);
	if(iter==img_cache.end()){
		if(m_pLogger){				
			m_pLogger->ERRORINFO(m_imgId +" pre-image NOT in cache");
		}	
		return 1;
	}
	m_preGrayImg = iter->second;
    m_imgId = getImgId(img_type::rs_reid);
	 
	//1 image segment	
	clock_t t;
	clock_t t0 = clock();	

	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_preprocess(img);
    if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after image gray.");				
	}

	if(m_cparam.image_show){
		destroyAllWindows();	
		imshow_wait("rs_pre_gray",m_preGrayImg);	
		imshow_wait("rs_gray",m_grayImg);		
	}
	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 reid timeout.");				
			}
			throw_msg(m_imgId+" time out");
		}
	}
    if(m_pLogger){	
		m_pLogger->DEBUG(m_imgId+" after pre- and cur- gray image show.");				
	}
	
	//特征提取
	int max_feature = 500;
	OrbFeatureDetector fts_detector(max_feature);
	//SurfFeatureDetector fts_detector(max_feature);
	std::vector<KeyPoint> keypoints_pre, keypoints_cur;
	fts_detector.detect( m_preGrayImg, keypoints_pre );
	fts_detector.detect( m_grayImg, keypoints_cur );

	SurfDescriptorExtractor extractor;
	Mat descriptors_pre, descriptors_cur;
	extractor.compute( m_preGrayImg, keypoints_pre, descriptors_pre );
	extractor.compute( m_grayImg, keypoints_cur, descriptors_cur );
	//-- Step 3: Matching descriptor vectors with a brute force matcher
	BFMatcher matcher(NORM_L2);
	std::vector< DMatch > matches;
	matcher.match( descriptors_pre, descriptors_cur, matches );
	if(m_cparam.image_show){
		//-- Draw matches
		Mat img_matches;
		drawMatches( m_preGrayImg, keypoints_pre, m_grayImg, keypoints_cur, matches, img_matches );
		//-- Show detected matches
		imshow("Matches", img_matches );
		waitKey(-1);
	}

	posinfo.rs_reid_upoint_x = 10.0;
	posinfo.rs_reid_upoint_y = 10.0;
	posinfo.rs_reid_lpoint_x = 20.0;
	posinfo.rs_reid_lpoint_y = 20.0;


	//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(posinfo.rs_img_id,m_imgId.c_str());

	//	//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 right point
	//	circle(m_binImg, Point(stem_fork_left_x,stem_fork_y),5, Scalar(128,0,128), -1, 8,0);
	//	m_pImginfoBinFork=mat2imginfo(m_binImg);	

	//	//3 cut point int gray image	
	//	circle(m_grayImg, Point(stem_fork_left_x,stem_fork_y),5, Scalar(128,0,128), -1, 8,0);
	//	circle(m_grayImg, Point(stem_fork_right_x,stem_fork_y),5, Scalar(128,0,128), -1, 8,0);//v0.5.9.3 reference point
	//	circle(m_grayImg, Point(cut_pt.x,stem_fork_y),5, Scalar(128,0,128), -1, 8,0);//v0.5.9.3 reference point
	//	circle(m_grayImg, Point(cut_pt.x,stem_fork_y),2, Scalar(255,0,255), -1, 8,0);
	//	circle(m_grayImg, Point(lower_cut_pt.x,lower_cut_pt.y),5, 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 reid detect finished.");				
	}
	return 0;
};

void CRootStockCutPointReid::img_preprocess(Mat&img)
{
	//灰度化
	Mat b_img;
	if(img.channels()!=1){
		//color image ,bgr, for testing		
		cvtColor(img,m_grayImg,COLOR_BGR2GRAY);
	}
	else{
		m_grayImg = img.clone();
	}

	/*Mat kernel = getStructuringElement(
		MORPH_ELLIPSE, 
		Size( 2*m_cparam.rs_morph_radius + 1, 2*m_cparam.rs_morph_radius+1 ),
		Point( m_cparam.rs_morph_radius, m_cparam.rs_morph_radius )
		);    	
	double th = threshold(m_grayImg, b_img, 255, 255,THRESH_OTSU);		

	morphologyEx(
		b_img, 
		m_binImg, 
		MORPH_CLOSE,
		kernel,
		Point(-1,-1),
		m_cparam.rs_morph_iteration
		);*/


	
}

}