#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
//#include <mkl.h>
#include <stdlib.h>
#include <direct.h>
// gdal
#include <proj.h>
#include <string>
#include "gdal_priv.h"
#include "ogr_geometry.h"
#include "gdalwarper.h"
#include "baseTool.h"
#include "simptsn.h"
using namespace std;
using namespace Eigen;

#include "test_moudel.h"


int test_main(int mode, string rootpath)
{
	switch (mode)
	{
	case 0:
	{
		test_mode_0();
		break;
	};
	case 1:
	{
		test_mode_1(rootpath);
		break;
	}
	case 7: {
		test_mode_7();
		break;
	}
	case 8: {
		std::string in_rpc_tiff = "D:\\MicroSAR\\C-SAR\\MicroWorkspace\\Ortho\\Temporary\\GF3_MYN_QPSI_011437_E99.2_N28.6_20181012_L1B_HH_L10003514912_db.tif";
		std::string out_lon_lat_path = "D:\\MicroSAR\\C-SAR\\MicroWorkspace\\Ortho\\Temporary\\RPC_ori_sim.tif";
		std::string dem_path = "D:\\MicroSAR\\C-SAR\\MicroWorkspace\\Ortho\\Temporary\\TestDEM\\mergedDEM.tif";
		simProcess process;
		//InitRPCIncAngle(std::string paras_path, std::string workspace_path, std::string out_dir_path, std::string in_dem_path, std::string in_rpc_lon_lat_path)
		std::cout << "==========================================================================" << endl;
		process.CreateRPC_refrenceTable(in_rpc_tiff, dem_path,out_lon_lat_path);
		std::cout << "==========================================================================" << endl;
		break;
	}
	default:
		test_ASF(rootpath);
		break;
	}
	return 0;
}


int test_mode_7() {
	//    
	std::cout << "mode 7: get rpc incident and local incident angle sar model:";
	std::cout << "SIMOrthoProgram.exe 7 in_parameter_path in_dem_path  in_gec_lon_lat_path  work_path taget_path  out_incident_angle_path  out_local_incident_angle_path";
	std::string parameter_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\orth_para.txt";
	std::string dem_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\TestDEM\\mergedDEM.tif";
	std::string in_gec_lon_lat_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\RPC_ori_sim.tif";
	std::string work_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary";
	std::string taget_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package";
	std::string out_incident_angle_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\inc_angle.tiff";
	std::string out_local_incident_angle_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\local_inc_angle.tiff";
	std::string out_incident_angle_geo_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\inc_angle_geo.tiff";
	std::string out_local_incident_angle_geo_path = "D:\\MicroSAR\\C-SAR\\Ortho\\Temporary\\package\\local_inc_angle_geo.tiff";
	simProcess process;
	//InitRPCIncAngle(std::string paras_path, std::string workspace_path, std::string out_dir_path, std::string in_dem_path, std::string in_rpc_lon_lat_path)
	std::cout << "==========================================================================" << endl;
	process.InitRPCIncAngle(parameter_path, work_path, taget_path, dem_path, in_gec_lon_lat_path, out_incident_angle_path, out_local_incident_angle_path, out_incident_angle_geo_path, out_local_incident_angle_geo_path);
	std::cout << "==========================================================================" << endl;
	return 0;
}


int test_mode_0()
{
	std::cout << "State:\tTEST MODE" << endl;
	test_LLA2XYZ_XYZ2LLA();
	return 0;
}

int test_mode_1(string rootpath)
{
	// .\baseTool\x64\Release\SIMOrthoProgram.exe 1 D:\MicroWorkspace\C-SAR\Ortho\Temporary\unpack\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_AHV_L10003923848\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_HH_L10003923848.tiff
//???????

	std::string root_path = rootpath;// "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary2";
	std::string workspace = "D:\\MicroSAR\\C-SAR\\MicroWorkspace\\Ortho\\Temporary";
	std::string parameter_path =workspace+ "\\package\\orth_para.txt"; //
	std::string dem_path = workspace + "\\TestDEM\\pindem.tif"; //
	std::string in_sar_path = workspace + "\\unpack\\GF3_KAS_FSI_020253_E110.8_N25.5_20200614_L1A_HHHV_L10004871459\\GF3_KAS_FSI_020253_E110.8_N25.5_20200614_L1A_HH_L10004871459.tiff"; //
	std::string work_path = workspace ; //
	std::string taget_path = workspace + "\\package"; //
	std::cout << "==========================================================================" << endl;
	std::cout << "in parameters:========================================================" << endl;
	std::cout << "parameters file path:\t" << parameter_path << endl;
	std::cout << "input dem image(WGS84)" << dem_path << endl;
	std::cout << "the sar image:\n" << in_sar_path << endl;
	std::cout << "the work path for outputing temp file :\t" << work_path << endl;
	std::cout << "the out file for finnal file:\t" << taget_path << endl;
	simProcess process;
	std::cout << "==========================================================================" << endl;
	process.InitSimulationSAR(parameter_path, work_path, taget_path, dem_path, in_sar_path);
	std::cout << "==========================================================================" << endl;
	return 0;
}


int test_ASF(string rootpath) {
	std::string root_path = rootpath;// "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary2";
	std::string parameter_path = root_path + "\\package\\orth_para.txt";
	std::string dem_path = root_path + "\\TestDEM\\mergedDEM.tif";
	std::string ori_sar_path = root_path + "\\unpack\\" + "GF3_KAS_FSI_020253_E110.8_N25.5_20200614_L1A_HHHV_L10004871459\\GF3_KAS_FSI_020253_E110.8_N25.5_20200614_L1A_HH_L10003923848.tiff";
	std::string work_path = root_path + "\\TestSim";
	std::string taget_path = root_path + "\\output";
	std::string out_GEC_dem_path = root_path + "\\output\\GEC_dem.tif";
	std::string out_GTC_rc_path = root_path + "\\output\\GTC_rc_wgs84.tif";
	std::string out_GEC_lon_lat_path = root_path + "\\output\\GEC_lon_lat.tif";
	std::string out_clip_DEM_path = root_path + "\\output\\Orth_dem.tif";
	std::cout << "==========================================================================" << endl;
	std::cout << "????????????????????DEM ??¦¶" << endl;
	std::cout << "SIMOrthoProgram.exe 1  in_parameter_path  in_dem_path  in_ori_sar_path  in_work_path  in_taget_path ";
	std::cout << "out_GEC_dem_path  out_GTC_rc_path  out_GEC_lon_lat_path  out_clip_dem_path" << endl;
	std::cout << "==========================================================================" << endl;
	std::cout << "in parameters:========================================================" << endl;
	std::cout << "parameters file path:\t" << parameter_path << endl;
	std::cout << "input dem image(WGS84)" << dem_path << endl;
	std::cout << "the reference sar image for match simulation sar image:\n" << ori_sar_path << endl;
	std::cout << "the work path for outputing temp file :\t" << work_path << endl;
	std::cout << "the out file for finnal file:\t" << taget_path << endl;
	simProcess process;
	ASFOrthClass ASFClass;
	std::cout << "==========================================================================" << endl;
	PSTNAlgorithm pstn(parameter_path);
	std::cout << "==========================================================================" << endl;
	// 
	dem_path = out_clip_DEM_path;// JoinPath(work_path, "clipDEM.tif");
	std::string out_simrc_path = JoinPath(work_path, "dem_sar_rc.tif");    //r,c
	std::string sim_sar_orth_path = JoinPath(work_path, "sim_sar_orth.tif");   // orth_sim,orth_inclocal

	std::string dem_slope_path = JoinPath(work_path, "dem_slope.tif");
	std::string dem_aspect_path = JoinPath(work_path, "dem_aspect.tif");

	std::string sim_sar_path = JoinPath(work_path, "sim_sar_sum.tif");  // sim count dem
	std::string ori_sar_rsc_path = JoinPath(work_path, "ori_sar_power.tif");  // ori_power
	std::string ori_sar_rsc_jpg_path = JoinPath(work_path, "ori_sar_power.jpg");
	std::string sim_sar_jpg_path = JoinPath(work_path, "sim_sar_sum.jpg");
	std::string matchmodel_path = JoinPath(work_path, "matchmodel.txt"); // matchmodel

	std::string out_dem_count_path = JoinPath(work_path, "dem_count.tif");

	std::string out_dem_path = out_GEC_dem_path;
	std::string out_inclocal_path = JoinPath(taget_path, "inclocal.tif");
	std::string out_sar_rc_path = out_GTC_rc_path;
	std::string out_lon_lat_path = out_GEC_lon_lat_path;

	if (process.isMatchModel) {
		process.correctOrth_rc(out_simrc_path, process.matchmodel);
	}
	if (boost::filesystem::exists(boost::filesystem::path(out_sar_rc_path))) {
		boost::filesystem::remove(boost::filesystem::path(out_sar_rc_path));
	}
	boost::filesystem::copy_file(boost::filesystem::path(out_simrc_path), boost::filesystem::path(out_sar_rc_path));

	process.correct_ati(out_sar_rc_path, dem_path, out_inclocal_path, out_dem_path, out_dem_count_path, pstn);
	// ASF

	process.ASF(out_dem_path, out_lon_lat_path,  ASFClass, pstn);

	return 0;
}


int test_mode_2()
{
	std::string parameter_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\orth_para.txt";
	std::string dem_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\RPC_Ortho\\RPC_DEM.tiff";
	std::string in_rc_wgs84_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\RPC_Ortho\\RPC_sar_rc.tiff";
	std::string out_incident_angle_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\RPC_Ortho\\RPC_incident_angle.tiff";
	std::string out_local_incident_angle_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\RPC_Ortho\\RPC_local_incident_angle.tiff";

	std::cout << "mode 2: get incident angle and local incident angle by rc_wgs84 and dem and statellite model:\n";
	std::cout << "SIMOrthoProgram.exe 2  in_parameter_path  in_dem_path  in_rc_wgs84_path  out_incident_angle_path  out_local_incident_angle_path";

	simProcess process;
	std::cout << "==========================================================================" << endl;
	PSTNAlgorithm pstn(parameter_path);
	std::cout << "==========================================================================" << endl;

	process.calcalIncident_localIncident_angle(dem_path, in_rc_wgs84_path, out_incident_angle_path, out_local_incident_angle_path, pstn);

	return 0;
}

int test_mode_3()
{
	std::string parameter_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\orth_para.txt";
	std::string in_rc_wgs84_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\GTC_rc_wgs84.tiff";
	std::string in_ori_sar_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\unpack\\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_AHV_L10003923848\\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_HH_L10003923848.tiff";
	std::string out_orth_sar_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\GTC\\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_HH_L10003923848.tiff";

	std::cout << "mode 3: interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model:\n ";
	std::cout << "SIMOrthoProgram.exe 3  in_parameter_path  in_rc_wgs84_path  in_ori_sar_path  out_orth_sar_path";

	simProcess process;
	std::cout << "==========================================================================" << endl;
	PSTNAlgorithm pstn(parameter_path);
	std::cout << "==========================================================================" << endl;
	process.interpolation_GTC_sar(in_rc_wgs84_path, in_ori_sar_path, out_orth_sar_path, pstn);

	return 0;
}

int test_mode_4()
{
	std::string parameter_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\orth_para.txt";
	std::string in_dem_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\Orth_dem.tiff";
	std::string in_rpc_rc_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\\RPC_Ortho\\RPC_sar_rc.tiff";
	std::string out_rpc_dem_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\RPC_Ortho\\RPC_DEM.tiff";
	std::string out_incident_angle_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\RPC_Ortho\\RPC_incident_angle.tiff";
	std::string out_local_incident_angle_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\RPC_Ortho\\RPC_local_incident_angle.tiff";

	std::cout << "mode 4: get RPC incident and local incident angle sar model:";
	std::cout << "SIMOrthoProgram.exe 4 in_parameter_path in_dem_path  in_rpc_rc_path  out_rpc_dem_path  out_incident_angle_path  out_local_incident_angle_path";

	simProcess process;
	std::cout << "==========================================================================" << endl;
	PSTNAlgorithm pstn(parameter_path);
	std::cout << "==========================================================================" << endl;
	process.CreateRPC_DEM(in_rpc_rc_path, in_dem_path, out_rpc_dem_path);
	process.calcalIncident_localIncident_angle(out_rpc_dem_path, in_rpc_rc_path, out_incident_angle_path, out_local_incident_angle_path, pstn);
	return 0;
}

int test_mode_5()
{
	std::string in_ori_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\output\\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_HH_L10003923848_GTC_geo_wp.tif";
	std::string out_power_path = "D:\\MicroSAR\\C-SAR\\SIMOrthoProgram\\Temporary\\package\\GF3_SAY_QPSI_013952_E118.9_N31.5_20190404_L1A_HH_L10003923848_GTC_geo_OrthoResult.tif";
	std::cout << "mode 5: convert ori tiff to power tiff:";
	std::cout << "SIMOrthoProgram.exe 5 in_ori_path out_power_path";
	simProcess process;
	process.ori_sar_power(in_ori_path, out_power_path);
	return 0;
}

int test_mode_6()
{

	return 0;
}

int test_LLA2XYZ_XYZ2LLA()
{
	std::cout << "===========================================" << endl;
	std::cout << "TEST LLA2XYZ and XYZ2LLA " << endl;
	bool pass = false;

	Landpoint lla_p{ 30,40,500 };
	Landpoint xyz_p = LLA2XYZ(lla_p);
	Landpoint lla_p2 = XYZ2LLA(xyz_p);

	std::cout << "LLA:\t" << lla_p.lon << "," << lla_p.lat << "," << lla_p.ati << endl;
	std::cout << "XYZ:\t" << xyz_p.lon << "," << xyz_p.lat << "," << xyz_p.ati << endl;
	std::cout<<"LLA2:\t"<< lla_p2.lon << "," << lla_p2.lat << "," << lla_p2.ati << endl;
	
	pass = (lla_p2.lon-lla_p.lon==0) && (lla_p2.lat- lla_p.lat==0) && (lla_p2.ati-lla_p.ati==0);
	std::cout << "TEST LLA2XYZ and XYZ2LLA passed:" << pass << endl;
	std::cout << "===========================================" << endl;
	return pass?1:0;
}

int test_vector_operator()
{
	// ????
	return 0;
}