/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#include "interpolate.h"

//一维线性插值
double gctl::line_interpolate(double x1,double x2,double y1,double y2,double x)
{
	if (x1 == x2) return 0.5*(y1 + y2);
	else return (x - x1)*(y2 -y1)/(x2 - x1) + y1;
}

double gctl::line_interpolate(double xmin, double xmax, const array<double> &y_arr, double x)
{
	int ynum = y_arr.size();
	double dx = (xmax - xmin)/(ynum - 1);
	int ix = floor((x - xmin)/dx);

	return line_interpolate(xmin + ix*dx, xmin + (ix+1)*dx, y_arr[ix], y_arr[ix+1], x);
}

double gctl::line_interpolate(const array<double> &x_arr, const array<double> &y_arr, double x)
{
	if (x_arr.size() != y_arr.size() || x_arr.empty() || y_arr.empty())
		throw std::runtime_error("[gctl::line_interpolate] Invalid array sizes.");

	for (size_t i = 0; i < x_arr.size() - 1; i++)
	{
		if (x_arr[i] > x_arr[i+1])
		{
			throw std::runtime_error("[gctl::line_interpolate] Invalid x coordinates.");
		}
	}

	if (x_arr.front() >= x) return y_arr.front();
	if (x_arr.back() <= x) return y_arr.back();

	for (size_t i = 0; i < x_arr.size() - 1; i++)
	{
		if (x_arr[i] < x && x <= x_arr[i+1])
		{
			return line_interpolate(x_arr[i], x_arr[i+1], y_arr[i], y_arr[i+1], x);
		}
	}

	return y_arr.front();
}

/*
lat
|
|
h21----h22
|       |
|       |
h11----h12----> lon
*/
// 球面双线性插值函数 单位为弧度 注意纬度为余纬度
double gctl::sph_linear_interpolate(double CoLat1, double CoLat2, double Lon1, double Lon2, 
	double CoLat0, double Lon0, double h11, double h12, double h21, double h22)
{
	double Delta=(Lon2-Lon1)*(cos(CoLat2)-cos(CoLat1));
	double A=(Lon1*(h12-h22)+Lon2*(h21-h11))/Delta;
	double B=(cos(CoLat1)*(h21-h22)+cos(CoLat2)*(h12-h11))/Delta;
	double C=(h11+h22-h21-h12)/Delta;
	double D=(Lon2*cos(CoLat2)*h11-Lon2*cos(CoLat1)*h21+Lon1*cos(CoLat1)*h22-Lon1*cos(CoLat2)*h12)/Delta;
	double h0=A*cos(CoLat0)+B*Lon0+C*Lon0*cos(CoLat0)+D;
	return h0;
}

/*
lat
|
|
h21----h22
|       |
|       |
h11----h12----> lon
*/
// 球面双线性插值函数 以度为单位的版本 注意纬度为余纬度
double gctl::sph_linear_interpolate_deg(double CoLat1,double CoLat2,double Lon1,double Lon2,
	double CoLat0,double Lon0,double h11,double h12,double h21,double h22)
{
	double Delta=(Lon2-Lon1)*(cosd(CoLat2)-cosd(CoLat1));
	double A=(Lon1*(h12-h22)+Lon2*(h21-h11))/Delta;
	double B=(cosd(CoLat1)*(h21-h22)+cosd(CoLat2)*(h12-h11))/Delta;
	double C=(h11+h22-h21-h12)/Delta;
	double D=(Lon2*cosd(CoLat2)*h11-Lon2*cosd(CoLat1)*h21+Lon1*cosd(CoLat1)*h22-Lon1*cosd(CoLat2)*h12)/Delta;
	double h0=A*cosd(CoLat0)+B*Lon0+C*Lon0*cosd(CoLat0)+D;
	return h0;
}

//规则网络插值 长方形内数据插值 距离平方反比
/*长方体示意图*/
// y
// |
// |
// 3------------2
// |            |
// |            |
// |            |
// 0------------1--->x
// 左下角坐标x0 y0
// 块体尺寸dx dy
// 插值点坐标x y
// 四个角点值
double gctl::rect_interpolate(double x0,double y0,double dx,double dy,double x,double y,
	double d0,double d1,double d2,double d3, double p)
{
	double res = 0;
	double total_dist = 0;
	double dist[4] = {0,0,0,0};
	double val[4];
	val[0] = d0; val[1] = d1; val[2] = d2; val[3] = d3;
	dist[0] = 1.0/(1e-30 + pow((x-x0)*(x-x0)+(y-y0)*(y-y0), 0.5*p));
	dist[1] = 1.0/(1e-30 + pow((x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0), 0.5*p));
	dist[2] = 1.0/(1e-30 + pow((x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0), 0.5*p));
	dist[3] = 1.0/(1e-30 + pow((x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0), 0.5*p));

	for (int i = 0; i < 4; i++)
	{
		total_dist += dist[i];
	}

	for (int i = 0; i < 4; i++)
	{
		res += val[i]*dist[i]/total_dist;
	}
	return res;
}

//规则网络插值 六面体内数据插值 距离平方反比
/*六面体示意图*/
//       y
//      /
//     /
//    3------------2
//   /|           /|
//  / |          / |
// 0------------1------> x
// |  7         |  6
// | /          | /
// |/           |/
// 4------------5
// |
// |
// z
// 左上角坐标x0 y0 z0
// 块体尺寸dx dy dz
// 插值点坐标x y z
// 八个角点值
double gctl::cube_interpolate(double x0,double y0,double z0,double dx,double dy,double dz,double x,double y,double z,
	double d0,double d1,double d2,double d3,double d4,double d5,double d6,double d7)
{
	double res = 0.0;
	double total_dist = 0;
	double dist[8] = {0,0,0,0,0,0,0,0};
	double val[8];
	val[0] = d0; val[1] = d1; val[2] = d2; val[3] = d3;
	val[4] = d4; val[5] = d5; val[6] = d6; val[7] = d7;
	//计算八个角点与待插值点的距离信息
	dist[0] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-y0)*(y-y0)+(z-z0)*(z-z0));
	dist[1] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0)+(z-z0)*(z-z0));
	dist[2] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0)+(z-z0)*(z-z0));
	dist[3] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0)+(z-z0)*(z-z0));
	dist[4] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-y0)*(y-y0)+(z-dz-z0)*(z-dz-z0));
	dist[5] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0)+(z-dz-z0)*(z-dz-z0));
	dist[6] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0)+(z-dz-z0)*(z-dz-z0));
	dist[7] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0)+(z-dz-z0)*(z-dz-z0));
	for (int i = 0; i < 8; i++)
	{
		total_dist += dist[i];
	}
	for (int i = 0; i < 8; i++)
	{
		res += val[i]*dist[i]/total_dist;
	}
	return res;
}

//球坐标下规则网络插值 六面体内数据插值
/*六面体示意图*/
//       lat
//      /
//     /
//    7------------6
//   /|           /|
//  / |          / |
// 4------------5------> lon
// |  3         |  2
// | /          | /
// |/           |/
// 0------------1
// |
// |
// rad
// 左下角坐标lon0 lat0 rad0
// 块体尺寸dlon dlat drad
// 插值点坐标lon lat rad
// 八个角点值
double gctl::cube_interpolate_sph(double lon0, double lat0, double rad0, double dlon, double dlat, double drad, 
	double lon, double lat, double rad, double d0, double d1, double d2, double d3, double d4, double d5, 
	double d6, double d7)
{
	double d04 = line_interpolate(rad0, rad0+drad, d0, d4, rad);
	double d15 = line_interpolate(rad0, rad0+drad, d1, d5, rad);
	double d26 = line_interpolate(rad0, rad0+drad, d2, d6, rad);
	double d37 = line_interpolate(rad0, rad0+drad, d3, d7, rad);
	return sph_linear_interpolate_deg(90.0-lat0, 90.0-lat0-dlat, lon0, lon0+dlon, 90.0-lat, lon, 
		d04, d15, d37, d26);
}

gctl::array<double> *gctl::p2p_dist_2d(point2dc * out_posi, int out_num, point2dc *in_posi, double *in_val, 
	int in_num, double search_xlen, double search_ylen, double search_deg)
{
	if (out_posi == nullptr || in_posi == nullptr || in_val == nullptr)
		throw domain_error("Invalid pointer. Thrown by gctl::p2p_dist_2d(...)");

	if (out_num < 0 || in_num < 0)
		throw invalid_argument("Negative array sizes. Thrown by gctl::p2p_dist_2d(...)");

	if (search_xlen <= 0 || search_ylen <= 0)
		throw invalid_argument("Negative search lengths. Thrown by gctl::p2p_dist_2d(...)");

	// 将输入数据装入很多排列整齐的小盒子 盒子的边长为搜索半径中较大的值
	double box_len = GCTL_MAX(search_xlen, search_ylen);
	// 找出盒子的边界范围
	double box_xmin = GCTL_BDL_MAX, box_xmax = GCTL_BDL_MIN;
	double box_ymin = GCTL_BDL_MAX, box_ymax = GCTL_BDL_MIN;
	for (int i = 0; i < in_num; i++)
	{
		box_xmin = GCTL_MIN(box_xmin, in_posi[i].x);
		box_xmax = GCTL_MAX(box_xmax, in_posi[i].x);
		box_ymin = GCTL_MIN(box_ymin, in_posi[i].y);
		box_ymax = GCTL_MAX(box_ymax, in_posi[i].y);
	}
	// 计算盒子的维度 注意这里是盒子的维度 不是盒子顶点的维度 将盒子的维度往上下左右各推出一格
	int box_xnum = std::ceil((box_xmax - box_xmin)/box_len) + 2;
	int box_ynum = std::ceil((box_ymax - box_ymin)/box_len) + 2;
	// 对盒子的范围重新赋值
	box_xmin -= box_len; box_xmax += box_len;
	box_ymin -= box_len; box_ymax += box_len;
	// 初始化盒子内的顶点列表
	std::vector<std::vector<int> > box_list(box_xnum*box_ynum);
	// 将输入顶点装入盒子内
	int tmp_m, tmp_n;
	for (int i = 0; i < in_num; i++)
	{
		tmp_m = std::floor((in_posi[i].x - box_xmin)/box_len);
		tmp_n = std::floor((in_posi[i].y - box_ymin)/box_len);

		box_list[tmp_m + tmp_n * box_xnum].push_back(i);
	}

	// 下面开始计算规则节点的数值
	array<double> *out_val = new array<double>(out_num, NAN);
	std::vector<double> dist_list, val_list;
	int tmp_id, search_id;
	double x_ang, tmp_dist; 
	for (int i = 0; i < out_num; i++)
	{
		dist_list.clear();
		val_list.clear();

		// 计算节点在盒子中的位置
		tmp_m = std::floor((out_posi[i].x - box_xmin)/box_len);
		tmp_n = std::floor((out_posi[i].y - box_ymin)/box_len);
		// 在节点所在的盒子以及相邻的盒子内查找搜索半径内的数据点
		for (int n = -1; n <= 1; n++)
		{
			for (int m = -1; m <= 1; m++)
			{
				if (tmp_m+m >= 0 && tmp_m+m <= box_xnum-1 && 
					tmp_n+n >= 0 && tmp_n+n <= box_ynum-1)
				{
					//当前盒子的索引号
					search_id = tmp_m + m + (tmp_n + n)*box_xnum;
					// 循环当前盒子内的输入点
					for (int b = 0; b < box_list[search_id].size(); b++)
					{
						tmp_id = box_list[search_id][b];
						// 如果点到节点的距离小于椭球的半径则添加到向量中
						tmp_dist = distance(out_posi[i], in_posi[tmp_id]);
						x_ang = std::atan2(in_posi[tmp_id].y - out_posi[i].y, 
							in_posi[tmp_id].x - out_posi[i].x);

						if (tmp_dist <= ellipse_radius_2d(search_xlen, search_ylen, search_deg, x_ang))
						{
							dist_list.push_back(tmp_dist);
							val_list.push_back(in_val[tmp_id]);
						}
					}
				}
			}
		}

		if (!dist_list.empty())
			out_val->at(i) = dist_inverse_weight(&dist_list, &val_list, 2);
	}

	destroy_vector(box_list);
	return out_val;
}

gctl::array<double> *gctl::p2p_dist(point3dc * out_posi, int out_num, point3dc *in_posi, 
	double *in_val, int in_num, double search_xlen, double search_ylen, double search_zlen)
{
	if (out_posi == nullptr || in_posi == nullptr || in_val == nullptr)
		throw domain_error("Invalid pointer. Thrown by gctl::p2p_dist(...)");

	if (out_num < 0 || in_num < 0)
		throw invalid_argument("Negative array sizes. Thrown by gctl::p2p_dist(...)");

	if (search_xlen <= 0 || search_ylen <= 0 || search_zlen <= 0)
		throw invalid_argument("Negative search lengths. Thrown by gctl::p2p_dist(...)");

	// 将输入数据装入很多排列整齐的小盒子 盒子的边长为搜索半径中较大的值
	double box_len = GCTL_MAX(GCTL_MAX(search_xlen, search_ylen), search_zlen);
	// 找出盒子的边界范围
	double box_xmin = GCTL_BDL_MAX, box_xmax = GCTL_BDL_MIN;
	double box_ymin = GCTL_BDL_MAX, box_ymax = GCTL_BDL_MIN;
	double box_zmin = GCTL_BDL_MAX, box_zmax = GCTL_BDL_MIN;
	for (int i = 0; i < in_num; i++)
	{
		box_xmin = GCTL_MIN(box_xmin, in_posi[i].x);
		box_xmax = GCTL_MAX(box_xmax, in_posi[i].x);
		box_ymin = GCTL_MIN(box_ymin, in_posi[i].y);
		box_ymax = GCTL_MAX(box_ymax, in_posi[i].y);
		box_zmin = GCTL_MIN(box_zmin, in_posi[i].z);
		box_zmax = GCTL_MAX(box_zmax, in_posi[i].z);
	}
	// 计算盒子的维度 注意这里是盒子的维度 不是盒子顶点的维度 将盒子的维度往上下左右各推出一格
	int box_xnum = std::ceil((box_xmax - box_xmin)/box_len) + 2;
	int box_ynum = std::ceil((box_ymax - box_ymin)/box_len) + 2;
	int box_znum = std::ceil((box_zmax - box_zmin)/box_len) + 2;
	// 对盒子的范围重新赋值
	box_xmin -= box_len; box_xmax += box_len;
	box_ymin -= box_len; box_ymax += box_len;
	box_zmin -= box_len; box_zmax += box_len;
	// 初始化盒子内的顶点列表
	std::vector<std::vector<int> > box_list(box_xnum*box_ynum*box_znum);
	// 将输入顶点装入盒子内
	int tmp_m, tmp_n, tmp_k;
	for (int i = 0; i < in_num; i++)
	{
		tmp_m = std::floor((in_posi[i].x - box_xmin)/box_len);
		tmp_n = std::floor((in_posi[i].y - box_ymin)/box_len);
		tmp_k = std::floor((in_posi[i].z - box_zmin)/box_len);

		box_list[tmp_m + tmp_n*box_xnum + tmp_k*box_xnum*box_ynum].push_back(i);
	}

	// 下面开始计算规则节点的数值
	array<double> *out_val = new array<double>(out_num, NAN);
	std::vector<double> dist_list, val_list;
	int tmp_id, search_id;
	double x_ang, z_ang, tmp_dist; 
	for (int i = 0; i < out_num; i++)
	{
		dist_list.clear();
		val_list.clear();

		// 计算节点在盒子中的位置
		tmp_m = std::floor((out_posi[i].x - box_xmin)/box_len);
		tmp_n = std::floor((out_posi[i].y - box_ymin)/box_len);
		tmp_k = std::floor((out_posi[i].z - box_zmin)/box_len);
		// 在节点所在的盒子以及相邻的盒子内查找搜索半径内的数据点
		for (int k = -1; k <= 1; k++)
		{
			for (int n = -1; n <= 1; n++)
			{
				for (int m = -1; m <= 1; m++)
				{
					if (tmp_m+m >= 0 && tmp_m+m <= box_xnum-1 && 
						tmp_n+n >= 0 && tmp_n+n <= box_ynum-1 && 
						tmp_k+k >= 0 && tmp_k+k <= box_znum-1)
					{
						//当前盒子的索引号
						search_id = tmp_m + m + (tmp_n + n)*box_xnum + (tmp_k + k)*box_xnum*box_ynum;
						// 循环当前盒子内的输入点
						for (int b = 0; b < box_list[search_id].size(); b++)
						{
							tmp_id = box_list[search_id][b];
							// 如果点到节点的距离小于椭球的半径则添加到向量中
							tmp_dist = distance(out_posi[i], in_posi[tmp_id]);
							x_ang = std::atan2(in_posi[tmp_id].y - out_posi[i].y, 
								in_posi[tmp_id].x - out_posi[i].x);
							z_ang = std::acos((in_posi[tmp_id].z - out_posi[i].z)/tmp_dist);

							if (tmp_dist <= ellipsoid_radius(search_xlen, search_ylen, search_zlen, x_ang, z_ang))
							{
								dist_list.push_back(tmp_dist);
								val_list.push_back(in_val[tmp_id]);
							}
						}
					}
				}
			}
		}

		if (!dist_list.empty())
			out_val->at(i) = dist_inverse_weight(&dist_list, &val_list, 2);
	}

	destroy_vector(box_list);
	return out_val;
}

gctl::array<double> *gctl::p2p_dist_sph(point3ds * out_posi, int out_num, point3ds *in_posi, 
	double *in_val, int in_num, double search_rlen, double search_deg)
{
	if (out_posi == nullptr || in_posi == nullptr || in_val == nullptr)
		throw domain_error("Invalid pointer. Thrown by gctl::p2p_dist_sph(...)");

	if (out_num < 0 || in_num < 0)
		throw invalid_argument("Negative array sizes. Thrown by gctl::p2p_dist_sph(...)");

	if (search_rlen <= 0 || search_deg <= 0)
		throw invalid_argument("Negative search lengths. Thrown by gctl::p2p_dist_sph(...)");

	// 找出盒子的边界范围
	double box_lonmin = GCTL_BDL_MAX, box_lonmax = GCTL_BDL_MIN;
	double box_latmin = GCTL_BDL_MAX, box_latmax = GCTL_BDL_MIN;
	double box_radmin = GCTL_BDL_MAX, box_radmax = GCTL_BDL_MIN;
	for (int i = 0; i < in_num; i++)
	{
		box_lonmin = GCTL_MIN(box_lonmin, in_posi[i].lon);
		box_lonmax = GCTL_MAX(box_lonmax, in_posi[i].lon);
		box_latmin = GCTL_MIN(box_latmin, in_posi[i].lat);
		box_latmax = GCTL_MAX(box_latmax, in_posi[i].lat);
		box_radmin = GCTL_MIN(box_radmin, in_posi[i].rad);
		box_radmax = GCTL_MAX(box_radmax, in_posi[i].rad);
	}
	// 计算盒子的维度 注意这里是盒子的维度 不是盒子顶点的维度 将盒子的维度往上下左右各推出一格
	int box_lonnum = std::ceil((box_lonmax - box_lonmin)/search_deg) + 2;
	int box_latnum = std::ceil((box_latmax - box_latmin)/search_deg) + 2;
	int box_radnum = std::ceil((box_radmax - box_radmin)/search_rlen) + 2;
	// 对盒子的范围重新赋值
	box_lonmin -= search_deg;  box_lonmax += search_deg;
	box_latmin -= search_deg;  box_latmax += search_deg;
	box_radmin -= search_rlen; box_radmax += search_rlen;
	// 初始化盒子内的顶点列表
	std::vector<std::vector<int> > box_list(box_lonnum*box_latnum*box_radnum);
	// 将输入顶点装入盒子内
	int tmp_m, tmp_n, tmp_k;
	for (int i = 0; i < in_num; i++)
	{
		tmp_m = std::floor((in_posi[i].lon - box_lonmin)/search_deg);
		tmp_n = std::floor((in_posi[i].lat - box_latmin)/search_deg);
		tmp_k = std::floor((in_posi[i].rad - box_radmin)/search_rlen);

		box_list[tmp_m + tmp_n*box_lonnum + tmp_k*box_lonnum*box_latnum].push_back(i);
	}

	// 下面开始计算规则节点的数值
	array<double> *out_val = new array<double>(out_num, NAN);
	std::vector<double> dist_list, val_list;
	int tmp_id, search_id;
	double tmp_dist; 
	point3dc tmp_pc1, tmp_pc2;
	for (int i = 0; i < out_num; i++)
	{
		dist_list.clear();
		val_list.clear();

		// 计算节点在盒子中的位置
		tmp_m = std::floor((out_posi[i].lon - box_lonmin)/search_deg);
		tmp_n = std::floor((out_posi[i].lat - box_latmin)/search_deg);
		tmp_k = std::floor((out_posi[i].rad - box_radmin)/search_rlen);
		// 在节点所在的盒子以及相邻的盒子内查找搜索半径内的数据点
		for (int k = -1; k <= 1; k++)
		{
			for (int n = -1; n <= 1; n++)
			{
				for (int m = -1; m <= 1; m++)
				{
					if (tmp_m+m >= 0 && tmp_m+m <= box_lonnum-1 && 
						tmp_n+n >= 0 && tmp_n+n <= box_latnum-1 && 
						tmp_k+k >= 0 && tmp_k+k <= box_radnum-1)
					{
						//当前盒子的索引号
						search_id = tmp_m + m + (tmp_n + n)*box_lonnum + (tmp_k + k)*box_lonnum*box_latnum;
						// 循环当前盒子内的输入点
						for (int b = 0; b < box_list[search_id].size(); b++)
						{
							tmp_id = box_list[search_id][b];
							// 如果点到节点的距离小于椭球的半径则添加到向量中
							tmp_pc1 = out_posi[i].s2c();
							tmp_pc2 = in_posi[tmp_id].s2c();
							tmp_dist = distance(tmp_pc1, tmp_pc2);

							if (geometry3d::angle(tmp_pc1, tmp_pc2) <= search_deg && 
								std::fabs(in_posi[tmp_id].rad - out_posi[i].rad) <= search_rlen)
							{
								dist_list.push_back(tmp_dist);
								val_list.push_back(in_val[tmp_id]);
							}
						}
					}
				}
			}
		}

		if (!dist_list.empty())
			out_val->at(i) = dist_inverse_weight(&dist_list, &val_list, 2);
	}

	destroy_vector(box_list);
	return out_val;
}