#pragma once
//#define EIGEN_USE_MKL_ALL
//#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>
// 本地方法

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <omp.h>
#include "baseTool.h"
#include "simptsn.h"
#include "SateOrbit.h"
#include <gdal_utils.h>
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include "ImageMatch.h"
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace Eigen;
using namespace cv;
/// <summary>
/// cpnvert  gdal to jpg 
/// </summary>
/// <param name="gdal_path"></param>
/// <param name="jpg_path"></param>
/// <param name="band_ids"></param>
/// <returns></returns>
int ImageMatch::gdal2JPG(std::string gdal_path, std::string jpg_path, int band_ids)
{
	std::cout << "convert gdal to jpg , beigining : \t" << getCurrentTimeString() << endl;
	gdalImage gdalimg(gdal_path);
	cv::Mat img(gdalimg.height, gdalimg.width, CV_8U);
	{

		int start_ids = 0;
		int line_invert = int(80000000 / gdalimg.width);
		Eigen::MatrixXd temp(line_invert, gdalimg.width);
		double min_value = 0, temp_min = 0;
		double max_value = 0, temp_max = 0;
		//  线性拉伸2%
		Eigen::MatrixXd hist = gdalimg.getHist(band_ids);
		
		int count = 0;
		int sum_count = gdalimg.height * gdalimg.width;
		int rows = hist.rows();
		for (int i = 0; i < rows; i++) {
			
			if ((count+ hist(i, 1)) / sum_count > 0.01) {
				min_value = hist(i, 0);
				break;
			}
			count = count + hist(i, 1);
		}
		count = 0;
		for (int i = rows - 1; i >= 0; i--) {
			if ((count + hist(i, 1)) / sum_count > 0.01) {
				max_value = hist(i, 0);
				break;
			}
			count = count + hist(i, 1);
		}

		//  重新缩放最大值，最小值
		std::cout << "min value ：\t" << min_value << "\n";
		std::cout << "max value ：\t" << max_value << "\n";
		start_ids = 0;
		do {
			line_invert = line_invert + start_ids < gdalimg.height ? line_invert : gdalimg.height - start_ids;

			temp = gdalimg.getData(start_ids, 0, line_invert, gdalimg.width, band_ids);
			temp = (temp.array() - min_value) * 250 / (max_value - min_value);
			for (int i = 0; i < line_invert; i++) {
				for (int j = 0; j < gdalimg.width; j++) {
					if (temp(i, j) < 0) { temp(i, j) = 0; }
					if (temp(i, j) > 250) { temp(i, j) = 250; }
					uchar tempvalue = uchar(temp(i, j));;
					img.at<uchar>(i + start_ids, j) = tempvalue;
				}
			}
			start_ids = start_ids + line_invert;
		} while (start_ids < gdalimg.height);
		cv::Mat  result1(gdalimg.height, gdalimg.width, CV_8U);
		result1 = img;
		//bilateralFilter(img, result1, 10, 80, 50);
		vector<int> compression_params;
		compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);  //选择jpeg
		compression_params.push_back(100); //在这个填入你要的图片质量
		cv::Mat out = result1;
		//cv::resize(img, out, cv::Size(img.cols, img.rows), cv::INTER_AREA);

		cv::imwrite(jpg_path, out, compression_params);
		
	}
	std::cout << "convert gdal to jpg , overing : \t" << getCurrentTimeString() << endl;
	std::cout << "=========================================\n";
	std::cout << " convert gdal to jpg :\n";
	std::cout << "input gdal img file path ：\t" << gdal_path << "\n";
	std::cout << "input gdal img band ：\t" << band_ids << "\n";
	std::cout << "out jpg file path ：\t" << jpg_path << "\n";
	std::cout << "=========================================\n";
	return 0;
}



/// <summary>
/// 获取模型匹配点
/// </summary>
/// <param name="ori_power_path"></param>
/// <param name="sim_sum_path"></param>
/// <returns></returns>
Eigen::MatrixXd ImageMatch::ImageMatch_ori_sim(std::string ori_power_path, std::string sim_sum_path, int roughSize, int PreciseSize, int scale, int searchSize, int roughStep,int preciseStep)
{
	std::cout << "match point between ori and sim  , overing : \t" << getCurrentTimeString() << endl;
	std::cout << "match parameters : \n";
	std::cout << "rough match templet size : \t" << roughSize << endl;
	std::cout << "Precise match templet size : \t" << PreciseSize << endl;
	std::cout << "Precise  match wait search  size : \t" << searchSize << endl;
	std::cout << "Precise match scale : \t" << scale << endl;
	std::cout << "Precise match step : \t" << preciseStep << endl;
	std::cout << "input ori image path : \t" << ori_power_path << endl;
	std::cout << "input sim image path : \t" << sim_sum_path << endl;

	//读取影像


	cv::Mat ori = openJPG(ori_power_path);
	cv::Mat sim = openJPG(sim_sum_path);
	int sim_rows = sim.rows;
	int sim_cols = sim.cols;

	int BlockCount = 0;
	for (int i = 0; i < sim_rows; i=i+ roughSize) {
		for (int j = 0; j < sim_cols; j=j+ roughSize) {
			if (i + 2 * roughSize < sim_rows && j + 2 * roughSize < sim_cols) {
				BlockCount++;
			}
		}
	}

	Eigen::MatrixXd sim_block(BlockCount,3);
	BlockCount = 0;
	for (int i = 0; i < sim_rows; i = i + roughSize) {
		for (int j = 0; j < sim_cols; j = j + roughSize) {
			if (i + 2 * roughSize < sim_rows && j + 2 * roughSize < sim_cols) {
				sim_block(BlockCount, 0) = i;
				sim_block(BlockCount, 1) = j;
				BlockCount++;
			}
		}
	}

	Eigen::MatrixXd Tempmatchpoints(BlockCount, 6);
	Tempmatchpoints = Tempmatchpoints.array() * 0;
	int count = 0;
#pragma omp parallel for num_threads(8) 
	for (int ii = 0; ii < BlockCount; ii++) {
		int i = sim_block(ii, 0);
		int j = sim_block(ii, 1);
		cv::Mat templet_mat = sim(Rect(j, i, 2 * roughSize, 2 * roughSize));
		cv::Scalar mean1;
		cv::Mat stddevMat;
		cv::meanStdDev(templet_mat, mean1, stddevMat);
		double minvalue = 0;
		double maxvalue = 0;
		cv::minMaxLoc(templet_mat, &minvalue, &maxvalue, NULL, NULL);//用于检测矩阵中最大值和最小值的位置
		double sig = (stddevMat.at<double>(0, 0)) / (maxvalue - minvalue);
		if (sig >1) {
			//continue;
		}

		// 构建搜索域
		int search_i = i - 2 * searchSize >= 0 ? i - 2 * searchSize >= 0 : 0;
		int search_j = j - 2 * searchSize >= 0 ? j - 2 * searchSize >= 0 : 0;
		int len_i = search_i + 4 * searchSize;
		int len_j = search_j + 4 * searchSize;
		len_i = search_i + len_i < ori.rows ? len_i : ori.rows - search_i - 1;
		len_j = search_j + len_j < ori.cols ? len_j : ori.rows - search_j - 1;
		cv::Mat serch = ori(Rect(search_j, search_i, len_j, len_i));
		// 检索
		cv::Mat result;
		matchTemplate(serch, templet_mat, result, cv::TM_CCOEFF_NORMED);
		Point minLoc;
		Point maxLoc;
		Point matchLoc;
		double tempminVal = 100, tempmaxVal = 0;
		cv::minMaxLoc(result, &tempminVal, &tempmaxVal, &minLoc, &maxLoc);//用于检测矩阵中最大值和最小值的位置
		double offset_j = maxLoc.x + search_j;
		double offset_i = maxLoc.y + search_i;

		//cv::Scalar mean1;
		//cv::Mat stddevMat;
		//cv::meanStdDev(templet_mat, mean1, stddevMat);


		Tempmatchpoints(ii, 0) = double(j); //sim_x
		Tempmatchpoints(ii, 1) = double(i); //sim_y
		Tempmatchpoints(ii, 2) = offset_j; // sim_x
		Tempmatchpoints(ii, 3) = offset_i; // sim_y
		Tempmatchpoints(ii, 4) = tempmaxVal; // maxVal
		Tempmatchpoints(ii, 5) = sig; // maxVal

		std::cout << 100.0*count/ BlockCount<<"\t:\t"<< count << " / " << BlockCount<<"\t" << Tempmatchpoints(ii, 0) << "\t" << Tempmatchpoints(ii, 1) << "\t" << Tempmatchpoints(ii, 2)
			<< "\t" << Tempmatchpoints(ii, 3) << "\t" << Tempmatchpoints(ii, 4) << "\t" << Tempmatchpoints(ii, 5) << endl;
		count++;
	}

	//BlockCount = 0;
	count = 0;
	for (int ii = 0; ii < BlockCount; ii++) {
		if (Tempmatchpoints(ii, 4) > 0.7) {
			std::cout << Tempmatchpoints(ii, 0) << "\t" << Tempmatchpoints(ii, 1) << "\t" << Tempmatchpoints(ii, 2)
				<< "\t" << Tempmatchpoints(ii, 3) << "\t" << Tempmatchpoints(ii, 4) << "\t" << Tempmatchpoints(ii, 5) << endl;
			//BlockCount++;
			count++;
		}
	}



	Eigen::MatrixXd matchpoints(count, 5);
	count = 0;
	for (int ii = 0; ii < BlockCount; ii++) {
		if (Tempmatchpoints(ii, 4) > 0.7) {
			matchpoints(count, 0)=Tempmatchpoints(ii, 0); //sim_x
			matchpoints(count, 1) = Tempmatchpoints(ii, 1); //sim_y
			matchpoints(count, 2) = Tempmatchpoints(ii, 2);// sim_x
			matchpoints(count, 3) = Tempmatchpoints(ii, 3);// sim_y
			matchpoints(count, 4) = Tempmatchpoints(ii, 4); // maxVal
			count++;
		}
	}


	/*
	// step 1: 粗匹配，分块均匀匹配
	std::cout << "rough match , begining : \t" << getCurrentTimeString() << endl;
	double offset_x = 0, offset_y = 0;
	cv::Mat mask = sim;
	cv::Mat temp_ori = ori;
	int offsetcount = 0;
	Eigen::MatrixXd  matchpoints;
	{
		int row_count = ori.rows;
		int col_count = ori.cols;
		int count = 0;
		for (int i = roughSize; i < row_count; i = i + roughSize + 500) { // y
			for (int j = roughSize; j < col_count; j = j + roughSize + 200) { //x 
				count = count + 1;
			}
		}
		matchpoints= Eigen::MatrixXd::Zero(count, 5);//ori_x,ori_y,sim_x,sim_y,maxval

		double minVal = 100, maxVal = 0.7;
		omp_lock_t lock;
		std::cout << "ori_x\tori_y\tsim_x\tsim_y\tmaxval \t" << endl;
		omp_init_lock(&lock); // 初始化互斥锁
		count = 0;
		int search_count = 0;
		#pragma omp parallel for num_threads(8) 
		for (int i = roughSize; i < row_count; i = i + roughSize+ 500) { // y
			double tempminVal = 100, tempmaxVal = 0;
			cv::Mat templet_mat;
			cv::Mat result;
			for (int j = roughSize; j < col_count; j = j + roughSize+ 200) { //x 
				templet_mat = ori(Rect(j - roughSize, i - roughSize, roughSize, roughSize));
				matchTemplate(sim, templet_mat, result, cv::TM_CCOEFF_NORMED);
				//normalize(result, result, 1, 0, cv::NORM_MINMAX);
				// 通过函数 minMaxLoc 定位最匹配的位置; 
				omp_set_lock(&lock); //获得互斥器
				Point minLoc;
				Point maxLoc;
				Point matchLoc;
				cv::minMaxLoc(result, &tempminVal, &tempmaxVal, &minLoc, &maxLoc);//用于检测矩阵中最大值和最小值的位置
				if (tempmaxVal >= maxVal) {
					offset_x = maxLoc.x - (j - roughSize);
					offset_y = maxLoc.y - (i - roughSize);
					//maxVal = tempmaxVal;

					matchpoints(count, 0) = (j - roughSize); //ori_x
					matchpoints(count, 1) = (i - roughSize); //ori_y
					double temp= maxLoc.x;
					matchpoints(count, 2) = temp; // sim_x
					temp = maxLoc.y;
					matchpoints(count, 3) = temp; // sim_y
					matchpoints(count, 4) = tempmaxVal; // maxVal
					count = count + 1;
					
					offsetcount += 1;
				}
				search_count = search_count + 1;
				std::cout << j - roughSize << "\t" << i - roughSize << "\t" << maxLoc.x << "\t" << maxLoc.y << "\t" << tempmaxVal << "\t" << search_count << "\t" << matchpoints.rows() << endl;
				omp_unset_lock(&lock); //释放互斥器
			}
		}
		omp_destroy_lock(&lock); //销毁互斥器
		offset_x = offset_x*1.0 / offsetcount;
		offset_y = offset_y * 1.0 / offsetcount;
		std::cout << "rough match point  : "<< offsetcount <<"\n" << endl;
		std::cout << "offset X  : \t" << offset_x << endl;
		std::cout << "offset Y  : \t" << offset_y << endl;
		std::cout << "maxVal  : \t" << maxVal << endl;
	}
	std::cout << "rough match out  : \t" << getCurrentTimeString() << endl;
	// step1.1: 粗匹配绘制结果

	std::string rough_math_path = sim_sum_path;
	boost::algorithm::replace_last(rough_math_path, ".", "_ori_in_sim.");
	std::cout << "ori in sim :\t" << rough_math_path << endl;
	cv::Mat ori_in_sim = sim;
	cv::rectangle(ori_in_sim, cv::Point(offset_x, offset_y), Point(offset_x + ori.cols, offset_y + ori.rows), Scalar(0, 255, 0), 2, 8, 0);

	vector<int> compression_params;
	compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);  //选择jpeg
	compression_params.push_back(100); //在这个填入你要的图片质量
	cv::Mat out = ori_in_sim;
	cv::imwrite(rough_math_path, out, compression_params);


	if (offsetcount == 0) { // 表示全局粗匹配失败，无法进行精校准，考虑直接定位法
		std::cout << "there are not effective point in  rought match \t" << endl;
		return Eigen::MatrixXd(0, 5);
	}

	Eigen::MatrixXd  matchpointstemp(offsetcount,5);
	for (int i = 0; i < matchpoints.rows(); i++) {
		if (matchpoints(i, 4) > 0.7) {
			for (int j = 0; j < matchpoints.cols(); j++) {
				matchpointstemp(i, j) = matchpoints(i, j);
			}
		}
	}
	matchpoints = matchpointstemp;
	*/


	//std::cout << "rough match , overing : \t" << getCurrentTimeString() << endl;
	//// step 2: 精匹配，
	//std::cout << "Precise match , begining : \t" << getCurrentTimeString() << endl;
	//Eigen::MatrixXd matchpoints;
	//{

	//	int row_count = ori.rows;
	//	int col_count = ori.cols;

	//	int count = 0;

	//	for (int i = PreciseSize; i < row_count; i = i + preciseStep) { // y
	//		for (int j = PreciseSize; j < col_count; j = j + preciseStep) { //x 
	//			count = count + 1;
	//		}
	//	}

	//	matchpoints(count, 5);//ori_x,ori_y,sim_x,sim_y,maxval
	//	double templeta_size = PreciseSize * scale;
	//	double search_size = searchSize * scale;
	//	count = 0;
	//	double ori_start_x, ori_start_y;
	//	double sim_start_x, sim_start_y;
	//	row_count = row_count - PreciseSize;
	//	col_count = col_count - PreciseSize;
	//	omp_lock_t lock;
	//	omp_init_lock(&lock); // 初始化互斥锁
	//	// 以搜索范围为核心计算
	//	#pragma omp parallel for num_threads(8) 
	//	for (int i = searchSize + offset_y; i < row_count; i = i + preciseStep) { // y
	//		cv::Mat templet_mat;
	//		cv::Mat search_mat;
	//		cv::Mat resample_templet_mat;
	//		cv::Mat resample_search_mat;
	//		cv::Mat result;
	//		double minVal = 100, maxVal = 0;
	//		for (int j = searchSize + offset_x; j < col_count; j = j + preciseStep) { //x 
	//			// 计算 起始点
	//			sim_start_x = j - searchSize;
	//			sim_start_y = i - searchSize;

	//			ori_start_x = (j - searchSize / 2) - PreciseSize / 2;
	//			ori_start_y = (i - searchSize / 2) - PreciseSize / 2;
	//			// 匹配模板，待匹配模板
	//			templet_mat = ori(Rect(ori_start_x, ori_start_y, PreciseSize, PreciseSize));
	//			search_mat = ori(Rect(sim_start_x, sim_start_y, searchSize, searchSize));

	//			resample_templet_mat = resampledMat(templet_mat, templeta_size);
	//			resample_search_mat = resampledMat(search_mat, search_size);

	//			matchTemplate(sim, templet_mat, result, cv::TM_CCORR_NORMED);
	//			// 通过函数 minMaxLoc 定位最匹配的位置; 
	//			cv::Point minLoc; cv::Point maxLoc;
	//			cv::Point matchLoc;
	//			cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc);//用于检测矩阵中最大值和最小值的位置
	//			if (maxVal > 0.7) {
	//				omp_set_lock(&lock); //获得互斥器
	//			
	//				matchpoints(count, 0) = ori_start_x; //ori_x
	//				matchpoints(count, 1) = ori_start_y; //ori_y

	//				matchpoints(count, 2) = sim_start_x + maxLoc.x * 1.0 / scale; // sim_x
	//				matchpoints(count, 3) = sim_start_y + maxLoc.y * 1.0 / scale; // sim_y
	//				matchpoints(count, 4) = maxVal; // maxVal
	//				count = count + 1;
	//			
	//				omp_unset_lock(&lock); //释放互斥器
	//			}
	//		}
	//	}
	//	omp_destroy_lock(&lock); //销毁互斥器

	//}
	//
	std::cout << "Precise match , ending : \t" << getCurrentTimeString() << endl;
	std::cout << "=======================================================================" << endl;
	std::cout << "match point result: \t" << getCurrentTimeString() << endl;
	std::cout << "=======================================================================" << endl;
	
	{
		std::cout << "ori_x\tori_y\tsim_x\tsim_y\tmaxval \t" << endl;
		int count = matchpoints.rows();
		for (int i = 0; i < count; i++) {
			std::cout << matchpoints(i, 0) << "\t" << matchpoints(i, 1) << "\t" << matchpoints(i, 2) << "\t" << matchpoints(i, 3) << "\t" << matchpoints(i, 4) << endl;
		}

	}
	std::cout << "=======================================================================" << endl;
	return matchpoints;
}

Eigen::MatrixXd ImageMatch::CreateMatchModel(Eigen::MatrixXd offsetXY_matrix)
{
	// ori_x,oir_y,to_x,to_y,maxval
	// 0      1		2	  3	   4
	Eigen::MatrixXd offset_x= offsetXY_matrix.col(2) - offsetXY_matrix.col(0);
	Eigen::MatrixXd offset_y = offsetXY_matrix.col(3) - offsetXY_matrix.col(1);
	// 计算最小二乘法模型

	Eigen::MatrixXd temp(offset_x.rows(), 6);
	temp.col(0) = temp.col(0).array()*0+1;  //1
	temp.col(1) = offsetXY_matrix.col(3).array(); // r
	temp.col(2) = offsetXY_matrix.col(2).array(); // c
	temp.col(3) = offsetXY_matrix.col(3).array().pow(2);//r2
	temp.col(4) = offsetXY_matrix.col(2).array().pow(2);//c2
	temp.col(5) = offsetXY_matrix.col(2).array()* offsetXY_matrix.col(3).array();//r*c
	Eigen::MatrixXd  matchmodel(2, 6);
	Eigen::MatrixXd tempx= temp.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(offset_x);//x c
	Eigen::MatrixXd tempy = temp.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(offset_y);//y r
	matchmodel.row(1) = tempx;
	matchmodel.row(0) = tempy;
	return matchmodel;
}

Eigen::MatrixXd ImageMatch::correctMatchModel(Eigen::MatrixXd r, Eigen::MatrixXd c)
{
	Eigen::MatrixXd a0 = Eigen::MatrixXd::Ones(1,r.cols());
	Eigen::MatrixXd a1 = r.array();
	Eigen::MatrixXd a2 = c.array();
	Eigen::MatrixXd a3 = r.array().pow(2);
	Eigen::MatrixXd a4 = c.array().pow(2);
	Eigen::MatrixXd a5 = r.array() * c.array();


	Eigen::MatrixXd offset(2, r.cols());//r,c
	offset.row(0) = r.array() + this->match_model(0, 0) * a0.array() + this->match_model(0, 1) * a1.array() + this->match_model(0, 2) * a2.array() + this->match_model(0, 3) * a3.array() + this->match_model(0, 4) * a4.array() + this->match_model(0, 5) * a5.array();
	offset.row(1) = c.array() + this->match_model(1, 0) * a0.array() + this->match_model(1, 1) * a1.array() + this->match_model(1, 2) * a2.array() + this->match_model(1, 3) * a3.array() + this->match_model(1, 4) * a4.array() + this->match_model(1, 5) * a5.array();

	return offset;
}

int ImageMatch::outMatchModel(std::string matchmodel_path)
{
	ofstream fout(matchmodel_path, ios::trunc);
	fout << "model:" << endl;
	fout << this->match_model(0, 0) << " " << this->match_model(0, 1) << " " << this->match_model(0, 2) << " " << this->match_model(0, 3) << " " << this->match_model(0, 4) << " " << this->match_model(0, 5) << endl;
	fout << this->match_model(1, 0) << " " << this->match_model(1, 1) << " " << this->match_model(1, 2) << " " << this->match_model(1, 3) << " " << this->match_model(1, 4) << " " << this->match_model(1, 5) << endl;
	fout << "model_points:" << endl;
	std::cout << "ori_x\tori_y\tsim_x\tsim_y\tmaxval \t" << endl;
	int count = this->offsetXY_matrix.rows();
	for (int i = 0; i < count; i++) {
		std::cout << this->offsetXY_matrix(i, 0) << "\t" << this->offsetXY_matrix(i, 1) << "\t" << this->offsetXY_matrix(i, 2) << "\t" << this->offsetXY_matrix(i, 3) << "\t" << this->offsetXY_matrix(i, 4) << endl;
	}
	fout.close();
	return 0;
}

/// <summary>
/// 读取jpg 文件
/// </summary>
/// <param name="jpg_path"></param>
/// <returns></returns>
cv::Mat openJPG(std::string jpg_path)
{
	cv::Mat image = cv::imread(jpg_path);
	if (image.data == nullptr)                     //nullptr是c++11新出现的空指针常量
	{
		throw new exception("图片文件不存在");
	}
	return image;
}

cv::Mat resampledMat(cv::Mat& image, int targetSize, int interpolation)
{
	cv::Mat out;
	cv::resize(image, out, cv::Size(targetSize, targetSize), interpolation);
	return out;
}