#include <psemek/app/app.hpp>
#include <psemek/app/main.hpp>

#include <psemek/phys/engine_2d.hpp>

#include <psemek/gfx/painter.hpp>

#include <psemek/geom/box.hpp>
#include <psemek/geom/camera.hpp>
#include <psemek/geom/rotation.hpp>

#include <psemek/util/clock.hpp>
#include <psemek/util/recursive.hpp>
#include <psemek/util/profiler.hpp>

#include <psemek/async/event_loop.hpp>

#include <psemek/random/generator.hpp>
#include <psemek/random/uniform.hpp>

#include <cstdint>
#include <vector>

using namespace psemek;

struct physics_2d_app
	: app::app
{
	phys2d::engine physics;

	geom::box<float, 2> view_box;
	geom::box<float, 2> simulation_box;

	gfx::painter painter;

	util::clock<std::chrono::duration<float>> frame_clock;

	float physics_lag = 0.f;

	phys2d::engine::material_handle material;
	phys2d::engine::shape_handle ball_shape, large_ball_shape, box_shape, small_box_shape, wide_box_shape;
	phys2d::engine::group_handle ball_group, box_group;

	float const world_width = 5.f;
	float const world_height = 5.f;

	float const line_width = 0.05f;

	float const ball_radius = 0.25f;
	float const box_width = 0.5f;
	float const box_height = 0.25f;

	std::vector<float> radiuses;

	std::optional<geom::point<float, 2>> mouse;

	std::optional<std::size_t> selected_ball;

	std::optional<geom::vector<float, 2>> drag_delta;

	std::size_t motor_id;

	float motor = 0.f;

	async::event_loop loop;

	random::generator gen;

	physics_2d_app()
		: app("Physics 2D example", 4)
	{
		simulation_box = {{{-world_width, world_width}, {-world_height, world_height}}};

		view_box = expand(simulation_box, 1.f);

		physics.set_gravity({0.f, -9.8f});

		float const inf = std::numeric_limits<float>::infinity();

		material = physics.add_material({1.f, 0.5f, 0.5f});

		ball_shape = physics.add_shape(phys2d::ball{ball_radius});
		ball_group = physics.create_group();

		large_ball_shape = physics.add_shape(phys2d::ball{8.f * ball_radius});

		box_shape = physics.add_shape(phys2d::box{box_width, box_height});
		box_group = physics.create_group();

		small_box_shape = physics.add_shape(phys2d::box{box_width / 2.f, box_height});

		wide_box_shape = physics.add_shape(phys2d::box{box_width * 8.f, box_height});

		int nx = std::ceil(simulation_box[0].length() / box_width);
		int ny = std::ceil(simulation_box[1].length() / box_height);

		// piramid
		if (false)
		{
			int ky = 10;

			for (int y = 0; y < ky; ++y)
			{
				for (int x = nx / 2 - ky / 2; x < nx / 2 - ky / 2 + (ky - y); ++x)
				{
					geom::point<float, 2> pos{simulation_box.corner((x + 0.5f * y + 0.5f) / nx, (y + 0.5f) / ny)};
					physics.add_object(box_group, box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
				}
			}
		}

		// pavement
//		if (false)
		{
			int chess = 1;
			for (int y = 0; y < 4; ++y)
			{
				if (chess && (y % 2))
				{
					geom::point<float, 2> pos{simulation_box.corner((0.25f) / nx, (y + 0.5f) / ny)};
					physics.add_object(box_group, small_box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
				}

				for (int x = 0; x < nx - chess * (y % 2); ++x)
				{
					geom::point<float, 2> pos{simulation_box.corner((x + 0.5f + chess * 0.5f * (y % 2)) / nx, (y + 0.5f) / ny)};
					physics.add_object(box_group, box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
				}

				if (chess && (y % 2))
				{
					geom::point<float, 2> pos{simulation_box.corner((nx - 0.25f) / nx, (y + 0.5f) / ny)};
					physics.add_object(box_group, small_box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
				}
			}
		}

		motor_id = physics.group_size(box_group);

//		physics.add_object(box_group, box_shape, material, {{- box_width * 1.2f, 0.f}, 0.f}, {});
//		physics.add_object(box_group, box_shape, material, {{  box_width * 1.2f, 0.f}, 0.f}, {});

		{

			if(false)
			for (int i = 0; i < 15; ++i)
			{
				float a = geom::rad(360 * i / 15.f);
				float r = ball_radius * 15.f / geom::pi;
				geom::point<float, 2> pos{r * std::cos(a), r * std::sin(a)};
				physics.add_object(ball_group, ball_shape, material, {pos, 0.f}, {});
			}
		}

		auto wall_group = physics.create_group();
		auto wall_material = physics.add_material({inf, 0.5f, 1.5f});

		auto wall_0_shape = physics.add_shape(phys2d::half_space{{1.f, 0.f}, simulation_box[0].min});
		physics.add_object(wall_group, wall_0_shape, wall_material, {}, {});

		auto wall_1_shape = physics.add_shape(phys2d::half_space{{-1.f, 0.f}, -simulation_box[0].max});
		physics.add_object(wall_group, wall_1_shape, wall_material, {}, {});

		auto wall_2_shape = physics.add_shape(phys2d::half_space{{0.f, 1.f}, simulation_box[1].min});
		physics.add_object(wall_group, wall_2_shape, wall_material, {}, {});

		auto wall_3_shape = physics.add_shape(phys2d::half_space{{0.f, -1.f}, -simulation_box[1].max});
		physics.add_object(wall_group, wall_3_shape, wall_material, {}, {});

		auto task = util::recursive([this, dx = box_width * 0.4f](auto && self) mutable -> void {
			physics.add_object(box_group, box_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});
//			physics.add_object(ball_group, ball_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});

//			float r = random::uniform_distribution<float>{0.05f, 0.5f}(gen);
//			auto new_shape = physics.add_shape(phys2d::ball{r});
//			physics.add_object(ball_group, new_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});
//			radiuses.push_back(r);

			dx *= -1.f;
			if (physics.group_size(box_group) < 100)
				loop.dispatch_at(std::chrono::system_clock::now() + std::chrono::milliseconds{100}, self);
		});
		(void)task;
//		task();

//		loop.dispatch_at(std::chrono::system_clock::now() + std::chrono::seconds{1}, [this]{
//			physics.add_object(ball_group, ball_shape, material, {simulation_box.corner(0.5f, 0.9f), 0.f}, {});
//		});

		loop.dispatch_at(std::chrono::system_clock::now() + std::chrono::seconds{1}, [this]{
			physics.explode(simulation_box.corner(0.5f, -0.2f), 1000.f, 100.f);
		});

//		loop.dispatch_at(std::chrono::system_clock::now() + std::chrono::seconds{5}, [this]{
//			stop();
//		});
	}

	~physics_2d_app()
	{
		util::profiler::dump();
	}

	void on_resize(int width, int height) override
	{
		app::on_resize(width, height);

		float const ratio = static_cast<float>(width) / height;

		float const c = view_box[0].center();
		float const l = view_box[1].length() * ratio;

		view_box[0] = {c - l / 2.f, c + l / 2.f};
	}

	void on_left_button_down() override
	{
		app::on_left_button_down();

		if (selected_ball && mouse)
		{
			auto const & s = physics.group_static_state(ball_group)[*selected_ball];
			auto r = s.position - *mouse;
			r = geom::plane_rotation<float, 2>(0, 1, -s.rotation)(r);
			drag_delta = r;
		}
		else if (mouse)
		{
//			physics.add_object(ball_group, ball_shape, material, {*mouse, 0.f}, {{0.f, 0.f}, 0.f});
//			physics.add_object(ball_group, large_ball_shape, material, {*mouse, 0.f}, {});
			physics.add_object(box_group, box_shape, material, {*mouse, 0.f}, {});
//			physics.add_object(box_group, wide_box_shape, material, {*mouse, 0.f}, {});
		}
	}

	void on_left_button_up() override
	{
		app::on_left_button_up();

		drag_delta = std::nullopt;
	}

	void on_right_button_down() override
	{
		app::on_right_button_down();

		if (mouse)
		{
			physics.add_object(ball_group, ball_shape, material, {*mouse, 0.f}, {{0.f, 0.f}, 0.f});
//			physics.add_object(box_group, box_shape, material, {*mouse, 0.f}, {});
//			physics.add_object(box_group, wide_box_shape, material, {*mouse, 0.f}, {});
//			physics.explode(*mouse, 10.f, 100.f);
		}
	}

	void on_mouse_move(int x, int y, int dx, int dy) override
	{
		app::on_mouse_move(x, y, dx, dy);

		float mx = x * 1.f / width();
		float my = 1.f - y * 1.f / height();

		mouse = view_box.corner(mx, my);
	}

	void update() override
	{
		util::profiler prof("update");

		float MOTOR = 15.f;

		float m = 0.f;
		if (is_key_down(SDLK_LEFT))
			m += MOTOR;
		if (is_key_down(SDLK_RIGHT))
			m += -MOTOR;
		m = 15.f;
		motor = m;

		auto apply_constraints = [this](float dt, float lambda){
			if (drag_delta && selected_ball)
			{
				auto & s = physics.group_static_state(ball_group)[*selected_ball];
				auto & d = physics.group_dynamic_state(ball_group)[*selected_ball];
	//			auto const & info = physics.info(ball_group, *selected_ball);
				{
					auto r = -*drag_delta;

					auto R = geom::plane_rotation<float, 2>(0, 1, s.rotation);

					geom::vector<float, 2> f;
					f[0] = s.position[0] + R(r)[0] - (*mouse)[0];
					f[1] = s.position[1] + R(r)[1] - (*mouse)[1];

					geom::matrix<float, 2, 3> J;
					J[0][0] = 1.f;
					J[0][1] = 0.f;
					J[0][2] = - std::sin(s.rotation) * r[0] + std::cos(s.rotation) * r[1];
					J[1][0] = 0.f;
					J[1][1] = 1.f;
					J[1][2] = std::cos(s.rotation) * r[0] - std::sin(s.rotation) * r[1];

					geom::vector<float, 3> v;
					v[0] = d.velocity[0];
					v[1] = d.velocity[1];
					v[2] = d.angular_velocity;

					auto dv = geom::least_squares(J, -(1.f / 8.f) * f / dt - (J * v));
					if (dv)
					{
						d.velocity[0] += (*dv)[0] * lambda;
						d.velocity[1] += (*dv)[1] * lambda;
						d.angular_velocity += (*dv)[2] * lambda;
					}
				}
			}

			auto solve_distance = [this, dt, lambda](auto group, std::size_t i, std::size_t j, float target)
			{
				auto & s0 = physics.group_static_state(group)[i];
				auto & s1 = physics.group_static_state(group)[j];
				auto & d0 = physics.group_dynamic_state(group)[i];
				auto & d1 = physics.group_dynamic_state(group)[j];

				auto r = s1.position - s0.position;
				float l = geom::length(r);

				geom::vector<float, 1> f;
				f[0] = l - target;

				geom::matrix<float, 1, 4> J;
				J[0][0] = -r[0] / l;
				J[0][1] = -r[1] / l;
				J[0][2] = r[0] / l;
				J[0][3] = r[1] / l;

				geom::vector<float, 4> v;
				v[0] = d0.velocity[0];
				v[1] = d0.velocity[1];
				v[2] = d1.velocity[0];
				v[3] = d1.velocity[1];

				auto dv = geom::least_squares(J, -(1.f / 8.f) * f / dt - (J * v));
				if (dv)
				{
					d0.velocity[0] += (*dv)[0] * lambda;
					d0.velocity[1] += (*dv)[1] * lambda;
					d1.velocity[0] += (*dv)[2] * lambda;
					d1.velocity[1] += (*dv)[3] * lambda;
				}
			};

			std::size_t const n = 15;//physics.group_size(ball_group);

			if (physics.group_size(ball_group) >= n)
			for (std::size_t i = 0; i + 1 < n; ++i)
			{
//				solve_distance(ball_group, i, (i + 1) % n, 2.f * ball_radius);
			}

			if (false && physics.group_size(box_group) >= motor_id + 2)
			{
				solve_distance(box_group, motor_id, motor_id + 1, 5.75f * ball_radius);
				if (motor != 0.f)
				{
					physics.group_dynamic_state(box_group)[motor_id + 0].angular_velocity = motor;
					physics.group_dynamic_state(box_group)[motor_id + 1].angular_velocity = motor;
				}
			}
		};

		(void)apply_constraints;

		loop.pump();

		float const frame_dt = frame_clock.restart().count();
		physics_lag += frame_dt;

		static int frame_count = 0;

		float const physics_dt = 0.001f;
		float const max_physics_time = 0.01f;
		if (false)
		while (physics_lag > physics_dt)
		{
			physics_lag -= physics_dt;
			physics.update(physics_dt);
			for (int i = 0; i < 8; ++i)
				apply_constraints(physics_dt, 1.f);

			if (false && frame_clock.count() > max_physics_time)
			{
				log::warning() << "Can't keep up with physics, running " << physics_lag << "s behind";
				break;
			}
		}

		for (int i = 0; i < 16; ++i)
			physics.update(physics_dt);

		++frame_count;
		if (frame_count == 300)
			stop();

		if (!drag_delta)
		{
			selected_ball = std::nullopt;

			if (mouse)
			{
				float closest = std::numeric_limits<float>::infinity();

				for (std::size_t i = 0; i < physics.group_size(ball_group); ++i)
				{
					auto p = physics.group_static_state(ball_group)[i].position;
					float d = geom::distance(p, *mouse);
					if (d <= std::min(closest, ball_radius))
					{
						closest = d;
						selected_ball = i;
					}
				}
			}
		}
	}

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

		for (std::size_t i = 0; i < physics.group_size(ball_group); ++i)
		{
			bool selected = selected_ball ? (*selected_ball == i) : false;

			auto p = physics.group_static_state(ball_group)[i].position;
			auto a = physics.group_static_state(ball_group)[i].rotation;

			float r = std::get<phys2d::ball>(physics.shape(ball_group, i)).radius;

			painter.circle(p, r, gfx::black);
			painter.circle(p, r - line_width, gfx::light(gfx::blue, selected ? 0.8f : 1.f).as_color_rgba());

			geom::vector const n{std::cos(a), std::sin(a)};
			geom::vector const m = geom::ort(n);

			painter.line(p - n * ball_radius / 2.f, p + n * ball_radius / 2.f, line_width, gfx::black, false);
			painter.line(p - m * ball_radius / 2.f, p + m * ball_radius / 2.f, line_width, gfx::black, false);
		}

		for (std::size_t i = 0; i < physics.group_size(box_group); ++i)
		{
//			bool selected = false;

			auto p = physics.group_static_state(box_group)[i].position;
			auto a = physics.group_static_state(box_group)[i].rotation;

			float w = std::get<phys2d::box>(physics.shape(box_group, i)).width / 2.f;
			float h = std::get<phys2d::box>(physics.shape(box_group, i)).height / 2.f;

			geom::vector<float, 2> q[4];
			q[0] = {-w, -h};
			q[1] = { w, -h};
			q[2] = { w,  h};
			q[3] = {-w,  h};

			auto m = geom::plane_rotation<float, 2>(0, 1, a).linear_matrix();

			for (std::size_t j = 0; j < 4; ++j)
				q[j] = m * q[j];

			painter.triangle(p + q[0], p + q[1], p + q[3], gfx::white);
			painter.triangle(p + q[3], p + q[1], p + q[2], gfx::white);

			painter.line(p + q[0], p + q[1], line_width, gfx::black, false);
			painter.line(p + q[1], p + q[2], line_width, gfx::black, false);
			painter.line(p + q[2], p + q[3], line_width, gfx::black, false);
			painter.line(p + q[3], p + q[0], line_width, gfx::black, false);
		}

		painter.line(simulation_box.corner(0, 0), simulation_box.corner(1, 0), line_width, gfx::black, false);
		painter.line(simulation_box.corner(1, 0), simulation_box.corner(1, 1), line_width, gfx::black, false);
		painter.line(simulation_box.corner(1, 1), simulation_box.corner(0, 1), line_width, gfx::black, false);
		painter.line(simulation_box.corner(0, 1), simulation_box.corner(0, 0), line_width, gfx::black, false);

		painter.render(geom::orthographic_camera{view_box}.transform());
	}

};

int main()
{
	return app::main<physics_2d_app>();
}