#include <optional>
#include <cmath>
#include "wrmath/geom/vector.h"
#include "wrmath/geom/orientation.h"


namespace wr {
namespace geom {


float
Heading2f(Vector2f vec)
{
	return vec.angle(Basis2f[Basis_x]);
}


float
Heading3f(Vector3f vec)
{
	Vector2f	vec2f {vec[0], vec[1]};
	return Heading2f(vec2f);
}


double
Heading2d(Vector2d vec)
{
	return vec.angle(Basis2d[Basis_x]);
}


double
Heading3d(Vector3d vec)
{
	Vector2d	vec2d {vec[0], vec[1]};
	return Heading2d(vec2d);
}


std::optional<double>
RBearing3d(Vector3d origin, Vector3d point)
{
	if (origin.isZero() || point.isZero())
		return std::nullopt;

	Vector3d u_origin = origin.unitVector();
	Vector3d u_point  = point.unitVector();

	double theta_h = std::atan2(u_origin[1], u_origin[0]);
	double theta_d = std::atan2(u_point[1],  u_point[0]);
	double diff = theta_d - theta_h;
	if (diff < 0.0) diff += 2.0 * M_PI;
	return diff;
}

std::optional<float>
RBearing3f(Vector3f origin, Vector3f point)
{
	if (origin.isZero() || point.isZero())
		return std::nullopt;

	Vector3f u_origin = origin.unitVector();
	Vector3f u_point  = point.unitVector();

	float theta_h = std::atan2(u_origin[1], u_origin[0]);
	float theta_d = std::atan2(u_point[1],  u_point[0]);
	float diff = theta_d - theta_h;
	if (diff < 0.0f) diff += 2.0f * (float)M_PI;
	return diff;
}

std::optional<double>
ABearing3d(Vector3d point)
{
	if (point.isZero()) return std::nullopt;
	Vector3d unit = point.unitVector();
	double theta = std::atan2(unit[0], unit[1]);
	if (theta < 0.0) theta += 2.0 * M_PI;
	return theta;
}

std::optional<float>
ABearing3f(Vector3f point)
{
	if (point.isZero()) return std::nullopt;
	Vector3f unit = point.unitVector();
	float theta = std::atan2(unit[0], unit[1]);
	if (theta < 0.0f) theta += 2.0f * (float)M_PI;
	return theta;
}

std::optional<double>
CompassHeading3d(Vector3d v)
{
	Vector2d v2 {v[0], v[1]};
	if (v2.isZero()) return std::nullopt;
	Vector2d unit = v2.unitVector();
	Vector2d y_basis {0.0, 1.0};
	double theta = unit.angle2(y_basis);
	if (theta < 0.0) theta += 2.0 * M_PI;
	return theta;
}

std::optional<float>
CompassHeading3f(Vector3f v)
{
	Vector2f v2 {v[0], v[1]};
	if (v2.isZero()) return std::nullopt;
	Vector2f unit = v2.unitVector();
	Vector2f y_basis {0.0f, 1.0f};
	float theta = unit.angle2(y_basis);
	if (theta < 0.0f) theta += 2.0f * (float)M_PI;
	return theta;
}


} // namespace geom
} // namespace wr