/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * 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 "mathfunc.h"

int gctl::random(int low, int hig)
{
	return rand()%(hig - low + 1) + low;
}

double gctl::random(double low, double hig)
{
	double f = (double) rand()/RAND_MAX;
	return f*(hig-low) + low;
}

std::complex<double> gctl::random(std::complex<double> low, std::complex<double> hig)
{
	std::complex<double> c;
	double r = (double) rand()/RAND_MAX;
	double i = (double) rand()/RAND_MAX;
	double rh = std::max(hig.real(), low.real());
	double rl = std::min(hig.real(), low.real());
	double ih = std::max(hig.imag(), low.imag());
	double il = std::min(hig.imag(), low.imag());

	c.real(r*(rh - rl) + rl);
	c.imag(i*(ih - il) + il);
	return c;
}

bool gctl::isequal(double f, double v, double eps)
{
	return fabs(f - v) < eps;
}

double gctl::sign(double a)
{
	if (a > GCTL_ZERO) return 1.0;
	if (a < -1.0*GCTL_ZERO) return -1.0; 
	return 0.0;
}

//利用二分法求一个正数的n次方根 注意输入值小于1的情况
double gctl::sqrtn(double d,int n,double eps)
{
	double xmin,xmax,halfx;
	if (d == 1)
	{
		return d;
	}
	else if (d > 1)
	{
		xmin = 1;
		xmax = d;
		halfx = 0.5*(xmin+xmax);
		while (fabs(d - pow(halfx,n)) > eps)
		{
			if (pow(halfx,n) > d)
			{
				xmax = halfx;
				halfx = 0.5*(xmin+xmax);
			}
			else
			{
				xmin = halfx;
				halfx = 0.5*(xmin+xmax);
			}
		}
	}
	else
	{
		xmin = 0;
		xmax = 1;
		halfx = 0.5*(xmin+xmax);
		while (fabs(d - pow(halfx,n)) > eps)
		{
			if (pow(halfx,n) > d)
			{
				xmax = halfx;
				halfx = 0.5*(xmin+xmax);
			}
			else
			{
				xmin = halfx;
				halfx = 0.5*(xmin+xmax);
			}
		}
	}
	return halfx;
}

double gctl::geographic_area(double lon1, double lon2, double lat1, double lat2, double R)
{
	return fabs(R*R*(arc(lon2) - arc(lon1))*(sind(lat2) - sind(lat1)));
}

double gctl::geographic_distance(double lon1, double lon2, double lat1, double lat2, double R)
{
	double n1 = arc(lon1), n2 = arc(lon2), t1 = arc(lat1), t2 = arc(lat2);
	double a = sin(0.5*(t2 - t1)), b = sin(0.5*(n2 - n1));
	return 2*R*asin(sqrt(a*a + cos(t1)*cos(t2)*b*b));
}

double gctl::ellipse_radius_2d(double x_len, double y_len, double arc, double x_arc)
{
	if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
	{
		return 0.5*(x_len + y_len);
	}
	
	return sqrt(pow(x_len*cos(arc-x_arc),2) + pow(y_len*sin(arc-x_arc),2));
}

void gctl::ellipse_plus_elevation_2d(double x_len, double y_len, double arc, double elev,
	double &out_arc, double &out_rad, double x_arc)
{
	if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
	{
		out_arc = arc;
		out_rad = 0.5*(x_len + y_len) + elev;
		return;
	}
	
	if (fabs(arc) < 1e-8) // 弧度太小 直接加和
	{
		out_arc = arc;
		out_rad = x_len + elev;
		return;
	}

	if (fabs(fabs(arc) - 0.5*GCTL_Pi) < 1e-8) // 到了弧顶 直接加和
	{
		out_arc = arc;
		out_rad = y_len + elev;
		return;
	}
	
	double ellip_rad = ellipse_radius_2d(x_len, y_len, arc, x_arc);
	double alpha = atan(x_len*x_len*sin(arc)/(y_len*y_len*cos(arc)));
	double sin_alpha = sin(alpha);
	double lx = ellip_rad * sin(alpha - arc)/sin_alpha;
	double lr = ellip_rad * sin(arc)/sin_alpha + elev;
	out_rad = sqrt(lx*lx + lr*lr - 2.0*lx*lr*cos(GCTL_Pi - alpha));
	out_arc = acos((lx*lx + out_rad*out_rad - lr*lr)/(2.0*out_rad*lx));
	if (arc < 0.0) out_arc *= -1.0;

	return;
}

double gctl::ellipsoid_radius(double x_len, double y_len, double z_len, double phi, double theta)
{
	return x_len*y_len*z_len/sqrt(pow(y_len*z_len*sin(theta)*cos(phi),2) + 
		pow(x_len*z_len*sin(theta)*sin(phi),2) + pow(x_len*y_len*cos(theta),2));
}

// 使用牛顿迭代法计算一个矩阵的近似逆
double gctl::newton_inverse(const _2d_matrix &in_mat, _2d_matrix &inverse_mat, 
	double epsilon, int iter_times, bool initiate)
{
	if (in_mat.empty())
		throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");

	if (in_mat.row_size() != in_mat.col_size())
		throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");

	if (iter_times < 0)
		throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");

	int m_size = in_mat.row_size();

	if (initiate)
	{
		// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
		if (!inverse_mat.empty()) inverse_mat.clear();
		inverse_mat.resize(m_size, m_size, 0.0);
		for (int i = 0; i < m_size; i++)
		{
			inverse_mat[i][i] = 1.0/in_mat[i][i];
		}
	}

	if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
		throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");

	// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
	// 计算矩阵的无穷大范数
	double maxi_mod = 0.0, mod, ele;
	for (int i = 0; i < m_size; i++)
	{
		mod = 0.0;
		for (int j = 0; j < m_size; j++)
		{
			ele = 0.0;
			for (int k = 0; k < m_size; k++)
			{
				ele += in_mat[i][k] * inverse_mat[k][j];
			}

			if (i == j)
			{
				mod += GCTL_FABS(1.0 - ele);
			}
			else
			{
				mod += GCTL_FABS(ele);
			}
		}

		maxi_mod = GCTL_MAX(maxi_mod, mod);
	}

	if (maxi_mod >= 1.0)
	{
		GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)", 
			GCTL_WARNING_ERROR, 0, 0, 0);
	}

	array<double> col_ax(m_size, 0.0); // A*X 的列向量
	_2d_matrix tmp_mat(m_size, m_size, 0.0);
	for (int t = 0; t < iter_times; t++)
	{
		if (maxi_mod <= epsilon)
			break;

		for (int i = 0; i < m_size; i++)
		{
			for (int j = 0; j < m_size; j++)
			{
				tmp_mat[i][j] = inverse_mat[i][j];
			}
		}

		for (int n = 0; n < m_size; n++)
		{
			for (int i = 0; i < m_size; i++)
			{
				for (int j = 0; j < m_size; j++)
				{
					col_ax[j] = 0.0;
					for (int k = 0; k < m_size; k++)
					{
						col_ax[j] += in_mat[j][k] * tmp_mat[k][i];
					}
					col_ax[j] *= -1.0;
				}
				col_ax[i] += 2.0;

				ele = 0.0;
				for (int j = 0; j < m_size; j++)
				{
					ele += tmp_mat[n][j] * col_ax[j];
				}

				inverse_mat[n][i] = ele;
			}
		}

		maxi_mod = 0.0;
		for (int i = 0; i < m_size; i++)
		{
			mod = 0.0;
			for (int j = 0; j < m_size; j++)
			{
				ele = 0.0;
				for (int k = 0; k < m_size; k++)
				{
					ele += in_mat[i][k] * inverse_mat[k][j];
				}

				if (i == j)
				{
					mod += GCTL_FABS(1.0 - ele);
				}
				else
				{
					mod += GCTL_FABS(ele);
				}
			}

			maxi_mod = GCTL_MAX(maxi_mod, mod);
		}
	}

	return maxi_mod;
}

// 使用牛顿迭代法计算一个矩阵的近似逆
double gctl::newton_inverse(const spmat<double> &in_mat, _2d_matrix &inverse_mat, 
	double epsilon, int iter_times, bool initiate)
{
	if (in_mat.empty())
		throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");

	if (in_mat.row_size() != in_mat.col_size())
		throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");

	if (iter_times < 0)
		throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");

	int m_size = in_mat.row_size();

	if (initiate)
	{
		// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
		if (!inverse_mat.empty()) inverse_mat.clear();
		inverse_mat.resize(m_size, m_size, 0.0);
		for (int i = 0; i < m_size; i++)
		{
			inverse_mat[i][i] = 1.0/in_mat.at(i, i);
		}
	}

	if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
		throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");

	// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
	// 计算矩阵的无穷大范数
	double maxi_mod = 0.0, mod, ele;
	array<double> tmp_arr(m_size, 0.0); // A*X 的列向量
	for (int i = 0; i < m_size; i++)
	{
		mod = 0.0;
		for (int j = 0; j < m_size; j++)
		{
			for (int k = 0; k < m_size; k++)
			{
				tmp_arr[k] = inverse_mat[k][j];
			}

			ele = in_mat.multiply_vector(tmp_arr, i);

			if (i == j)
			{
				mod += GCTL_FABS(1.0 - ele);
			}
			else
			{
				mod += GCTL_FABS(ele);
			}
		}

		maxi_mod = GCTL_MAX(maxi_mod, mod);
	}

	if (maxi_mod >= 1.0)
	{
		GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)", 
			GCTL_WARNING_ERROR, 0, 0, 0);
	}

	array<double> col_ax(m_size, 0.0);
	_2d_matrix tmp_mat(m_size, m_size, 0.0);
	for (int t = 0; t < iter_times; t++)
	{
		if (maxi_mod <= epsilon)
			break;
		//else std::cout << "epsilon = " << maxi_mod << std::endl;

		for (int i = 0; i < m_size; i++)
		{
			for (int j = 0; j < m_size; j++)
			{
				tmp_mat[i][j] = inverse_mat[i][j];
			}
		}

		for (int n = 0; n < m_size; n++)
		{
			for (int i = 0; i < m_size; i++)
			{
				for (int j = 0; j < m_size; j++)
				{
					for (int k = 0; k < m_size; k++)
					{
						tmp_arr[k] = tmp_mat[k][i];
					}
					col_ax[j] = in_mat.multiply_vector(tmp_arr, j);
					col_ax[j] *= -1.0;
				}
				col_ax[i] += 2.0;

				ele = 0.0;
				for (int j = 0; j < m_size; j++)
				{
					ele += tmp_mat[n][j] * col_ax[j];
				}

				inverse_mat[n][i] = ele;
			}
		}

		maxi_mod = 0.0;
		for (int i = 0; i < m_size; i++)
		{
			mod = 0.0;
			for (int j = 0; j < m_size; j++)
			{
				for (int k = 0; k < m_size; k++)
				{
					tmp_arr[k] = inverse_mat[k][j];
				}

				ele = in_mat.multiply_vector(tmp_arr, i);

				if (i == j)
				{
					mod += GCTL_FABS(1.0 - ele);
				}
				else
				{
					mod += GCTL_FABS(ele);
				}
			}

			maxi_mod = GCTL_MAX(maxi_mod, mod);
		}
	}

	return maxi_mod;
}

void gctl::schmidt_orthogonal(const array<double> &a, array<double> &e, int a_s)
{
	if (a.empty())
		throw runtime_error("The input array is empty. Thrown by gctl::schmidt_orthogonal(...)");

	if (a_s <= 1) // a_s >= 2
		throw invalid_argument("vector size must be bigger than one. Thrown by gctl::schmidt_orthogonal(...)");

	int t_s = a.size();
	if (t_s%a_s != 0 || t_s <= 3) // t_s >= 4
		throw invalid_argument("incompatible total array size. Thrown by gctl::schmidt_orthogonal(...)");

	int len = t_s/a_s;
	if (len < a_s)
		throw invalid_argument("the vectors are over-determined. Thrown by gctl::schmidt_orthogonal(...)");

	e.resize(t_s, 0.0);

	double ae, ee;
	for (int i = 0; i < a_s; i++)
	{
		for (int l = 0; l < len; l++)
		{
			e[l + i*len] = a[l + i*len];
		}

		for (int m = 0; m < i; m++)
		{
			ae = ee = 0.0;
			for (int n = 0; n < len; n++)
			{
				ae += a[n + i*len] * e[n + m*len];
				ee += e[n + m*len] * e[n + m*len];
			}

			for (int n = 0; n < len; n++)
			{
				e[n + i*len] -= e[n + m*len] * ae/ee;
			}
		}
	}

	for (int i = 0; i < a_s; i++)
	{
		ee = 0.0;
		for (int l = 0; l < len; l++)
		{
			ee += e[l + i*len] * e[l + i*len];
		}
		ee = sqrt(ee);

		for (int l = 0; l < len; l++)
		{
			e[l + i*len] /= ee;
		}
	}
	return;
}