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

#ifndef _GCTL_POINT3C_H
#define _GCTL_POINT3C_H

#include "point.h"
#include "../core/array.h"

namespace gctl
{
	typedef point3c<double> point3dc;
	typedef point3c<float>  point3fc;

	/**
	 * @brief      直角坐标系内的实点
	 */
	template <typename T>
	struct point3c
	{
		T x, y, z;
		/**
		 * Constructor
		 */
		point3c();
		/**
		 * @brief      Constructor with initial parameters
		 *
		 * @param[in]  a     x coordinate
		 * @param[in]  b     y coordinate
		 * @param[in]  c     z coordinate
		 */
		point3c(T a, T b, T c);
		/**
		 * @brief      Copy constructor
		 *
		 * @param[in]  b     the original point
		 */
		point3c(const point3c<T> &b);
		/**
		 * @brief      De-constructor
		 */
		virtual ~point3c(){}
		/**
		 * @brief      If the point has valid coordinates
		 *
		 * @return     true for valid, otherwise return false
		 */
		bool valid() const;
		/**
		 * @brief      Set coordinate values
		 *
		 * @param[in]  a     x coordinate
		 * @param[in]  b     y coordinate
		 * @param[in]  c     z coordinate
		 */
		void set(T a, T b, T c);
		/**
		 * @brief      Set coordinates
		 *
		 * @param[in]  b     the original point
		 */
		void set(const point3c<T> &b);
		/**
		 * @brief      Set the point/vector's module to the given value.
		 *
		 * @param[in]  mod      The new module
		 * @param[in]  cut_off  The round value to zero
		 */
		void set2module(T mod, T cut_off = GCTL_ZERO);
		/**
		 * @brief      Return vector's module
		 *
		 * @return     module
		 */
		T module() const;
		/**
		 * @brief      Return the corresponding spherical coordinates
		 *
		 * @return     spherical coordinates point
		 */
		point3s<T> c2s() const;
		/**
		 * @brief      Return the normal vector
		 *
		 * @return     normal vector
		 */
		point3c<T> normal() const;
		/**
		 * @brief      Parse the coordinates from a string
		 * 
		 * @note       String format: (x, y, z)
		 *
		 * @param[in]  str   The input string
		 */
		void str(std::string str);
		/**
		 * @brief      Sets the i/o precision of the type.
		 *
		 * @param[in]  psn   The desired precision
		 */
		void set_io_precision(int psn);
		/**
		 * @brief      计算三维空间矢量与坐标轴的夹角（弧度），以引用的形式返回。
		 *
		 * @param      angle_x  矢量与 x 轴的夹角
		 * @param      angle_y  矢量与 y 轴的夹角
		 * @param      angle_z  矢量与 z 轴的夹角
		 */
		void axis_angle(T &angle_x, T &angle_y, T &angle_z);
		/**
		 * @brief      绕x y z轴旋转point3dc
		 *
		 * @param[in]  angx  绕x轴逆时针旋转的角度（单位为弧度）。
		 * @param[in]  angy  绕y轴逆时针旋转的角度（单位为弧度）。
		 * @param[in]  angz  绕z轴逆时针旋转的角度（单位为弧度）。
		 *
		 * @return     旋转后的矢量。
		 */
		point3c<T> rotate(T angx, T angy, T angz);
		/**
		 * @brief      绕通过原点的旋转轴旋转point3dc
		 *
		 * @param[in]  axis  旋转轴
		 * @param[in]  ang   绕旋转轴逆时针旋转的角度（单位为弧度）。
		 *
		 * @return     返回的矢量。
		 */
		point3c<T> rotate(const point3c<T> &axis, double ang);
		/**
		 * @brief      输出位置
		 *
		 * @param      os    输出流
		 */
		void out_loc(std::ostream &os, char deli) const;
		/**
		 * @brief      输入位置
		 *
		 * @param      os    输入流
		 */
		void in_loc(std::istream &os);
		/**
		 * @brief      返回位置
		 *
		 * @return     位置
		 */
		point3c<T> get_loc() const;
	};

	template <typename T>
	gctl::point3c<T>::point3c()
	{
		x = y = z = NAN;
	}

	template <typename T>
	gctl::point3c<T>::point3c(T a, T b, T c)
	{
		set(a, b, c);
	}

	template <typename T>
	gctl::point3c<T>::point3c(const point3c<T> &b)
	{
		set(b);
	}

	template <typename T>
	bool gctl::point3c<T>::valid() const
	{
		if (std::isnan(x) || std::isnan(y) || std::isnan(z)) return false;
		if (std::isinf(x) || std::isinf(y) || std::isinf(z)) return false;
		return true;
	}

	template <typename T>
	void gctl::point3c<T>::set(T a, T b, T c)
	{
		if (std::isnan(a) || std::isnan(b) || std::isnan(c) || 
			std::isinf(a) || std::isinf(b) || std::isinf(c))
		{
			throw invalid_argument("Invalid value detected. From point3c::set(...)");
		}

		x = a; y = b; z = c;
		return;
	}

	template <typename T>
	void gctl::point3c<T>::set(const point3c<T> &b)
	{
		if (std::isnan(b.x) || std::isnan(b.y) || std::isnan(b.z) || 
			std::isinf(b.x) || std::isinf(b.y) || std::isinf(b.z))
		{
			throw invalid_argument("Invalid value detected. From point3c::set(...)");
		}

		x = b.x; y = b.y; z = b.z;
		return;
	}

	template <typename T>
	void gctl::point3c<T>::set2module(T mod, T cut_off)
	{
		if (cut_off <= 0.0)
		{
			throw invalid_argument("Invalid cut-off value. From point3c::set2module(...)");
		}

		if (mod < cut_off && mod > -1.0*cut_off)
		{
			x = y = z = 0.0;
			return;
		}

		T old_mod = module();
		if (old_mod < cut_off)
		{
			throw length_error("Zero module. From point3c::set2module(...)");
		}

		x = x*mod/old_mod;
		y = y*mod/old_mod;
		z = z*mod/old_mod;
		return;
	}

	template <typename T>
	T gctl::point3c<T>::module() const
	{
		return sqrt(x*x + y*y + z*z);
	}

	template <typename T>
	gctl::point3s<T> gctl::point3c<T>::c2s() const
	{
		point3s<T> outs;
		outs.rad = module();
		if (outs.rad <= GCTL_ZERO) //点距离原点极近 将点置于原点
		{
			throw runtime_error("The point is at the origin. From point3c::c2s()");
		}

		outs.lat = 90.0 - acos(z/outs.rad)*180.0/GCTL_Pi;
		outs.lon = atan2(y, x)*180.0/GCTL_Pi;
		return outs;
	}

	template <typename T>
	gctl::point3c<T> gctl::point3c<T>::normal() const
	{
		point3c<T> nor_v;
		double length = module();
		if (length <= GCTL_ZERO) //如果矢量为0则令向量与z轴重合
		{
			throw runtime_error("The point is at the origin. From point3c::normal()");
		}

		nor_v.x = x/length;
		nor_v.y = y/length;
		nor_v.z = z/length;
		return nor_v;
	}

	template <typename T>
	void gctl::point3c<T>::str(std::string str)
	{
		std::smatch ret;
		std::regex pattern("\\((-?\\d*\\.?\\d+?),[ ]*(-?\\d*\\.?\\d+?),[ ]*(-?\\d*\\.?\\d+?)\\)");

		if (regex_search(str, ret, pattern))
		{
			x = atof(std::string(ret[1]).c_str());
			y = atof(std::string(ret[2]).c_str());
			z = atof(std::string(ret[3]).c_str());
			return;
		}

		throw runtime_error("Fail to parse the input string: " + str + ". From point3c::str(...)");
	}

	template <typename T>
	void gctl::point3c<T>::set_io_precision(int psn)
	{
		if (psn < 0)
		{
			throw invalid_argument("Invalid precision. From point3c::set_io_precision(...)");
		}

		io_psn = psn;
		return;
	}

	template <typename T>
	void gctl::point3c<T>::axis_angle(T &angle_x, T &angle_y, T &angle_z)
	{
		double length = module();
		if (length <= GCTL_ZERO)
		{
			throw runtime_error("The point is at the origin. From point3c::axis_angle(...)");
		}
		else
		{
			angle_x = acos(x/length);
			angle_y = acos(y/length);
			angle_z = acos(z/length);
			return;
		}
	}

	template <typename T>
	gctl::point3c<T> gctl::point3c<T>::rotate(T angx, T angy, T angz)
	{
		gctl::point3c<T> cp, temp_cp;
		// 绕x轴旋转
		cp.x = x;
		cp.y = cos(angx)*y - sin(angx)*z;
		cp.z = sin(angx)*y + cos(angx)*z;
		// 绕y轴旋转
		temp_cp.x = cos(angy)*cp.x + sin(angy)*cp.z;
		temp_cp.y = cp.y;
		temp_cp.z = -1.0*sin(angy)*cp.x + cos(angy)*cp.z;
		// 绕z轴旋转
		cp.x = cos(angz)*temp_cp.x - sin(angz)*temp_cp.y;
		cp.y = sin(angz)*temp_cp.x + cos(angz)*temp_cp.y;
		cp.z = temp_cp.z;
		return cp;
	}

	template <typename T>
	gctl::point3c<T> gctl::point3c<T>::rotate(const point3c<T> &axis, double ang)
	{
		point3c<T> cp;
		double A[3][3], A2[3][3], A3[3][3], M[3][3];
		point3c<T> nor_axis = axis.normal();
		A[0][0] = nor_axis.x*nor_axis.x; A[0][1] = nor_axis.x*nor_axis.y; A[0][2] = nor_axis.x*nor_axis.z;
		A[1][0] = nor_axis.y*nor_axis.x; A[1][1] = nor_axis.y*nor_axis.y; A[1][2] = nor_axis.y*nor_axis.z;
		A[2][0] = nor_axis.z*nor_axis.x; A[2][1] = nor_axis.z*nor_axis.y; A[2][2] = nor_axis.z*nor_axis.z;

		A2[0][0] = 1.0-nor_axis.x*nor_axis.x; A2[0][1] = nor_axis.x*nor_axis.y;     A2[0][2] = nor_axis.x*nor_axis.z;
		A2[1][0] = nor_axis.y*nor_axis.x;     A2[1][1] = 1.0-nor_axis.y*nor_axis.y; A2[1][2] = nor_axis.y*nor_axis.z;
		A2[2][0] = nor_axis.z*nor_axis.x;     A2[2][1] = nor_axis.z*nor_axis.y;     A2[2][2] = 1.0-nor_axis.z*nor_axis.z;

		A3[0][0] = 0.0;             A3[0][1] = -1.0*nor_axis.z; A3[0][2] = nor_axis.y;
		A3[1][0] = nor_axis.z;      A3[1][1] = 0.0;             A3[1][2] = -1.0*nor_axis.x;
		A3[2][0] = -1.0*nor_axis.y; A3[2][1] = nor_axis.x;      A3[2][2] = 0.0;

		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				M[i][j] = A[i][j] + cos(ang)*A2[i][j] + sin(ang)*A3[i][j];
			}
		}

		cp.x = x*M[0][0] + y*M[1][0] + z*M[2][0];
		cp.y = x*M[0][1] + y*M[1][1] + z*M[2][1];
		cp.z = x*M[0][2] + y*M[1][2] + z*M[2][2];
		return cp;
	}

	template <typename T>
	void gctl::point3c<T>::out_loc(std::ostream &os, char deli) const
	{
		os << std::setprecision(io_psn) << x << deli << y << deli << z;
		return;
	}

	template <typename T>
	void gctl::point3c<T>::in_loc(std::istream &os)
	{
		os >> x >> y >> z;
		return;
	}

	template <typename T>
	gctl::point3c<T> gctl::point3c<T>::get_loc() const
	{
		return point3c<T>(x, y, z);
	}

	template <typename T>
	bool operator ==(const point3c<T> &a, const point3c<T> &b)
	{
		if( fabs(a.x - b.x) < GCTL_ZERO && 
			fabs(a.y - b.y) < GCTL_ZERO && 
			fabs(a.z - b.z) < GCTL_ZERO)
		{
			return true;
		}
		else return false;
	}

	template <typename T>
	bool operator !=(const point3c<T> &a, const point3c<T> &b)
	{
		if( fabs(a.x - b.x) >= GCTL_ZERO || 
			fabs(a.y - b.y) >= GCTL_ZERO || 
			fabs(a.z - b.z) >= GCTL_ZERO)
		{
			return true;
		}
		else return false;
	}

	template <typename T>
	point3c<T> operator -(const point3c<T> &a, const point3c<T> &b)
	{
		point3c<T> m;
		m.x = a.x-b.x;
		m.y = a.y-b.y;
		m.z = a.z-b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator +(const point3c<T> &a, const point3c<T> &b)
	{
		gctl::point3c<T> m;
		m.x = a.x+b.x;
		m.y = a.y+b.y;
		m.z = a.z+b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(int sign, const point3c<T> &b)
	{
		gctl::point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(float sign, const point3c<T> &b)
	{
		gctl::point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(double sign, const point3c<T> &b)
	{
		gctl::point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(const point3c<T> &b, int sign)
	{
		gctl::point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(const point3c<T> &b, float sign)
	{
		gctl::point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	point3c<T> operator *(const point3c<T> &b, double sign)
	{
		point3c<T> m;
		m.x = sign*b.x;
		m.y = sign*b.y;
		m.z = sign*b.z;
		return m;
	}

	template <typename T>
	std::ostream &operator <<(std::ostream & os, const point3c<T> &a)
	{
		os << std::setprecision(io_psn) << a.x << " " << a.y << " " << a.z;
		return os;
	}

	template <typename T>
	std::istream &operator >>(std::istream & os, point3c<T> &a)
	{
		os >> a.x >> a.y >> a.z;
		return os;
	}

	/**
	 * @brief      两个三维空间向量的点积
	 *
	 * @param[in]  a     三维空间内的一个向量或实点a。
	 * @param[in]  b     三维空间内的一个向量或实点b。
	 *
	 * @return     点积
	 */
	template <typename T>
	double dot(const point3c<T> &a, const point3c<T> &b)
	{
		return a.x*b.x+a.y*b.y+a.z*b.z;
	}

	/**
	 * @brief      矢量叉乘 注意矢量的叉乘是有顺序的 这里默认为第一个矢量叉乘第二个矢量
	 *
	 * @param[in]  a        向量a
	 * @param[in]  b        向量b
	 * @param[in]  cut_off  截断误差。当叉乘向量的任一方向的坐标值的绝对值小于截断误差时将直接置为0。
	 * 截断误差越小则对输入向量的精度要求越高，在实际计算中适当放开截断误差有助于使算法稳定。这个参数有默认值，
	 * 为 gctl_macro.h 中预设的 GCTL_ZERO 值。
	 *
	 * @return     两个向量的叉乘
	 */
	template <typename T>
	point3c<T> cross(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
	{
		gctl::point3c<T> v;
		v.x = a.y*b.z-a.z*b.y;
		v.y = a.z*b.x-a.x*b.z;
		v.z = a.x*b.y-a.y*b.x;
		if (fabs(v.x) <= cut_off) v.x = 0;
		if (fabs(v.y) <= cut_off) v.y = 0;
		if (fabs(v.z) <= cut_off) v.z = 0;
		return v;
	}

	/**
	 * @brief      两个矢量之间的距离
	 *
	 * @param[in]  a     三维空间内的一个向量或实点a。
	 * @param[in]  b     三维空间内的一个向量或实点b。
	 *
	 * @return     两点之间的距离
	 */
	template <typename T>
	double distance(const point3c<T> &a, const point3c<T> &b)
	{
		return (a - b).module();
	}

	/**
	 * @brief      函数判断两个point3dc类型是否等于
	 *
	 * @param[in]  a     三维空间内的一个向量或实点a。
	 * @param[in]  b     三维空间内的一个向量或实点b。
	 * @param[in]  cut_off  截断误差，默认值为 gctl_macro.h 中预设的 GCTL_ZERO 值，可在调用时改变。
	 * 注意比较的精度为 cut_off 减一位，比如 cut_off 为1e-10则比较的精度为小数点后9位。
	 *
	 * @return     是否相等。
	 */
	template <typename T>
	bool isequal(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
	{
		if( fabs(a.x - b.x) < cut_off && 
			fabs(a.y - b.y) < cut_off &&
			fabs(a.z - b.z) < cut_off)
		{
			return true;
		}
		else return false;
	}

	/**
	 * @brief      函数判断两个point3dc类型是否不等于
	 *
	 * @param[in]  a     三维空间内的一个向量或实点a。
	 * @param[in]  b     三维空间内的一个向量或实点b。
	 * @param[in]  cut_off  截断误差，默认值为 gctl_macro.h 中预设的 GCTL_ZERO 值，可在调用时改变。
	 *
	 * @return     是否不相等。
	 */
	template <typename T>
	bool notequal(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
	{
		if( fabs(a.x - b.x) >= cut_off || 
			fabs(a.y - b.y) >= cut_off || 
			fabs(a.z - b.z) >= cut_off)
		{
			return true;
		}
		else return false;
	}

	/**
	 * @brief      计算一个矢量的单位矢量相对一个标量（如顶点半径）的偏导数矢量
	 *
	 * @param[in]  n     该矢量
	 * @param[in]  n_rj  该矢量对于这个标量的偏导数矢量
	 *
	 * @return     偏导数矢量
	 */
	template <typename T>
	point3c<T> nr_dr(const point3c<T> &v, const point3c<T> &v_rj)
	{
		point3c<T> out, n;
		double moduel_v = v.module();
		n.x = v.x/moduel_v;
		n.y = v.y/moduel_v;
		n.z = v.z/moduel_v;

		double temp = dot(v, v_rj)/moduel_v;
		out.x = (v_rj.x - n.x*temp)/moduel_v;
		out.y = (v_rj.y - n.y*temp)/moduel_v;
		out.z = (v_rj.z - n.z*temp)/moduel_v;

		return out;
	}

	/**
	 * @brief      生成二维网格节点数组，其中每一个点都是一个三维坐标点。
	 * 
	 * @note 数组排序方式默认为左下至右上（BtmLeft），按行优先排列。可更改为左上至右下（TopLeft）。
	 *
	 * @param      out_ps  返回网格节点数组
	 * @param[in]  xmin    x最小值
	 * @param[in]  xmax    x最大值
	 * @param[in]  ymin    y最小值
	 * @param[in]  ymax    y最大值
	 * @param[in]  dx      x间隔
	 * @param[in]  dy      y间隔
	 * @param[in]  ele     高程值
	 * @param[in]  cor     网格起始位置
	 */
	template <typename T>
	void grid_points_2d(array<point3c<T>> &out_ps, T xmin, T xmax, T ymin, T ymax, 
		T dx, T dy, T ele, matrix_corner_e cor = BtmLeft)
	{
		if (xmin >= xmax || ymin >= ymax || 
			xmin + dx > xmax || ymin + dy > ymax || 
			dx <= 0 || dy <= 0)
		{
			throw std::invalid_argument("[gctl::grid_points_2d] Invalid parameters.");
		}

		int xnum = round((xmax-xmin)/dx) + 1;
		int ynum = round((ymax-ymin)/dy) + 1;

		out_ps.resize(xnum*ynum);	
		if (cor == BtmLeft)
		{
			for (int j = 0; j < ynum; j++)
			{
				for (int i = 0; i < xnum; i++)
				{
					out_ps[j*xnum + i].x = xmin + dx*i;
					out_ps[j*xnum + i].y = ymin + dy*j;
					out_ps[j*xnum + i].z = ele;
				}
			}
		}
		else // cor == TopLeft
		{
			for (int j = 0; j < ynum; j++)
			{
				for (int i = 0; i < xnum; i++)
				{
					out_ps[j*xnum + i].x = xmin + dx*i;
					out_ps[j*xnum + i].y = ymax - dy*j;
					out_ps[j*xnum + i].z = ele;
				}
			}
		}
		return;
	}
};

#endif // _GCTL_POINT3C_H