#include <psemek/geom/camera.hpp>
#include <psemek/geom/gauss.hpp>
#include <psemek/geom/orthographic.hpp>
#include <psemek/geom/perspective.hpp>
#include <psemek/geom/translation.hpp>
#include <psemek/geom/rotation.hpp>
#include <psemek/geom/permutation.hpp>
#include <psemek/geom/scale.hpp>

#include <cmath>

namespace psemek::geom
{

	matrix<float, 4, 4> camera::transform() const
	{
		return projection() * view();
	}

	point<float, 3> camera::position() const
	{
		vector<float, 4> b{ 0.f, 0.f, 0.f, 1.f };
		gauss(view(), b);
		return { b[0], b[1], b[2] };
	}

	vector<float, 3> camera::axis_x() const
	{
		vector<float, 4> b{ 1.f, 0.f, 0.f, 0.f };
		gauss(view(), b);
		return { b[0], b[1], b[2] };
	}

	vector<float, 3> camera::axis_y() const
	{
		vector<float, 4> b{ 0.f, 1.f, 0.f, 0.f };
		gauss(view(), b);
		return { b[0], b[1], b[2] };
	}

	vector<float, 3> camera::axis_z() const
	{
		vector<float, 4> b{ 0.f, 0.f, 1.f, 0.f };
		gauss(view(), b);
		return { b[0], b[1], b[2] };
	}

	vector<float, 3> camera::direction() const
	{
		return -axis_z();
	}

	vector<float, 3> camera::direction(float x, float y) const
	{
		vector<float, 4> b{ x, y, -1.f, 1.f};
		gauss(transform(), b);

		point<float, 3> p{ b[0] / b[3], b[1] / b[3], b[2] / b[3] };
		return p - position();
	}

	std::array<vector<float, 4>, 6> camera::clip_planes() const
	{
		auto const m = transpose(transform());

		std::array<vector<float, 4>, 6> p;

		p[0] = m * vector{1.f, 0.f, 0.f, 1.f};
		p[1] = m * vector{-1.f, 0.f, 0.f, 1.f};
		p[2] = m * vector{0.f, 1.f, 0.f, 1.f};
		p[3] = m * vector{0.f, -1.f, 0.f, 1.f};
		p[4] = m * vector{0.f, 0.f, 1.f, 1.f};
		p[5] = m * vector{0.f, 0.f, -1.f, 1.f};

		return p;
	}

	matrix<float, 4, 4> window_camera::view() const
	{
		return matrix<float, 4, 4>::identity();
	}

	matrix<float, 4, 4> window_camera::projection() const
	{
		return orthographic<float, 3>({{ {0.f, width}, {height, 0.f}, {-1.f, 1.f} }}).homogeneous_matrix();
	}

	matrix<float, 4, 4> orthographic_camera::view() const
	{
		return matrix<float, 4, 4>::identity();
	}

	matrix<float, 4, 4> orthographic_camera::projection() const
	{
		return orthographic<float, 3>(box).homogeneous_matrix();
	}

	matrix<float, 4, 4> perspective_camera::projection() const
	{
		return perspective<float, 3>(fov_x, fov_y, near_clip, far_clip).homogeneous_matrix();
	}

	void perspective_camera::set_fov(float fov_y, float aspect_ratio)
	{
		this->fov_y = fov_y;
		fov_x = 2.f * std::atan(aspect_ratio * std::tan(fov_y / 2.f));
	}

	matrix<float, 4, 4> spherical_camera::view() const
	{
		auto tr =
			  translation<float, 3>({0.f, 0.f, -distance}).transform()
			* swap<float, 3>(1, 2).transform()
			* scale<float, 3>({1.f, -1.f, 1.f}).transform()
			* plane_rotation<float, 3>(1, 2, elevation_angle).transform()
			* plane_rotation<float, 3>(1, 0, azimuthal_angle).transform()
			* translation<float, 3>({ -target[0], -target[1], -target[2] }).transform()
			;

		return tr.homogeneous_matrix();
	}

	static void rotate(geom::vector<float, 3> & a, geom::vector<float, 3> & b, float angle)
	{
		auto s = std::sin(angle);
		auto c = std::cos(angle);

		auto aa = a * c - b * s;
		auto bb = a * s + b * c;
		a = aa;
		b = bb;
	}

	void free_camera::rotateXY(float angle)
	{
		rotate(axes[0], axes[1], angle);
	}

	void free_camera::rotateYZ(float angle)
	{
		rotate(axes[1], axes[2], angle);
	}

	void free_camera::rotateZX(float angle)
	{
		rotate(axes[2], axes[0], angle);
	}

	matrix<float, 4, 4> free_camera::view() const
	{
		return homogeneous(by_rows(axes[0], axes[1], axes[2])) * translation{geom::point<float, 3>::zero() - pos}.homogeneous_matrix();
	}

}