/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2022  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 "gkernel_triangle.h"
#include "cmath"

void gctl::callink_gravity_para(array<grav_triangle> &in_tri, array<gravtri_para> &out_para)
{
	point3dc v0, v1, v2, nor_f;

	out_para.resize(in_tri.size());
	for (int i = 0; i < in_tri.size(); ++i)
	{
		if (in_tri[i].vert[0] == nullptr || in_tri[i].vert[1] == nullptr || 
			in_tri[i].vert[2] == nullptr)
		{
			throw runtime_error("Invalid vertex pointer. From callink_gravity_para(...)");
		}

		v0 = *in_tri[i].vert[0];
		v1 = *in_tri[i].vert[1];
		v2 = *in_tri[i].vert[2];

		nor_f = cross(v1-v0, v2-v0).normal();
		out_para[i].F = kron(nor_f, nor_f);

		for (int e = 0; e < 3; ++e)
		{
			v2 = *in_tri[i].vert[(e+1)%3] - *in_tri[i].vert[e];
			out_para[i].edglen[e] = v2.module();
			out_para[i].E[e] = kron(nor_f, cross(v2, nor_f).normal());
		}

		in_tri[i].att = out_para.get(i);
	}
	return;
}

double gkernel_triangle_potential_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op);
gctl::point3dc gkernel_triangle_gradient_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op);
gctl::tensor gkernel_triangle_tensor_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op);

double gkernel_triangle_potential_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op);
gctl::point3dc gkernel_triangle_gradient_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op);
gctl::tensor gkernel_triangle_tensor_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op);


void gctl::gobser(array<double> &out_obs, const array<grav_triangle> &ele, 
	const array<point3dc> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, 0.0);

	gctl::progress_bar bar(e_size, "gobser_potential");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] += gkernel_triangle_potential_sig(ele[j], ops[i]) * rho;
		}
	}
	return;
}

void gctl::gobser(array<point3dc> &out_obs, const array<grav_triangle> &ele, 
	const array<point3dc> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, point3dc(0.0, 0.0, 0.0));

	gctl::progress_bar bar(e_size, "gobser_gradient");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] = out_obs[i] + gkernel_triangle_gradient_sig(ele[j], ops[i]) * rho;
		}
	}
	return;
}

void gctl::gobser(array<tensor> &out_obs, const array<grav_triangle> &ele, 
	const array<point3dc> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, tensor(0.0));

	gctl::progress_bar bar(e_size, "gobser_tensor");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] = out_obs[i] + gkernel_triangle_tensor_sig(ele[j], ops[i]) * rho;
		}
	}
	return;
}

void gctl::gobser(array<double> &out_obs, const array<grav_triangle> &ele, 
	const array<point3ds> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, 0.0);

	gctl::progress_bar bar(e_size, "gobser_potential");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] += gkernel_triangle_potential_sig_sph(ele[j], ops[i]) * rho;
		}
	}
	return;
}

void gctl::gobser(array<point3dc> &out_obs, const array<grav_triangle> &ele, 
	const array<point3ds> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, point3dc(0.0, 0.0, 0.0));

	gctl::progress_bar bar(e_size, "gobser_gradient");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] = out_obs[i] + gkernel_triangle_gradient_sig_sph(ele[j], ops[i]) * rho;
		}
	}
	return;
}

void gctl::gobser(array<tensor> &out_obs, const array<grav_triangle> &ele, 
	const array<point3ds> &ops, double rho, verbose_type_e verbose)
{
	int i, j;
	int o_size = ops.size();
	int e_size = ele.size();

	out_obs.resize(o_size, tensor(0.0));

	gctl::progress_bar bar(e_size, "gobser_tensor");
	for (j = 0; j < e_size; j++)
	{
		if (verbose == gctl::FullMsg) bar.progressed(j);
		else if (verbose == gctl::ShortMsg) bar.progressed_simple(j);

#pragma omp parallel for private (i) schedule(guided)
		for (i = 0; i < o_size; i++)
		{
			out_obs[i] = out_obs[i] + gkernel_triangle_tensor_sig_sph(ele[j], ops[i]) * rho;
		}
	}
	return;
}


// define individual algorithm here
double gkernel_triangle_potential_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op)
{
	double Le,wf;
	double dv1,dv2;
	double face_sum, edge_sum;
	gctl::point3dc re;
	gctl::point3dc r_ijk[3];
	gctl::point3dc face_tmp(0.0, 0.0, 0.0);
	gctl::point3dc edge_tmp(0.0, 0.0, 0.0);
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	r_ijk[0] = *a_ele.vert[0] - a_op;
	r_ijk[1] = *a_ele.vert[1] - a_op;
	r_ijk[2] = *a_ele.vert[2] - a_op;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0], cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_tmp = gp->F * r_ijk[0];
	face_sum = dot(r_ijk[0], face_tmp) * wf;

	edge_sum = 0.0;
	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], a_op);
		dv2 = distance(*a_ele.vert[(e+1)%3], a_op);

		re = 0.5*(*a_ele.vert[e] + *a_ele.vert[(e+1)%3]) - a_op;
		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_tmp = gp->E[e] * re;
		edge_sum += dot(re, edge_tmp) * Le;
	}

	// 重力正演中通常z方向取垂直向下为正方向
	return -0.5e+8*GCTL_G0*(face_sum - edge_sum);
}

gctl::point3dc gkernel_triangle_gradient_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op)
{
	double Le,wf;
	double dv1,dv2;
	gctl::point3dc re;
	gctl::point3dc r_ijk[3];
	gctl::point3dc face_sum(0.0, 0.0, 0.0);
	gctl::point3dc edge_sum(0.0, 0.0, 0.0);
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	r_ijk[0] = *a_ele.vert[0] - a_op;
	r_ijk[1] = *a_ele.vert[1] - a_op;
	r_ijk[2] = *a_ele.vert[2] - a_op;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0], cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_sum = (gp->F * r_ijk[0]) * wf;

	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], a_op);
		dv2 = distance(*a_ele.vert[(e+1)%3], a_op);

		re = 0.5*(*a_ele.vert[e] + *a_ele.vert[(e+1)%3]) - a_op;
		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_sum = edge_sum + (gp->E[e] * re) * Le;
	}

	gctl::point3dc out_p = 1.0e+8*GCTL_G0*(face_sum - edge_sum);
	out_p.z *= -1.0; // 重力正演中通常z方向取垂直向下为正方向
	return out_p;
}

gctl::tensor gkernel_triangle_tensor_sig(const gctl::grav_triangle &a_ele, const gctl::point3dc &a_op)
{
	double Le,wf;
	double dv1,dv2;
	gctl::point3dc r_ijk[3];
	gctl::tensor face_sum(0.0);
	gctl::tensor edge_sum(0.0);
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	r_ijk[0] = *a_ele.vert[0] - a_op;
	r_ijk[1] = *a_ele.vert[1] - a_op;
	r_ijk[2] = *a_ele.vert[2] - a_op;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0], cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_sum = wf * gp->F;

	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], a_op);
		dv2 = distance(*a_ele.vert[(e+1)%3], a_op);

		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_sum = edge_sum + Le * gp->E[e];
	}

	// 重力正演中通常z方向取垂直向下为正方向
	gctl::tensor out_t = 1.0e+8*GCTL_G0*(face_sum - edge_sum);
	out_t[0][0] *= -1.0;
	out_t[0][1] *= -1.0;
	out_t[1][0] *= -1.0;
	out_t[1][1] *= -1.0;
	out_t[2][2] *= -1.0;
	return out_t;
}

double gkernel_triangle_potential_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op)
{
	double Le,wf;
	double dv1,dv2;
	double face_sum, edge_sum;
	gctl::point3dc re, op_c;
	gctl::point3dc r_ijk[3];
	gctl::point3dc face_tmp(0.0, 0.0, 0.0);
	gctl::point3dc edge_tmp(0.0, 0.0, 0.0);
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	op_c = a_op.s2c();

	r_ijk[0] = *a_ele.vert[0] - op_c;
	r_ijk[1] = *a_ele.vert[1] - op_c;
	r_ijk[2] = *a_ele.vert[2] - op_c;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0], cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_tmp = gp->F * r_ijk[0];
	face_sum = dot(r_ijk[0], face_tmp) * wf;

	edge_sum = 0.0;
	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], op_c);
		dv2 = distance(*a_ele.vert[(e+1)%3], op_c);

		re = 0.5*(*a_ele.vert[e] + *a_ele.vert[(e+1)%3]) - op_c;
		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_tmp = gp->E[e] * re;
		edge_sum += dot(re, edge_tmp) * Le;
	}

	// 重力正演中通常负r方向为正方向
	return -0.5e+8*GCTL_G0*(face_sum - edge_sum);
}

gctl::point3dc gkernel_triangle_gradient_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op)
{
	double Le,wf;
	double dv1,dv2;
	// 直角坐标系下观测点的位置
	gctl::point3dc op_c;
	// 注意face_tmp与edge_tmp并不是直角坐标系下的点 我们只是借用向量操作而已
	gctl::point3dc face_tmp, edge_tmp;
	gctl::point3dc face_sum(0.0, 0.0, 0.0);
	gctl::point3dc edge_sum(0.0, 0.0, 0.0);
	gctl::tensor R;
	gctl::point3dc re;
	gctl::point3dc r_ijk[3];
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	R[0][0] = sin((0.5-a_op.lat/180.0)*GCTL_Pi)*cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[0][1] = sin((0.5-a_op.lat/180.0)*GCTL_Pi)*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[0][2] = cos((0.5-a_op.lat/180.0)*GCTL_Pi);

	R[1][0] = cos((0.5-a_op.lat/180.0)*GCTL_Pi)*cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[1][1] = cos((0.5-a_op.lat/180.0)*GCTL_Pi)*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[1][2] = -1.0*sin((0.5-a_op.lat/180.0)*GCTL_Pi);

	R[2][0] = -1.0*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[2][1] = cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[2][2] = 0.0;

	op_c = a_op.s2c();

	r_ijk[0] = *a_ele.vert[0] - op_c;
	r_ijk[1] = *a_ele.vert[1] - op_c;
	r_ijk[2] = *a_ele.vert[2] - op_c;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0],cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_tmp = gp->F * r_ijk[0];
	face_sum = (R * face_tmp) * wf;

	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], op_c);
		dv2 = distance(*a_ele.vert[(e+1)%3], op_c);

		re = 0.5*(*a_ele.vert[e] + *a_ele.vert[(e+1)%3]) - op_c;
		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_tmp = gp->E[e] * re;
		edge_sum = edge_sum + (R * edge_tmp) * Le;
	}

	gctl::point3dc out_p = 1.0e+8*GCTL_G0*(face_sum - edge_sum);
	out_p.x *= -1.0; // 重力正演中通常负r方向为正方向
	return out_p;
}

gctl::tensor gkernel_triangle_tensor_sig_sph(const gctl::grav_triangle &a_ele, const gctl::point3ds &a_op)
{
	double Le,wf;
	double dv1,dv2;
	// 直角坐标系下观测点的位置
	gctl::point3dc op_c;
	// 注意face_tmp与edge_tmp并不是直角坐标系下的点 我们只是借用向量操作而已
	gctl::tensor face_tmp, edge_tmp;
	gctl::tensor face_sum(0.0), edge_sum(0.0);
	// 这里我们需要完整的转换矩阵
	gctl::tensor R, R_T;
	gctl::point3dc r_ijk[3];
	double L_ijk[3];

	gctl::gravtri_para *gp = a_ele.att;

	R[0][0] = sin((0.5-a_op.lat/180.0)*GCTL_Pi)*cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[0][1] = sin((0.5-a_op.lat/180.0)*GCTL_Pi)*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[0][2] = cos((0.5-a_op.lat/180.0)*GCTL_Pi);

	R[1][0] = cos((0.5-a_op.lat/180.0)*GCTL_Pi)*cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[1][1] = cos((0.5-a_op.lat/180.0)*GCTL_Pi)*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[1][2] = -1.0*sin((0.5-a_op.lat/180.0)*GCTL_Pi);

	R[2][0] = -1.0*sin((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[2][1] = cos((2.0+a_op.lon/180.0)*GCTL_Pi);
	R[2][2] = 0.0;

	R_T = R.transpose();

	op_c = a_op.s2c();

	r_ijk[0] = *a_ele.vert[0] - op_c;
	r_ijk[1] = *a_ele.vert[1] - op_c;
	r_ijk[2] = *a_ele.vert[2] - op_c;

	L_ijk[0] = r_ijk[0].module();
	L_ijk[1] = r_ijk[1].module();
	L_ijk[2] = r_ijk[2].module();

	wf =2*atan2(dot(r_ijk[0],cross(r_ijk[1],r_ijk[2])),
				L_ijk[0]*L_ijk[1]*L_ijk[2] + L_ijk[0]*dot(r_ijk[1],r_ijk[2]) + 
				L_ijk[1]*dot(r_ijk[2],r_ijk[0]) + L_ijk[2]*dot(r_ijk[0],r_ijk[1]));

	face_tmp = gp->F * R_T;
	face_sum = (R * face_tmp) * wf;

	for (int e = 0; e < 3; e++)
	{
		dv1 = distance(*a_ele.vert[e], op_c);
		dv2 = distance(*a_ele.vert[(e+1)%3], op_c);

		Le = log((dv1+dv2+gp->edglen[e])/(dv1+dv2-gp->edglen[e]));

		edge_tmp = gp->E[e] * R_T;
		edge_sum = edge_sum + (R * edge_tmp) * Le;
	}

	// 重力正演中通常负r方向为正方向
	gctl::tensor out_t = 1.0e+8*GCTL_G0*(face_sum - edge_sum);
	out_t[0][0] *= -1.0;
	out_t[1][2] *= -1.0;
	out_t[2][1] *= -1.0;
	out_t[1][1] *= -1.0;
	out_t[2][2] *= -1.0;
	return out_t;
}