#include <sstream>
#include <opencv2\highgui\highgui.hpp>
#include "grab_occlusion.h"
#include "utils.h"


namespace graft_cv {
	CStemResultInfos::CStemResultInfos(double seedling_dist,
		int holes_number,
		double x_min,
		double x_max,
		double z_min,
		double z_max,
		std::string pcd_id, 
		CGcvLogger*pLog)
		: m_pLogger(pLog)
		, m_seedling_dist(seedling_dist)
		, m_holes_number(holes_number)
		, m_xmin(x_min)
		, m_xmax(x_max)
		, m_zmin(z_min)
		, m_zmax(z_max)
		//, m_append_counter(0)
		, m_pcdId(pcd_id)	
		, m_max_size(50)
	{
		gen_root_centers();
	}
	
	CStemResultInfos::~CStemResultInfos()
	{}

	void CStemResultInfos::append(
		CStemResult& sr
	)
	{

		m_infos.insert(m_infos.begin(), sr);
		if (m_infos.size() > m_max_size) {
			m_infos.pop_back();
		}
		//m_append_counter += 1;
		//每次都更新
		_update_root_centers(sr);
		//if (m_append_counter % 10 == 0) {
		//	//定期写入数据
		//	_update_root_centers();
		//	write_root_centers(m_json_filename);
		//}
	}
	void CStemResultInfos::clear()
	{
		m_infos.clear();
	}
	void CStemResultInfos::get_root_centers(
		std::vector<CStemResult>&rst
	)
	{		
		rst.clear();
		for (auto& sr : m_root_centers) {
			rst.push_back(sr);
		}		
	}
	void CStemResultInfos::_update_root_centers(CStemResult& sr) {
		//直接在m_root_centers中找到最接近的中心，如果小于指定距离，更新m_root_centers
		double d1 = m_seedling_dist / 4.0;
		
		double d_min = 1.0e6;
		int min_root_idx = -1;
		for (int i = 0; i < m_root_centers.size(); ++i) {
			double dist = m_root_centers.at(i).calcluate_distance(sr);
			if (dist < d_min) {
				d_min = dist;
				min_root_idx = i;
			}
		}
		if (d_min < d1 && min_root_idx >= 0) {
			//更新指定中心
			double mu_x, mu_y, mu_z;
			CStemResult& min_root = m_root_centers.at(min_root_idx);
			
			mu_x = min_root.root_x;			
			if (min_root.root_count == 0) {
				mu_y = sr.root_y;
				mu_z = sr.root_z;
			}
			else {
				mu_y = (min_root.root_y * min_root.root_count +
					sr.root_y * sr.root_count) / (double)(min_root.root_count + sr.root_count);
				mu_z = (min_root.root_z * min_root.root_count +
					sr.root_z * sr.root_count) / (double)(min_root.root_count + sr.root_count);

			}	
			min_root.root_x = mu_x;
			min_root.root_y = mu_y;
			min_root.root_z = mu_z;
			min_root.root_count += sr.root_count;
		}
	}

	//void CStemResultInfos::_update_root_centers() {
	//	//依据m_infos中的数据，苗间距m_seedling_dist，聚类生成茎的平均位置，茎的点云平均数量
	//	//更新m_root_centers

	//	//以root_center为基础进行聚类，如果root_center是空的，因当前数据进行聚类
	//	std::vector<CStemResult> root_centers;
	//	double d1 = m_seedling_dist / 2.0;
	//	double d2 = d1*1.75;
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
	//	for (auto&sr : m_infos) {
	//		cloud2d_pos->points.push_back(pcl::PointXYZ((float)sr.root_x, 0.0, (float)sr.root_z));
	//	}
	//	cloud2d_pos->width = cloud2d_pos->points.size();
	//	std::vector<pcl::PointXYZ>cluster_center;
	//	std::vector<std::vector<int>> cluster_member;
	//	euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);

	//	for (size_t i = 0; i < cluster_center.size(); ++i) {
	//		double x = (double)cluster_center.at(i).x;
	//		double y = (double)cluster_center.at(i).y;
	//		double z = (double)cluster_center.at(i).z;
	//		int count = (int)cluster_member.at(i).size();
	//		int pc_size = 0.0;
	//		for (auto& idx : cluster_member.at(i)) {
	//			pc_size += m_infos.at(idx).stem_size;
	//		}
	//		pc_size = (int)(pc_size / (float)count);

	//		CStemResult sr = CStemResult(x, y, z, pc_size, std::string(""), count);
	//		root_centers.push_back(sr);
	//	}

	//	if (m_root_centers.size() == 0) {
	//		//如果没有历史数据，直接赋值
	//		m_root_centers = root_centers;
	//	}
	//	else {
	//		if (m_infos.size() > 50) {
	//			//如果当前数据量较大，用当前的结果
	//			m_root_centers = root_centers;
	//		}
	//		else {
	//			//否则将当前结果合并到m_root_centers
	//			//m_root_centers
	//			for (auto& cent : root_centers) {					
	//				double dist_min = d2;
	//				int min_root_idx = 0;

	//				for (int j = 0; j < m_root_centers.size();++j) {
	//					CStemResult& root = m_root_centers.at(j);
	//					double dist = root.calcluate_distance(cent);
	//					if (dist < dist_min) {
	//						dist_min = dist;
	//						min_root_idx = j;
	//					}
	//				}
	//				if (dist_min < d1) {
	//					//merge
	//					double mu_x, mu_y,mu_z;
	//					CStemResult& min_root = m_root_centers.at(min_root_idx);
	//					mu_x = (min_root.root_x * min_root.root_count +
	//						cent.root_x * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					mu_y = (min_root.root_y * min_root.root_count +
	//						cent.root_y * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					mu_z = (min_root.root_z * min_root.root_count +
	//						cent.root_z * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					min_root.root_x = mu_x;
	//					min_root.root_y = mu_y;
	//					min_root.root_z = mu_z;
	//					min_root.root_count += cent.root_count;
	//				}
	//				else {
	//					//add root
	//					m_root_centers.push_back(cent);
	//				}
	//			}
	//		}
	//	}
	//	//m_root_centers 排序、剔除
	//	std::sort(m_root_centers.begin(), m_root_centers.end(),
	//		[](const CStemResult& sr1, const CStemResult& sr2) {return sr1.root_x < sr2.root_x; });
	//	//nms		
	//	_filter_root_centers(d1, d2);
	//}
	//void CStemResultInfos::_filter_root_centers(double d1, double d2)
	//{
	//	//对生成的根中心m_root_centers进行过滤，剔除异常
	//	//1 z值，通过中值，距离中值远的剔除

	//	if (m_root_centers.size() > 3) {
	//		std::vector<double>root_z;
	//		for (auto&rc : m_root_centers) {
	//			root_z.push_back(rc.root_z);
	//		}
	//		std::sort(root_z.begin(), root_z.end());
	//		double mid_z = 0.0;
	//		if (root_z.size() % 2 == 1) {
	//			int idx = (m_root_centers.size() - 1) / 2;
	//			mid_z = root_z.at(idx);
	//		}
	//		else {
	//			int idx = root_z.size() / 2;
	//			mid_z = 0.5 * (root_z.at(idx) + root_z.at(idx - 1));
	//		}
	//		std::vector<CStemResult> valid_root_centers;
	//		for (auto&rc : m_root_centers) {
	//			if (fabs(rc.root_z - mid_z) < d2) {
	//				valid_root_centers.push_back(rc);
	//			}
	//		}
	//		m_root_centers = valid_root_centers;
	//	}

	//	//2 按x做分组，排除权重小的
	//	if (m_root_centers.size() > 3) {
	//		std::vector<double> center_dist;
	//		for (int i = 0; i < m_root_centers.size(); ++i) {
	//			double x = m_root_centers.at(i).root_x;
	//			double mod_sum = 0.0;
	//			for (int j = 0; j < m_root_centers.size(); ++j) {
	//				double dist = fabs(m_root_centers.at(j).root_x - x);
	//				double mod = fmod(dist, m_seedling_dist);
	//				double mod_inv = m_seedling_dist - mod;
	//				mod = min(mod, mod_inv);
	//				mod_sum += mod;
	//			}
	//			mod_sum /= (m_root_centers.size() - 1);
	//			center_dist.push_back(mod_sum);
	//		}

	//		std::vector<CStemResult> valid_root_centers;
	//		for (int i = 0; i < center_dist.size(); ++i) {
	//			double dist = center_dist.at(i);
	//			if (dist < d1) {
	//				valid_root_centers.push_back(m_root_centers.at(i));
	//			}
	//		}
	//		m_root_centers = valid_root_centers;
	//	}
	//	//3 数量少的剔除
	//	int total_count = 0;
	//	for (auto&rc : m_root_centers) {
	//		total_count += rc.root_count;
	//	}
	//	if (total_count > 50) {
	//		bool need_del = false;
	//		for (auto& rc : m_root_centers) {
	//			if (rc.root_count <= 2) {
	//				need_del = true;
	//				break;
	//			}
	//		}
	//		if (need_del) {
	//			std::vector<CStemResult> valid_root_centers;
	//			for (int i = 0; i < m_root_centers.size(); ++i) {					
	//				if (m_root_centers.at(i).root_count > 2) {
	//					valid_root_centers.push_back(m_root_centers.at(i));
	//				}
	//			}
	//			m_root_centers = valid_root_centers;
	//		}
	//		
	//	}
	//}
	void CStemResultInfos::gen_root_centers()
	{
		//根据 m_seedling_dist, m_holes_number， m_xmin， m_xmax生成初始的穴位中心
		//以m_xmin， m_xmax的中间点为中心，分别找到间隔m_seedling_dist的m_holes_number个穴位中心
		//初始的z设成-1，等待更新赋值
		double x_mid = 0.5 * (m_xmin + m_xmax);
		double holes_mid = 0.5 * (m_holes_number - 1) * m_seedling_dist; 
		double x0 = x_mid - holes_mid;
		double z_mid = 0.5 * (m_zmin + m_zmax);
		m_root_centers.clear();
		for (int i=0; i<m_holes_number; ++i) {
			double x = x0 + i * m_seedling_dist;
			double y = 0;
			double z = z_mid;
			int size = 0;
			int count = 0;
			CStemResult sr = CStemResult(x, y, z, size, std::string(""), count);
			m_root_centers.push_back(sr);
		}
	}

	//void CStemResultInfos::set_json_filename(std::string& filename) {
	//	m_json_filename = std::string(filename);
	//}
	//void CStemResultInfos::read_root_centers(
	//	std::string& filename
	//) 
	//{
	//	//读取历史数据，每次启动时，去取
	//	m_json_filename = std::string(filename);
	//	//1 文件是否存在
	//	ifstream f(filename.c_str());
	//	if (!f.good()) {
	//		//文件不存在
	//		if (m_pLogger) {
	//			stringstream buff;
	//			buff << m_pcdId << ": json file not exists:" << filename;
	//			m_pLogger->INFO(buff.str());
	//		}
	//		return;
	//	}	

	//	cv::FileStorage fs(filename, cv::FileStorage::READ);
	//	cv::FileNode root_centers = fs["root_centers"];
	//	cv::FileNodeIterator it = root_centers.begin(), it_end = root_centers.end();
	//	m_root_centers.clear();
	//	for (; it != it_end; ++it) {
	//		double x = (double)(*it)["x"];
	//		double y = (double)(*it)["y"];
	//		double z = (double)(*it)["z"];
	//		int size = (int)(*it)["size"];
	//		std::string bid = (std::string)(*it)["batch_id"];
	//		int count = (int)(*it)["count"];
	//		CStemResult sr = CStemResult(x,y,z,size, bid, count);
	//		m_root_centers.push_back(sr);
	//	}
	//	fs.release();

	//}
	//void CStemResultInfos::write_root_centers(
	//	std::string& filename
	//)
	//{
	//	cv::FileStorage fs(filename, cv::FileStorage::WRITE);
	//	fs << "root_centers" << "[";
	//	for (auto& sr : m_root_centers) {		
	//		fs << "{" << "x" << sr.root_x
	//			<< "y" << sr.root_y
	//			<< "z" << sr.root_z
	//			<< "size" << sr.stem_size
	//			<< "batch_id" << sr.batch_id	
	//			<< "count" << sr.root_count
	//			<< "}";
	//	}
	//	fs << "]";
	//	fs.release();

	//}

	//void CStemResultInfos::euclidean_clustering_ttsas(
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
	//	double d1, double d2,
	//	std::vector<pcl::PointXYZ>&cluster_center,
	//	std::vector<std::vector<int>> &clustr_member
	//)
	//{
	//	if (m_pLogger) {
	//		stringstream buff;
	//		buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() begin...";
	//		m_pLogger->INFO(buff.str());
	//	}
	//	std::vector<int> cluster_weight;
	//	std::vector<int> data_stat;

	//	std::vector<pcl::PointXYZ>cluster_center_raw;
	//	std::vector<std::vector<int>> clustr_member_raw;

	//	for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
	//	size_t data_len = in_cloud->points.size();
	//	int exists_change = 0;
	//	int prev_change = 0;
	//	int cur_change = 0;
	//	int m = 0;
	//	while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
	//		bool new_while_first = true;
	//		for (size_t i = 0; i < data_len; ++i) {
	//			if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
	//				new_while_first = false;
	//				std::vector<int> idx;
	//				idx.push_back(i);
	//				clustr_member_raw.push_back(idx);
	//				pcl::PointXYZ center;
	//				center.x = in_cloud->points.at(i).x;
	//				center.y = in_cloud->points.at(i).y;
	//				center.z = in_cloud->points.at(i).z;

	//				cluster_center_raw.push_back(center);
	//				data_stat.at(i) = 1;
	//				cur_change += 1;
	//				cluster_weight.push_back(1);
	//				m += 1;
	//			}
	//			else if (data_stat[i] == 0) {
	//				std::vector<float> distances;
	//				for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
	//					std::vector<float> distances_sub;
	//					for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
	//						size_t ele_idx = clustr_member_raw.at(j).at(jj);
	//						double d = sqrt(
	//							(in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
	//							(in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
	//							(in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z) * (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z));
	//						distances_sub.push_back(d);
	//					}
	//					double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
	//					distances.push_back(min_dist);
	//				}
	//				int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
	//				if (distances.at(min_idx) < d1) {
	//					data_stat.at(i) = 1;
	//					double w = cluster_weight.at(min_idx);
	//					cluster_weight.at(min_idx) += 1;
	//					clustr_member_raw.at(min_idx).push_back(i);
	//					cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
	//					cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
	//					cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
	//					cur_change += 1;
	//				}
	//				else if (distances.at(min_idx) > d2) {
	//					std::vector<int> idx;
	//					idx.push_back(i);
	//					clustr_member_raw.push_back(idx);
	//					pcl::PointXYZ center;
	//					center.x = in_cloud->points.at(i).x;
	//					center.y = in_cloud->points.at(i).y;
	//					center.z = in_cloud->points.at(i).z;

	//					cluster_center_raw.push_back(center);
	//					data_stat.at(i) = 1;
	//					cur_change += 1;
	//					cluster_weight.push_back(1);
	//					m += 1;
	//				}

	//			}
	//			else if (data_stat.at(i) == 1) {
	//				cur_change += 1;
	//			}
	//		}
	//		exists_change = fabs(cur_change - prev_change);
	//		prev_change = cur_change;
	//		cur_change = 0;
	//	}

	//	// copy result
	//	for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
	//		//if (clustr_member_raw.at(i).size() < 20) { continue; }
	//		cluster_center.push_back(cluster_center_raw.at(i));
	//		clustr_member.push_back(clustr_member_raw.at(i));
	//	}
	//	if (m_pLogger) {
	//		stringstream buff;
	//		buff << m_pcdId << ":  root center estimate: euclidean_clustering_ttsas() end";
	//		m_pLogger->INFO(buff.str());
	//	}
	//}

CSeedlingStatus::CSeedlingStatus(
	int dtype,
	double step,
	double x_min,
	double x_max,	
	double pc_mean_dist,
	CGcvLogger*pLog)
	: m_pLogger(pLog)
	, m_dtype(dtype)
	, m_bin_step(step)
	, m_xmin(x_min)
	, m_xmax(x_max)
	, m_record_cursor(-1)
	, m_max_size(50)
	, m_pc_mean_dist(pc_mean_dist)
{
	int x0 = int(x_min);
	int x1 = int(x_max);
	m_hist_length = int((x1 - x0) / step);	
	m_history_histogram = cv::Mat::zeros(m_max_size, m_hist_length, CV_32F);
	m_history_point_size = cv::Mat::zeros(m_max_size, 1, CV_32F);	
}

CSeedlingStatus::~CSeedlingStatus()
{}

void CSeedlingStatus::set_x_centers(std::vector<double>&cx)
{
	m_center_x.clear();
	for (auto&x : cx) {
		m_center_x.push_back(x);
	}
	std::sort(m_center_x.begin(), m_center_x.end());

	double seedling_distance = m_center_x.at(1) - m_center_x.at(0);
	m_idx_low.clear();
	m_idx_up.clear();
	for (int i = 0; i < m_center_x.size(); ++i) {
		int idx_low = int((m_center_x.at(i) - 0.5 * seedling_distance - m_xmin) / m_bin_step);
		int idx_up = int((m_center_x.at(i) + 0.5 * seedling_distance - m_xmin) / m_bin_step);
		m_idx_low.push_back(idx_low);
		m_idx_up.push_back(idx_up);
	}
	m_history_status = cv::Mat::zeros(m_max_size, m_center_x.size(), CV_8U);

}
void CSeedlingStatus::append_hist(
	std::vector<int>&xhist,		//input
	std::vector<bool>&xstatus	//output
)
{
	assert(xhist.size() == m_hist_length);
	
	if (m_record_cursor < m_max_size) {
		m_record_cursor++;
		float pc_size = 0.0;
		for (int i = 0; i < xhist.size(); ++i) {
			m_history_histogram.at<float>(m_record_cursor,i) = xhist.at(i);
			pc_size += xhist.at(i);
		}
		m_history_point_size.at<float>(m_record_cursor, 0) = pc_size;
		get_status(xstatus);		
	}
	else {
		//数据上移一行，数据放在最后一行
		memcpy_s(m_history_histogram.data,
			m_history_histogram.step[0] * (m_max_size - 1),
			m_history_histogram.data + m_history_histogram.step[0], 
			m_history_histogram.step[0] * (m_max_size - 1));

		memcpy_s(m_history_point_size.data,
			m_history_point_size.step[0] * (m_max_size - 1),
			m_history_point_size.data + m_history_point_size.step[0],
			m_history_point_size.step[0] * (m_max_size - 1));

		float pc_size = 0.0;
		for (int i = 0; i < xhist.size(); ++i) {
			m_history_histogram.at<float>(m_max_size - 1, i) = float(xhist.at(i));
			pc_size += xhist.at(i);
		}
		m_history_point_size.at<float>(m_max_size - 1, 0) = pc_size;
		get_status(xstatus);
	}
	

}
void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
{
	xstatus.clear();
	xstatus.assign(m_center_x.size(), false);

	//1 如果没有记录输入，返回没有苗
	if (m_record_cursor < 0) { return; }

	//2 如果是第一次，没有参考，用自身的分布阈值判断是否有苗（可能不准确，但没有其他办法）
	//点云分布情况分析
	// 每个bin的数量阈值设定m_bin_step宽度，至少有10毫米的点云，才认为有苗
	float th_hist = m_bin_step * 10 / m_pc_mean_dist / m_pc_mean_dist; 
	if (m_record_cursor == 0) {
		std::vector<bool> hist_status;		
		hist_status.assign(m_hist_length, false);
		for (int i = 0; i < m_hist_length; ++i) {
			if (m_history_histogram.at<float>(m_record_cursor, i) > th_hist) {
				hist_status.at(i) = true;
			}
		}
		for (int i = 0; i < m_center_x.size(); ++i) {
			int idx_low = m_idx_low.at(i);
			int idx_up = m_idx_up.at(i);
			int  valid_bin_cnt = 0;
			for (int k = idx_low; k < idx_up; ++k) {
				if (hist_status.at(k)) { valid_bin_cnt++; }
			}
			double valid_ratio = (double)valid_bin_cnt / (double)(idx_up - idx_low);
			xstatus.at(i) = valid_ratio > 0.5;			
		}
		//update m_history_status
		for (int i = 0; i < m_history_status.cols; ++i) {
			m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
		}
		return;
	}

	//3 2次或更多，通过前后2次差分析苗取走的情况
	//3.1 计算被取走的点云位置分布
	std::vector<float>hist_diff;
	hist_diff.assign(m_hist_length, 0.0);
	float sum_dn = 0.0;
	float sum_n = 0.0;
	float diff_cnt = 0.0;
	if (m_record_cursor < m_max_size) {
		for (int i = 0; i < m_hist_length; ++i) {
			diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) - 
				m_history_histogram.at<float>(m_record_cursor, i);
			hist_diff.at(i) =  diff_cnt;
			sum_n += diff_cnt;
			sum_dn += diff_cnt * i;
		}
	}
	else {
		for (int i = 0; i < m_hist_length; ++i) {
			diff_cnt = m_history_histogram.at<float>(m_max_size - 2, i) -
				m_history_histogram.at<float>(m_max_size - 1, i);
			hist_diff.at(i) = diff_cnt;
			sum_n += diff_cnt;
			sum_dn += diff_cnt * i;
		}

	}
	
	//3.2 统计增减点云的状态，区分点云增加，点云减小，点云没变化的部分
	std::vector<int> hist_status_2d;	//3种状态记录： -1取走，0没变化，1上苗
	hist_status_2d.assign(m_hist_length, 0);
	int add_cnt = 0;
	int sub_cnt = 0;
	for (int i = 0; i < m_hist_length; ++i) {
		if (hist_diff.at(i) > th_hist) {
			hist_status_2d.at(i) = -1;
			sub_cnt += 1;
		}
		if (hist_diff.at(i) < -th_hist) {
			hist_status_2d.at(i) = 1;
			add_cnt += 1;
		}
	}

	//3.3 判断苗的整体情况
	double seedling_distance = m_center_x.at(1) - m_center_x.at(0); //株间距离
	double grid_one_seedling = seedling_distance / m_bin_step;		//每穴位占histogram的桶数
	//3.3.1进一排苗
	if (add_cnt > grid_one_seedling*3.0) {		
		xstatus.assign(m_center_x.size(), true);
		//update m_history_status
		if (m_record_cursor < m_max_size) {
			for (int i = 0; i < m_history_status.cols; ++i) {
				m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
			}
		}
		else {
			memcpy_s(m_history_status.data,
				m_history_status.step[0] * (m_max_size - 1),
				m_history_status.data + m_history_status.step[0],
				m_history_status.step[0] * (m_max_size - 1));
			
			for (int i = 0; i < m_history_status.cols; ++i) {
				m_history_status.at<unsigned char>(m_max_size-1, i) = xstatus.at(i);
			}
		}		
		return;
	}

	std::vector<size_t>sorted_idx;
	std::vector<float>sub_seedling_score;	//移出植株得分，记录每个穴位上点云变化得分
	//3.3.2 变化很小，说明没有改变（没能成功抓走）
	if (add_cnt + sub_cnt < 0.5 * grid_one_seedling) {
		goto no_change;
	}
	//3.3.3 否则的话，就是抓走过一个苗
	//找到被取走的苗的中心，然后根据dtype确定有苗的位置
	//找覆盖范围最大的区域
	double sub_cent_indx = sum_dn / sum_n;	//计算改变范围的中心，目前没用到	
	sub_seedling_score.assign(m_center_x.size(), 0.0);
	for (int idx = 0; idx < hist_status_2d.size(); ++idx) {
		if (hist_status_2d.at(idx) >= 0) {
			//这个histogram上没有移出，不统计， hist_status_2d的值域：-1取走，0没变化，1上苗
			continue; 
		}
		for (int i = 0; i < m_center_x.size(); ++i) {
			int idx_low = m_idx_low.at(i);
			int idx_up = m_idx_up.at(i);
			if (idx >= idx_low && idx < idx_up) {
				sub_seedling_score.at(i) += 1.0;
			}
		}
	}
	int sub_pos = -1;
	sorted_idx = sort_indexes_e(sub_seedling_score, false);
	for (auto& idx : sorted_idx) {
		if (sub_seedling_score.at(idx) < 0.25 * grid_one_seedling) {
			//如果改变量，不到穴位范围的一半，不认为是移走的
			continue; 
		}
		if (m_history_status.at<unsigned char>(m_record_cursor - 1, idx) == 0) { 
			//如果这个位置上一帧就没有苗，那判别也是错误的
			continue; 
		}
		sub_pos = idx;
		break;//找到得分最高，并且满足条件的位置，就是被抓走的位置，跳出
	}
	if (sub_pos >= 0) {
		xstatus.assign(m_center_x.size(), false);
		int cursor = m_record_cursor - 1;
		if (m_record_cursor < m_max_size) {
			cursor = m_record_cursor - 1;		
		}
		else {
			cursor = m_max_size - 1;			
		}
		for (int i = 0; i < m_history_status.cols; ++i) {
			if (m_history_status.at<unsigned char>(cursor, i) == 1) {
				xstatus.at(i) = true;					
			}
		}
		xstatus.at(sub_pos) = false;
		
		//update m_history_status
		if (m_record_cursor < m_max_size) {
			for (int i = 0; i < m_history_status.cols; ++i) {
				m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
			}
		}
		else{
			memcpy_s(m_history_status.data,
				m_history_status.step[0] * (m_max_size - 1),
				m_history_status.data + m_history_status.step[0],
				m_history_status.step[0] * (m_max_size - 1));
			for (int i = 0; i < m_history_status.cols; ++i) {
				m_history_status.at<unsigned char>(m_max_size - 1, i) = xstatus.at(i);
			}
		}		
		return;
	}
	else {
		//如果没有找到有效位置，按没有变化处理
		goto no_change;
	}

no_change:
	//没有改变,用上一次的结果
	xstatus.assign(m_center_x.size(), true);
	//update m_history_status
	if (m_record_cursor < m_max_size) {
		for (int i = 0; i < m_history_status.cols; ++i) {
			m_history_status.at<unsigned char>(m_record_cursor, i) = 
				m_history_status.at<unsigned char>(m_record_cursor - 1, i);
			if (m_history_status.at<unsigned char>(m_record_cursor - 1, i) == 0) {
				xstatus.at(i) = false;
			}
		}
	}
	else{
		memcpy_s(m_history_status.data,
			m_history_status.step[0] * (m_max_size - 1),
			m_history_status.data + m_history_status.step[0],
			m_history_status.step[0] * (m_max_size - 1));
		for (int i = 0; i < m_history_status.cols; ++i) {
			m_history_status.at<unsigned char>(m_max_size - 1, i) = 
				m_history_status.at<unsigned char>(m_max_size - 2, i);
			if (m_history_status.at<unsigned char>(m_max_size - 2, i) == 0) {
				xstatus.at(i) = false;
			}
		}
	}
}

}