#include "magtet.h"


magtet::magtet(){}

magtet::~magtet(){}

void magtet::read_tet(std::string tet_name)
{
	gctl::read_Tetgen_node(tet_name, node_);
	gctl::read_Tetgen_element(tet_name, ele_, node_);

	node_num_ = node_.size();
	ele_num_ = ele_.size();
	return;
}

void magtet::read_magz(std::string mag_name)
{
	std::vector<gctl::point3d_c> tmp_vec;
	gctl::read_text2vector(mag_name, "", tmp_vec);

	if (tmp_vec.size() != ele_num_)
	{
		throw gctl::length_error("Invalid magnetization vector size. From magtet::read_magz(...)");
	}

	magz_.resize(tmp_vec);
	gctl::destroy_vector(tmp_vec);
	return;
}

void magtet::init_site(std::string para)
{
	// try to use the para as a file name
	if (access(para.c_str(), F_OK) != -1)
	{
		std::vector<gctl::point3d_c> tmp_vec;
		gctl::read_text2vector(para, "", tmp_vec);

		site_num_ = tmp_vec.size();
		site_.resize(tmp_vec);
		gctl::destroy_vector(tmp_vec);
		return;
	}

	// try to use the para in the format <xmin>/<xmax>/<ymin>/<ymax>/<z>/<xsize>/<ysize>
	double xmin, xmax, ymin, ymax, z;
	int xsize, ysize;
	gctl::utility::parse_string_to_value(para, '/', xmin, xmax, ymin, ymax, z, xsize, ysize);

	if (xsize <= 0 || ysize <= 0)
	{
		throw gctl::invalid_argument("Invalid site size. From magtet::init_site(...)");
	}

	std::vector<double> xs, ys;
	gctl::linespace(xmin, xmax, xsize, xs);
	gctl::linespace(ymin, ymax, ysize, ys);

	site_num_ = xsize*ysize;
	site_.resize(site_num_);
	for (int i = 0; i < ysize; ++i)
	{
		for (int j = 0; j < xsize; ++j)
		{
			site_[i*xsize + j].set(xs[j], ys[i], z);
		}
	}

	gctl::destroy_vector(xs);
	gctl::destroy_vector(ys);
	return;
}

void magtet::write_text(std::string out_name)
{
	std::vector<std::string> opt_info;
	gopt_.get_options(opt_info);

	std::ofstream outfile;
	if (!mag_pot_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_pot.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) V" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_pot_[i] << std::endl;
		}
		outfile.close();
	}

	if (!mag_grad_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_grad.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) Bx(nT) By(nT) Bz(nT)" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_grad_[i].x << " " 
				<< mag_grad_[i].y << " " << mag_grad_[i].z << " " << std::endl;
		}
		outfile.close();
	}

	if (!mag_tensor_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_tensor.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) Txx(nT/m) Txy(nT/m) Txz(nT/m) Tyx(nT/m) Tyy(nT/m) Tyz(nT/m) Tzx(nT/m) Tzy(nT/m) Tzz(nT/m)" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_tensor_[i].value(0,0) << " " 
				<< mag_tensor_[i].value(0,1) << " " << mag_tensor_[i].value(0,2) << " " 
				<< mag_tensor_[i].value(1,0) << " " << mag_tensor_[i].value(1,1) << " " 
				<< mag_tensor_[i].value(1,2) << " " << mag_tensor_[i].value(2,0) << " " 
				<< mag_tensor_[i].value(2,1) << " " << mag_tensor_[i].value(2,2) << std::endl;
		}
		outfile.close();
	}
	return;
}

void magtet::cal_tensors()
{
	// malloc space
	fnorm_.resize(ele_num_, 4);
	enorm_.resize(ele_num_, 12);
	etang_.resize(ele_num_, 12);

	gctl::point3d_c v1, v2, v3, nf, ne;
	for (int e = 0; e < ele_num_; ++e)
	{
		for (int i = 0; i < 4; ++i)
		{
			v1 = *ele_[e].fget(i, 1) - *ele_[e].fget(i, 0);
			v2 = *ele_[e].fget(i, 2) - *ele_[e].fget(i, 0);
			nf = gctl::cross(v1, v2).normal();
			fnorm_[e][i] = nf;

			for (int j = 0; j < 3; ++j)
			{
				v3 = *ele_[e].fget(i, (j+1)%3) - *ele_[e].fget(i, j);
				ne = gctl::cross(v3, nf).normal();
				enorm_[e][j+i*3] = ne;
				etang_[e][j+i*3] = gctl::cross(nf, ne);
			}
		}
	}
	return;
}

double magtet::mag_potential(const gctl::tetrahedron &tet, const gctl::point3d_c &site, 
	const gctl::point3d_c &mz, gctl::point3d_c *fn, gctl::point3d_c *en, gctl::point3d_c *et)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double part1, part2, k0, absw, mdotn, beta;
	gctl::point3d_c oi;

	double out_pot = 0.0;
	for (int f = 0; f < 4; ++f)
	{
		k0 = 0.0;
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), fn[f]);
				absw = std::abs(wi0);
			}

			oi = site - wi0*fn[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, et[3*f+j]);
		    Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, et[3*f+j]);
		    mij0 = gctl::dot(*tet.fget(f, j) - oi, en[3*f+j]);
		    Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			part2 = 0.0;
			if (absw > TOL)
			{
				beta = std::atan((mij0*Sij_plus)/(Rij0*Rij0+absw*Rij_plus))
					- std::atan((mij0*Sij_minus)/(Rij0*Rij0+absw*Rij_minus));
				part2 = absw*beta;
			}

			part1 = 0.0;
			if (std::abs(mij0) > TOL)
			{
				part1 = mij0*std::log((Rij_plus+Sij_plus)/(Rij_minus+Sij_minus));
			}

			k0 += (part1 - part2);
		}

		mdotn = gctl::dot(mz, fn[f]);
		out_pot += Cm*k0*mdotn;
	}

	return out_pot;
}

gctl::point3d_c magtet::mag_gradient(const gctl::tetrahedron &tet, const gctl::point3d_c &site, 
	const gctl::point3d_c &mz, gctl::point3d_c *fn, gctl::point3d_c *en, gctl::point3d_c *et)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double mdotn, beta, Aij, sig, absw;
	gctl::point3d_c oi, k1, part1, part2;

	gctl::point3d_c out_grad(0.0, 0.0, 0.0);
	for (int f = 0; f < 4; ++f)
	{
		k1.set(0.0, 0.0, 0.0);
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), fn[f]);
				sig = sign(wi0);
				absw = std::abs(wi0);
			}

			oi = site - wi0*fn[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, et[3*f+j]);
			Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, et[3*f+j]);
			mij0 = gctl::dot(*tet.fget(f, j) - oi, en[3*f+j]);
			Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			part2.set(0.0, 0.0, 0.0);
			if (absw > TOL)
			{
				beta = atan((mij0*Sij_plus)/(Rij0*Rij0 + absw*Rij_plus))
					- atan((mij0*Sij_minus)/(Rij0*Rij0 + absw*Rij_minus));

				part2 = sig*beta*fn[f];
			}

			if (std::abs(Rij0) > TOL)
			{
				Aij = std::log((long double)(Rij_plus+Sij_plus)) - std::log((long double)(Rij_minus+Sij_minus));
			}
			else if (Sij_plus > 0.0 && Sij_minus > 0.0)
			{
				Aij = std::log(Sij_plus) - std::log(Sij_minus);
			}
			else if (Sij_plus < 0.0 && Sij_minus < 0.0)
			{
				Aij = std::log(-1.0*Sij_minus) - std::log(-1.0*Sij_plus);
			}
			else
			{
				throw gctl::runtime_error("Observation site on edge. From magtet::mag_gradient()");
			}

			part1 = Aij * en[3*f+j];
			k1 = k1 - (part1 + part2);
		}

		mdotn = gctl::dot(mz, fn[f]);
		out_grad = out_grad - 1e+9*Cm*mdotn*k1;
	}

	return out_grad;
}

gctl::tensor magtet::mag_tensor(const gctl::tetrahedron &tet, const gctl::point3d_c &site, 
	const gctl::point3d_c &mz, gctl::point3d_c *fn, gctl::point3d_c *en, gctl::point3d_c *et)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double mdotn, beta, sig, absw;
	double factor_n_mij, factor_tij;
	gctl::point3d_c oi, grad_Aij;
	gctl::point3d_c grad_Rij_plus, grad_Rij_minus, grad_Sij_plus, grad_Sij_minus;
	gctl::point3d_c grad_mij0, grad_Rij0, grad_abs_wi0;
	gctl::point3d_c grad_a_plus, grad_b_plus, grad_a_minus, grad_b_minus;
	gctl::point3d_c grad_betaij_plus, grad_betaij_minus, grad_betaij;
	double a_plus, b_plus, a_minus, b_minus;
	double k3;
	gctl::tensor tmp_k, k2;

	gctl::tensor out_tensor(0.0);
	for (int f = 0; f < 4; ++f)
	{
		k2.set(0.0);
		k3 = 0.0;
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), fn[f]);
				sig = sign(wi0);
				absw = std::abs(wi0);
			}

			oi = site - wi0*fn[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, et[3*f+j]);
			Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, et[3*f+j]);
			mij0 = gctl::dot(*tet.fget(f, j) - oi, en[3*f+j]);
			Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			if (std::abs(Rij0) > TOL)
			{
				factor_n_mij = -1.0*(Sij_plus/(Rij0*Rij0*Rij_plus) - Sij_minus/(Rij0*Rij0*Rij_minus));
				factor_tij   = -1.0/Rij_plus + 1.0/Rij_minus;
				grad_Aij = (wi0*factor_n_mij)*fn[f] + factor_tij*et[3*f+j] 
					- (mij0*factor_n_mij)*en[3*f+j];
			}
			else
			{
				factor_tij   = -1.0/Rij_plus + 1.0/Rij_minus;
				grad_Aij = factor_tij*et[3*f+j];
			}

			//tmp_k = gctl::kron(grad_Aij, enorm_[e][3*f+j]);
			tmp_k = gctl::kron(en[3*f+j], grad_Aij);
			k2 = k2 - tmp_k;

			if (absw > TOL)
			{
				grad_Rij_plus  = (1.0/Rij_plus)*(site - *tet.fget(f, (j+1)%3));
				grad_Rij_minus = (1.0/Rij_minus)*(site - *tet.fget(f, j));
				grad_Sij_plus  = -1.0*et[3*f+j];
				grad_Sij_minus = -1.0*et[3*f+j];
				grad_mij0    = -1.0*en[3*f+j];
				grad_Rij0    = (1.0/Rij0)*(wi0*fn[f] - mij0*en[3*f+j]);
				grad_abs_wi0 = sig*fn[f];
				a_plus = Rij0*Rij0 + absw*Rij_plus;
				b_plus = mij0*Sij_plus;
				grad_a_plus = (2.0*Rij0)*grad_Rij0 + Rij_plus*grad_abs_wi0 + absw*grad_Rij_plus;
				grad_b_plus = Sij_plus*grad_mij0 + mij0*grad_Sij_plus;
				a_minus = Rij0*Rij0 + absw*Rij_minus;
				b_minus = mij0*Sij_minus;
				grad_a_minus = (2.0*Rij0)*grad_Rij0 + Rij_minus*grad_abs_wi0 + absw*grad_Rij_minus;
				grad_b_minus = Sij_minus*grad_mij0 + mij0*grad_Sij_minus;
				grad_betaij_plus  = (1.0/(a_plus*a_plus + b_plus*b_plus))*(a_plus*grad_b_plus - b_plus*grad_a_plus);
				grad_betaij_minus = (1.0/(a_minus*a_minus + b_minus*b_minus))*(a_minus*grad_b_minus - b_minus*grad_a_minus);

				grad_betaij = grad_betaij_plus - grad_betaij_minus;

				tmp_k = gctl::kron(grad_betaij, fn[f]);
				k2 = k2 - sig*tmp_k;
			}
			else
			{
				if (std::abs(mij0) > TOL)
				{
					k3 += (-1.0/mij0)*(Sij_plus/Rij_plus - Sij_minus/Rij_minus);
				}
			}
		}

		if (k3 != 0.0)
		{
			tmp_k = gctl::kron(fn[f], fn[f]);
			k2 = k2 - k3*tmp_k;
		}

		mdotn = gctl::dot(mz, fn[f]);
		out_tensor = out_tensor - 1e+9*Cm*mdotn*k2;
	}

	return out_tensor;
}

void magtet::total_potential()
{
	int e, s;
	// malloc space
	mag_pot_.resize(site_num_, 0.0);

	gctl::utility::progress_bar bar(ele_num_, "MagPotential");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_pot_[s] += mag_potential(ele_[e], site_[s], magz_[e], fnorm_.get(e), enorm_.get(e), etang_.get(e));
		}
	}
	return;
}

void magtet::total_gradient()
{
	int e, s;
	// malloc space
	mag_grad_.resize(site_num_, gctl::point3d_c(0.0, 0.0, 0.0));

    gctl::utility::progress_bar bar(ele_num_, "MagGradient");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_grad_[s] = mag_grad_[s] + 
				mag_gradient(ele_[e], site_[s], magz_[e], fnorm_.get(e), enorm_.get(e), etang_.get(e));
		}
	}
	return;
}

void magtet::total_tensor()
{
	int e, s;
	// malloc space
	mag_tensor_.resize(site_num_, gctl::tensor(0.0));

    gctl::utility::progress_bar bar(ele_num_, "MagTensor");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_tensor_[s] = mag_tensor_[s] + 
				mag_tensor(ele_[e], site_[s], magz_[e], fnorm_.get(e), enorm_.get(e), etang_.get(e));
		}
	}
	return;
}

double magtet::sign(double s)
{
	if (s > TOL) return 1.0;
	if (s < -1.0*TOL) return -1.0; 
	return 0.0;
}

void magtet::routine(const char *para_file)
{
	// initialize options
	gopt_.init_options(para_file);
	gopt_.show_options();

	// read tetgen files
	RUN_ECHO(read_tet(gopt_.get_value(TETFILE)), "Reading 3D model file");
	// read magnetization file
	RUN_ECHO(read_magz(gopt_.get_value(MAGFILE)), "Reading magnetization file");
	// read site file
	RUN_ECHO(init_site(gopt_.get_value(SITEFILE)), "Initiating observations points");
	// initialize tensors
	RUN_ECHO(cal_tensors(), "Calculating tensors");

	// calculate targets
	bool matched = false;
	std::string target_str = gopt_.get_value(CALTYPE);

	int pos = target_str.find("potential");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_potential(), "Calculating magnetic potential");
		matched = true;
	}

	pos = target_str.find("gradient");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_gradient(), "Calculating magnetic gradient");
		matched = true;
	}

	pos = target_str.find("tensor");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_tensor(), "Calculating magnetic tensor");
		matched = true;
	}

	if (!matched)
	{
		throw gctl::runtime_error("Invalid calculating type. From magtet::main(...)");
	}

	// output results
	RUN_ECHO(write_text(gopt_.get_value(OBSFILE)), "Writing outputs");
	return;
}