#include <psemek/app/application_base.hpp>
#include <psemek/app/default_application_factory.hpp>

#include <psemek/phys/engine_3d.hpp>

#include <psemek/gfx/program.hpp>
#include <psemek/gfx/mesh.hpp>

#include <psemek/cg/body/icosahedron.hpp>
#include <psemek/cg/body/box.hpp>

#include <psemek/geom/camera.hpp>
#include <psemek/geom/mesh.hpp>
#include <psemek/geom/rotation.hpp>
#include <psemek/geom/translation.hpp>
#include <psemek/geom/scale.hpp>

#include <psemek/random/device.hpp>
#include <psemek/random/generator.hpp>
#include <psemek/random/uniform.hpp>
#include <psemek/random/uniform_sphere.hpp>
#include <psemek/random/uniform_ball.hpp>

#include <psemek/util/clock.hpp>

#include <psemek/log/log.hpp>

using namespace psemek;

static char const program_vs[] =
R"(#version 330

uniform mat4 u_camera_transform;
uniform mat4 u_object_transform;

layout (location = 0) in vec4 in_position;
layout (location = 1) in vec3 in_normal;

out vec3 position;
out vec3 normal;
out vec3 test_position;

void main()
{
	test_position = in_position.xyz;

	vec4 p = u_object_transform * in_position;
	position = p.xyz;
	gl_Position = u_camera_transform * p;

	normal = (u_object_transform * vec4(in_normal, 0.0)).xyz;
}
)";

static char const program_fs[] =
R"(#version 330

uniform vec3 u_light_direction;
uniform vec3 u_light_color;

uniform vec3 u_object_color;
uniform int u_grid;

layout (location = 0) out vec4 out_color;

in vec3 position;
in vec3 normal;
in vec3 test_position;

float mmin(vec3 v)
{
	return min(v.x, min(v.y, v.z));
}

void main()
{
	float lit = dot(normalize(normal), u_light_direction) * 0.5 + 0.5;

	vec3 object_color = u_object_color;
	if (u_grid == 1)
		object_color = mix(vec3(1.0), u_object_color, smoothstep(0.0, 0.05, mmin(abs(test_position))));

	vec3 color = u_light_color * object_color * lit;
	color = pow(color, vec3(1.0 / 2.2));
	out_color = vec4(color, 1.0);
}
)";

struct physics_3d_app
	: app::application_base
{
	physics_3d_app(options const &, context const &)
		: program_(program_vs, program_fs)
		, rng_{random::device{}}
	{
		camera_.near_clip = 0.1f;
		camera_.far_clip = 1000.f;
		camera_.fov_y = geom::rad(90.f);
		camera_.fov_x = camera_.fov_y;
		camera_.target = {0.f, 0.f, 0.f};
		camera_.distance = 10.f;
		camera_.elevation_angle = geom::rad(45.f);
		camera_.azimuthal_angle = geom::rad(30.f);

		camera_distance_tgt_ = camera_.distance;
		camera_azimuthal_angle_tgt_ = camera_.azimuthal_angle;
		camera_elevation_angle_tgt_ = camera_.elevation_angle;

		struct vertex
		{
			geom::point<float, 3> position;
			geom::vector<float, 3> normal;

			static void setup(gfx::mesh & m)
			{
				m.setup<geom::point<float, 3>, geom::vector<float, 3>>();
			}
		};

		{
			std::vector<vertex> vertices;
			vertices.push_back({{-10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
			vertices.push_back({{ 10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
			vertices.push_back({{-10.f,  10.f, 0.f}, {0.f, 0.f, 1.f}});
			vertices.push_back({{-10.f,  10.f, 0.f}, {0.f, 0.f, 1.f}});
			vertices.push_back({{ 10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
			vertices.push_back({{ 10.f,  10.f, 0.f}, {0.f, 0.f, 1.f}});

			vertex::setup(plane_mesh_);
			plane_mesh_.load(vertices, gl::TRIANGLES, gl::STATIC_DRAW);
		}

		{
			cg::icosahedron<float> body{{0.f, 0.f, 0.f}, 1.f};

			auto const & body_vertices = cg::vertices(body);
			auto const & body_triangles = cg::triangles(body);

			std::vector<geom::point<float, 3>> positions;
			std::copy(body_vertices.begin(), body_vertices.end(), std::back_inserter(positions));

			std::vector<geom::triangle<std::uint32_t>> triangles;
			for (auto const & t : body_triangles)
				triangles.push_back({t[0], t[1], t[2]});

			for (int iterations = 0; iterations < 2; ++iterations)
			{
				geom::subdivide(positions, triangles);
				for (auto & p : positions)
					p = p.zero() + geom::normalized(p - p.zero());
			}

			auto normals = geom::smooth_normals(positions, triangles);


			std::vector<vertex> vertices;
			for (std::size_t i = 0; i < positions.size(); ++i)
				vertices.push_back({positions[i], normals[i]});

			vertex::setup(sphere_mesh_);
			sphere_mesh_.load(vertices, triangles, gl::STATIC_DRAW);
		}

		{
			cg::box<float, 3> body{{{{-1.f, 1.f}, {-1.f, 1.f}, {-1.f, 1.f}}}};

			auto const & body_vertices = cg::vertices(body);
			auto const & body_triangles = cg::triangles(body);

			std::vector<geom::point<float, 3>> positions;
			std::copy(body_vertices.begin(), body_vertices.end(), std::back_inserter(positions));

			std::vector<geom::triangle<std::uint32_t>> triangles;
			for (auto const & t : body_triangles)
				triangles.push_back({t[0], t[1], t[2]});

			auto flat_positions = geom::deindex(positions, triangles);

			std::vector<vertex> vertices;
			for (auto const & t : flat_positions)
			{
				auto n = geom::normal(t[0], t[1], t[2]);
				vertices.push_back({t[0], n});
				vertices.push_back({t[1], n});
				vertices.push_back({t[2], n});
			}

			vertex::setup(box_mesh_);
			box_mesh_.load(vertices, gl::TRIANGLES, gl::STATIC_DRAW);
		}

		engine_.set_gravity({0.f, 0.f, -10.f});
//		auto h0 = engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 1.f, 10.f}), {{-4.f,  0.25f, 2.f}});
//		auto h1 = engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 1.f, 10.f}), {{ 4.f, -0.25f, 2.f}});
//		engine_.get_object_state(h0).velocity = { 3.f, 0.f, 0.f};
//		engine_.get_object_state(h1).velocity = {-3.f, 0.f, 0.f};
//		(void)h0;
//		(void)h1;

		engine_.add_object(engine_.add_shape(phys3d::half_space{{0.f, 0.f, 1.f}, 0.f}), engine_.add_material({1.f, 0.f, 10.f}), {{0.f, 0.f, 0.f}});

//		engine_.add_object(engine_.add_shape(phys3d::half_space{{ 1.f,  0.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
//		engine_.add_object(engine_.add_shape(phys3d::half_space{{-1.f,  0.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
//		engine_.add_object(engine_.add_shape(phys3d::half_space{{ 0.f,  1.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
//		engine_.add_object(engine_.add_shape(phys3d::half_space{{ 0.f, -1.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
	}

	void on_event(app::resize_event const & event) override
	{
		app::application_base::on_event(event);

		camera_.set_fov(camera_.fov_y, (1.f * event.size[0]) / event.size[1]);
	}

	void on_event(app::mouse_move_event const & event) override
	{
		auto const old_mouse = state().mouse;

		app::application_base::on_event(event);

		if (state().mouse_button_down.contains(app::mouse_button::right))
		{
			auto const delta = event.position - old_mouse;
			camera_elevation_angle_tgt_ += delta[1] * 0.003f;
			camera_azimuthal_angle_tgt_ -= delta[0] * 0.003f;
		}
	}

	void on_event(app::mouse_wheel_event const & event) override
	{
		app::application_base::on_event(event);

		camera_distance_tgt_ *= std::pow(0.8f, event.delta);
	}

	void on_event(app::key_event const & event) override
	{
		if (event.down && event.key == app::keycode::SPACE)
		{
//			float r = 7.f;
//			engine_.add_object(engine_.add_shape(phys3d::ball{3.f}), engine_.add_material({1.f, 0.125f, 50.f}), {{random::uniform(rng_, -r, r), random::uniform(rng_, -r, r), 20.f}});


//			engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 0.5f, 50.f}), {{0.f, 0.f, 20.f}});

			phys3d::object_state state;
			state.rotation.coords = random::uniform_sphere_vector_distribution<float, 4>()(rng_);
			if (!engine_.is_object(1))
				state.position = {0.f, 0.f, 4.f};
			else
				state.position = {0.01f, 0.01f, 4.f};
//			state.angular_velocity = random::uniform_ball_vector_distribution<float, 3>(25.f)(rng_);
			engine_.add_object(engine_.add_shape(phys3d::box{{1.5f, 1.5f, 1.5f}}), engine_.add_material({1.f, 0.f, 10.f}), state);
//			engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 0.5f, 100.f}), state);
		}
		else if (event.down && event.key == app::keycode::F)
		{
			float const f = 1.f;
			for (std::uint32_t h = 1; h < engine_.object_count(); ++h)
				if (engine_.is_object(h))
					engine_.add_impulse(h, {random::uniform(rng_, -f, f), random::uniform(rng_, -f, f), random::uniform(rng_, 0.f, 0.f)});
		}
	}

	void update() override
	{
		float const dt = clock_.restart().count();

		physics_lag_ += dt;

		float const ph_dt = 0.004f;

		while (physics_lag_ >= ph_dt)
		{
			physics_lag_ -= ph_dt;
			engine_.update(ph_dt);

			float E = 0.f;
			for (phys3d::engine::object_handle h = 1; h < engine_.object_count(); ++h)
			{
				if (!engine_.is_object(h)) continue;

				auto m = engine_.get_object_mass(h);
				auto I = engine_.get_object_inertia(h);
				auto v = engine_.get_object_state(h).velocity;
				auto w = engine_.get_object_state(h).angular_velocity;
				auto H = engine_.get_object_state(h).position[2];

				E += 0.5f * geom::length_sqr(v) * m + 0.5f * geom::dot(w, I * w) + m * 10.f * H;
			}
			log::info() << "Energy: " << E;
		}

		camera_.distance += (camera_distance_tgt_ - camera_.distance) * (1.f - std::exp(- 20.f * dt));
		camera_.azimuthal_angle += (camera_azimuthal_angle_tgt_ - camera_.azimuthal_angle) * (1.f - std::exp(- 20.f * dt));
		camera_.elevation_angle += (camera_elevation_angle_tgt_ - camera_.elevation_angle) * (1.f - std::exp(- 20.f * dt));
	}

	void present() override
	{
		gl::ClearColor(0.8f, 0.8f, 1.f, 1.f);
		gl::Clear(gl::COLOR_BUFFER_BIT | gl::DEPTH_BUFFER_BIT);

		gl::Enable(gl::DEPTH_TEST);
		gl::DepthFunc(gl::LESS);

		program_.bind();
		program_["u_camera_transform"] = camera_.transform();

		program_["u_light_direction"] = geom::normalized(geom::vector{1.f, 1.f, 1.f});
		program_["u_light_color"] = geom::vector{1.f, 1.f, 1.f};

		program_["u_object_transform"] = geom::matrix<float, 4, 4>::identity();
		program_["u_object_color"] = geom::vector{0.5f, 0.5f, 0.5f};
		program_["u_grid"] = 0;
		plane_mesh_.draw();

		program_["u_object_color"] = geom::vector{0.f, 0.f, 1.f};
		program_["u_grid"] = 1;
		for (phys3d::engine::object_handle h = 0; h < engine_.object_count(); ++h)
		{
			if (!engine_.is_object(h)) continue;

			auto sh = engine_.get_shape(engine_.get_object_shape(h));

			if (auto s = std::get_if<phys3d::ball const *>(&sh))
			{
				program_["u_object_transform"] =
						geom::translation<float, 3>(engine_.get_object_state(h).position - geom::point<float, 3>::zero()).homogeneous_matrix() *
						geom::quaternion_rotation<float>(engine_.get_object_state(h).rotation).homogeneous_matrix() *
						geom::scale<float, 3>((*s)->radius).homogeneous_matrix();
				sphere_mesh_.draw();
			}
			else if (auto s = std::get_if<phys3d::box const *>(&sh))
			{
				program_["u_object_transform"] =
						geom::translation<float, 3>(engine_.get_object_state(h).position - geom::point<float, 3>::zero()).homogeneous_matrix() *
						geom::quaternion_rotation<float>(engine_.get_object_state(h).rotation).homogeneous_matrix() *
						geom::scale<float, 3>((*s)->dimensions / 2.f).homogeneous_matrix();
				box_mesh_.draw();
			}
		}
	}

private:
	gfx::program program_;

	geom::spherical_camera camera_;
	float camera_distance_tgt_;
	float camera_azimuthal_angle_tgt_;
	float camera_elevation_angle_tgt_;

	gfx::mesh plane_mesh_;
	gfx::mesh sphere_mesh_;
	gfx::mesh box_mesh_;

	util::clock<std::chrono::duration<float>, std::chrono::high_resolution_clock> clock_;

	phys3d::engine engine_;
	float physics_lag_ = 0.f;

	random::generator rng_;
};

namespace psemek::app
{

	std::unique_ptr<application::factory> make_application_factory()
	{
		return default_application_factory<physics_3d_app>({.name = "Physics 3D example"});
	}

}