#include <psemek/phys/engine_2d.hpp>

#include <psemek/util/heterogeneous_container.hpp>
#include <psemek/prof/profiler.hpp>

#include <psemek/geom/constants.hpp>
#include <psemek/geom/matrix.hpp>
#include <psemek/geom/gauss.hpp>
#include <psemek/geom/rotation.hpp>
#include <psemek/geom/math.hpp>
#include <psemek/geom/interval.hpp>

#include <psemek/log/log.hpp>

#include <optional>
#include <set>

namespace psemek::phys2d
{

	namespace
	{

		float shape_area(ball const & b)
		{
			return geom::pi * b.radius * b.radius;
		}

		float shape_inertia(ball const & b)
		{
			return shape_area(b) * b.radius * b.radius / 2.f;
		}

		float shape_area(half_space const &)
		{
			return std::numeric_limits<float>::infinity();
		}

		float shape_inertia(half_space const &)
		{
			return std::numeric_limits<float>::infinity();
		}

		float shape_area(box const & b)
		{
			return b.width * b.height;
		}

		float shape_inertia(box const & b)
		{
			return shape_area(b) * (b.width * b.width + b.height * b.height) / 12.f;
		}

		geom::point<float, 2> shape_closest(ball const & b, static_state const & s, geom::point<float, 2> const & p)
		{
			auto d = p - s.position;
			auto l = geom::length(d);
			if (l <= b.radius)
				return p;
			return s.position + (d / l) * b.radius;
		}

		geom::point<float, 2> shape_closest(half_space const & b, static_state const &, geom::point<float, 2> const & p)
		{
			auto v = geom::dot(b.normal, p - geom::point<float, 2>::zero()) - b.value;
			if (v <= 0.f)
				return p;
			return p - b.normal * v;
		}

		geom::point<float, 2> shape_closest(box const & b, static_state const & s, geom::point<float, 2> const & p)
		{
			geom::vector ex{std::cos(s.rotation), std::sin(s.rotation)};
			geom::vector ey{-std::sin(s.rotation), std::cos(s.rotation)};

			float w = b.width / 2.f;
			float h = b.height / 2.f;

			auto d = p - s.position;

			float x = geom::dot(d, ex);
			float y = geom::dot(d, ey);

			x = geom::clamp(x, {-w, w});
			y = geom::clamp(y, {-h, h});

			return s.position + ex * x + ey * y;
		}

		geom::box<float, 2> shape_bbox(ball const & b, static_state const & s)
		{
			return {{{s.position[0] - b.radius, s.position[0] + b.radius}, {s.position[1] - b.radius, s.position[1] + b.radius}}};
		}

		geom::box<float, 2> shape_bbox(box const & b, static_state const & s)
		{
			float cc = std::abs(std::cos(s.rotation));
			float ss = std::abs(std::sin(s.rotation));

			float w = b.width / 2.f;
			float h = b.height / 2.f;

			float tx = w * cc + h * ss;
			float ty = w * ss + h * cc;

			return {{{s.position[0] - tx, s.position[0] + tx}, {s.position[1] - ty, s.position[1] + ty}}};
		}

		geom::box<float, 2> shape_bbox(half_space const &, static_state const &)
		{
			return geom::box<float, 2>::full();
		}

		template <typename Shape>
		struct wrapped_shape
		{
			Shape shape;
			float area;
			float inertia;

			wrapped_shape(Shape const & shape)
				: shape{shape}
				, area(shape_area(shape))
				, inertia(shape_inertia(shape))
			{}
		};

		template <typename Shape>
		wrapped_shape<Shape> wrap(Shape const & shape)
		{
			return {shape};
		}

		// penetration points from object 1 to object 2
		struct collision
		{
			geom::point<float, 2> position;
			geom::vector<float, 2> penetration;
		};

		std::optional<collision> invert(std::optional<collision> c)
		{
			if (c)
				c->penetration *= -1.f;
			return c;
		}

		template <typename Points1, typename Normals1, typename Points2, typename Normals2>
		std::optional<collision> convex_collision(Points1 const & points1, Normals1 const & normals1, Points2 const & points2, Normals2 const & normals2)
		{
			static constexpr float inf = std::numeric_limits<float>::infinity();

			bool invert = false;
			geom::point<float, 2> point{0.f, 0.f};
			float penetration = -inf;
			geom::vector<float, 2> normal{0.f, 0.f};

			// Normals of 1st body against points of 2nd body
			{
				auto p = std::begin(points1);
				auto n = std::begin(normals1);

				for (; n != std::end(normals1); ++p, ++n)
				{
					float min = inf;
					geom::point<float, 2> minp{0.f, 0.f};
					for (auto const & p2 : points2)
					{
						float v = geom::dot(p2 - *p, *n);
						if (v < min)
						{
							min = v;
							minp = p2;
						}
					}

					if (min > 0.f)
						return std::nullopt;

					if (min > penetration)
					{
						point = minp;
						penetration = min;
						normal = *n;
					}
				}
			}

			// Normals of 2nd body against points of 1st body
			{
				auto p = std::begin(points2);
				auto n = std::begin(normals2);

				for (; n != std::end(normals2); ++p, ++n)
				{
					float min = inf;
					geom::point<float, 2> minp{0.f, 0.f};
					for (auto const & p1 : points1)
					{
						float v = geom::dot(p1 - *p, *n);
						if (v < min)
						{
							min = v;
							minp = p1;
						}
					}

					if (min > 0.f)
						return std::nullopt;

					if (min > penetration)
					{
						invert = true;
						point = minp;
						penetration = min;
						normal = *n;
					}
				}
			}

			return collision{point - normal * penetration / 2.f, (invert ? 1.f : -1.f) * normal * penetration};
		}

		std::optional<collision> shape_collision(ball const & b1, static_state const & s1, ball const & b2, static_state const & s2)
		{
			auto const d = s2.position - s1.position;
			float const l = geom::length(d);
			if (l > b1.radius + b2.radius)
				return std::nullopt;

			auto const p = geom::lerp(s1.position, s2.position, (b1.radius + l - b2.radius) / 2.f / l);
			return collision{p, d * (b1.radius + b2.radius - l) / l};
		}

		std::optional<collision> shape_collision(ball const & b1, static_state const & s1, half_space const & b2, static_state const &)
		{
			float v = b2.normal[0] * s1.position[0] + b2.normal[1] * s1.position[1] - b2.value - b1.radius;
			if (v > 0.f)
				return std::nullopt;

			return collision{s1.position - b2.normal * b1.radius, v * b2.normal};
		}

		std::optional<collision> shape_collision(ball const & b1, static_state const & s1, box const & b2, static_state const & s2)
		{
			geom::vector ex{std::cos(s2.rotation), std::sin(s2.rotation)};
			geom::vector ey{-std::sin(s2.rotation), std::cos(s2.rotation)};

			float w = b2.width / 2.f;
			float h = b2.height / 2.f;

			auto r = s1.position - s2.position;

			float x = geom::dot(r, ex);
			float y = geom::dot(r, ey);

			if (std::abs(x) < w && std::abs(y) < h)
			{
				// ball center is inside the box

				float vx = w - std::abs(x);
				float vy = h - std::abs(y);

				if (vx < vy)
				{
					if (x > 0.f)
					{
						return collision{s1.position, geom::vector{-vx - b1.radius, 0.f}};
					}
					else
					{
						return collision{s1.position, geom::vector{vx + b1.radius, 0.f}};
					}
				}
				else
				{
					if (y > 0.f)
					{
						return collision{s1.position, geom::vector{0.f, -vy - b1.radius}};
					}
					else
					{
						return collision{s1.position, geom::vector{0.f, vy + b1.radius}};
					}
				}
			}
			else
			{
				float cx = geom::clamp(x, {-w, w});
				float cy = geom::clamp(y, {-h, h});

				geom::vector n = ex * (cx - x) + ey * (cy - y);

				float l = geom::length(n);

				if (l < b1.radius)
				{
					float v = b1.radius - l;
					n = n / l;
					return collision{s1.position + n * (b1.radius - v / 2.f), n * v};
				}
			}


			return std::nullopt;
		}

		std::optional<collision> shape_collision(half_space const & b1, static_state const & s1, ball const & b2, static_state const & s2)
		{
			return invert(shape_collision(b2, s2, b1, s1));
		}

		std::optional<collision> shape_collision(half_space const &, static_state const &, half_space const &, static_state const &)
		{
			return std::nullopt;
		}

		std::optional<collision> shape_collision(half_space const & b1, static_state const &, box const & b2, static_state const & s2)
		{
			geom::vector a{ b2.width / 2.f, b2.height / 2.f};
			geom::vector b{-b2.width / 2.f, b2.height / 2.f};

			auto const rot = geom::plane_rotation<float, 2>(0, 1, s2.rotation);
			a = rot(a);
			b = rot(b);

			float v = b1.normal[0] * s2.position[0] + b1.normal[1] * s2.position[1] - b1.value;

			geom::vector<float, 2> d;

			float va = geom::dot(a, b1.normal);
			float vb = geom::dot(b, b1.normal);
			v -= std::max(std::abs(va), std::abs(vb));
			if (std::abs(va) > std::abs(vb))
			{
				if (va > 0.f)
					d = -a;
				else
					d = a;
			}
			else if (std::abs(vb) > std::abs(va))
			{
				if (vb > 0.f)
					d = -b;
				else
					d = b;
			}
			else
			{
				// multiple points of contact
				auto da = (va > 0.f) ? -a : a;
				auto db = (vb > 0.f) ? -b : b;
				d = da * 0.5f + db * 0.5f;
			}

			if (v > 0.f)
				return std::nullopt;
			return collision{s2.position + d, - v * b1.normal};
		}

		std::optional<collision> shape_collision(box const & b1, static_state const & s1, ball const & b2, static_state const & s2)
		{
			return invert(shape_collision(b2, s2, b1, s1));
		}

		std::optional<collision> shape_collision(box const & b1, static_state const & s1, half_space const & b2, static_state const & s2)
		{
			return invert(shape_collision(b2, s2, b1, s1));
		}

		std::optional<collision> shape_collision(box const & b1, static_state const & s1, box const & b2, static_state const & s2)
		{
			geom::vector<float, 2> ex1{ std::cos(s1.rotation), std::sin(s1.rotation)};
			geom::vector<float, 2> ey1{-std::sin(s1.rotation), std::cos(s1.rotation)};

			geom::vector<float, 2> ex2{ std::cos(s2.rotation), std::sin(s2.rotation)};
			geom::vector<float, 2> ey2{-std::sin(s2.rotation), std::cos(s2.rotation)};

			float w1 = b1.width  / 2.f;
			float h1 = b1.height / 2.f;
			float w2 = b2.width  / 2.f;
			float h2 = b2.height / 2.f;

			geom::point<float, 2> points1[4] =
			{
				s1.position - ex1 * w1 - ey1 * h1,
				s1.position + ex1 * w1 - ey1 * h1,
				s1.position + ex1 * w1 + ey1 * h1,
				s1.position - ex1 * w1 + ey1 * h1,
			};

			geom::vector<float, 2> normals1[4] =
			{
				-ey1,
				ex1,
				ey1,
				-ex1
			};

			geom::point<float, 2> points2[4] =
			{
				s2.position - ex2 * w2 - ey2 * h2,
				s2.position + ex2 * w2 - ey2 * h2,
				s2.position + ex2 * w2 + ey2 * h2,
				s2.position - ex2 * w2 + ey2 * h2,
			};

			geom::vector<float, 2> normals2[4] =
			{
				-ey2,
				ex2,
				ey2,
				-ex2
			};

			return convex_collision(points1, normals1, points2, normals2);
		}

		struct contact
		{
			std::size_t gi, i, gj, j;
			geom::vector<float, 2> ri, rj;
			geom::vector<float, 2> penetration;
			geom::vector<float, 2> normal;
			float penetration_depth;
			geom::vector<float, 2> velocity;
			float velocity_projection;
		};

		struct velocity_comparator
		{
			bool operator()(contact const & c1, contact const & c2) const
			{
				if (c1.velocity_projection == c2.velocity_projection)
					return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
				return c1.velocity_projection < c2.velocity_projection;
			}
		};

		struct penetration_comparator
		{
			bool operator()(contact const & c1, contact const & c2) const
			{
				if (c1.penetration_depth == c2.penetration_depth)
					return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
				return c1.penetration_depth > c2.penetration_depth;
			}
		};

	}

	struct engine::impl
	{
		util::heterogeneous_container<engine::shape_handle
			, wrapped_shape<ball>
			, wrapped_shape<half_space>
			, wrapped_shape<box>
//			, convex_polygon
			> shapes;

		util::flat_list<material, engine::material_handle> materials;

		struct constraint
		{
			std::function<void()> resolve;
			std::vector<std::pair<group_handle, std::size_t>> objects;
		};

		util::flat_list<constraint> constraints;
		std::vector<constraint_handle> constraint_handles;

		struct group
		{
			std::vector<object_info> infos;
			std::vector<std::vector<constraint_handle>> constraints;
			std::vector<static_state> static_states;
			std::vector<static_state> static_states_temp;
			std::vector<dynamic_state> dynamic_states;

			std::vector<std::vector<std::set<contact, velocity_comparator>::iterator>> contacts_by_velocity;
			std::vector<std::vector<std::set<contact, penetration_comparator>::iterator>> contacts_by_penetration;
		};

		std::vector<group> groups;

		geom::vector<float, 2> gravity{0.f, 0.f};
		float air_friction = 0.f;

		std::set<contact, velocity_comparator> contacts_by_velocity;
		std::set<contact, penetration_comparator> contacts_by_penetration;

		void explode(geom::point<float, 2> const & center, float strength, float attenuation);

		void update(float dt);

		void apply_gravity(float dt);
		void integrate(float dt);
		void resolve_constraints(float dt);
		void gather_collisions();
		void resolve_impulses();
		void resolve_penetrations();
		void apply_air_friction(float dt);

		void log_energy();
		float energy();
	};

	void engine::impl::explode(geom::point<float, 2> const & center, float strength, float attenuation)
	{
		for (auto & g : groups)
		{
			for (std::size_t i = 0; i < g.infos.size(); ++i)
			{
				if (g.infos[i].inv_mass == 0.f) continue;

				auto const closest = shapes.visit([&](auto const & s) -> geom::point<float, 2> { return shape_closest(s.shape, g.static_states[i], center); }, g.infos[i].shape);

				auto r = closest - center;
				float l = geom::length(r);

				auto J = (r / l) * strength / (1.f + l * l * attenuation);

				g.dynamic_states[i].velocity += J * g.infos[i].inv_mass;
				g.dynamic_states[i].angular_velocity += geom::det(closest - g.static_states[i].position, J) * g.infos[i].inv_inertia;
			}
		}
	}

	void engine::impl::update(float dt)
	{
		profile_function;

		apply_gravity(dt);
		integrate(dt);
		resolve_constraints(dt);
		gather_collisions();
		resolve_impulses();
		resolve_penetrations();
		apply_air_friction(dt);
	}

	void engine::impl::apply_gravity(float dt)
	{
		if (gravity != geom::vector{0.f, 0.f})
		{
			for (auto & g : groups)
			{
				for (std::size_t i = 0; i < g.infos.size(); ++i)
				{
					if (g.infos[i].inv_mass == 0.f) continue;

					g.dynamic_states[i].velocity += gravity * dt;
				}
			}
		}
	}

	void engine::impl::integrate(float dt)
	{
		for (auto & g : groups)
		{
			for (std::size_t i = 0; i < g.infos.size(); ++i)
			{
				g.static_states[i].position += dt * g.dynamic_states[i].velocity;
				g.static_states[i].rotation += dt * g.dynamic_states[i].angular_velocity;
			}
		}
	}

	void engine::impl::resolve_constraints(float dt)
	{
		for (std::size_t gi = 0; gi < groups.size(); ++gi)
			for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
				groups[gi].static_states_temp[i] = groups[gi].static_states[i];
		(void)dt;

		for (int i = 0; i < 4; ++i)
		{
			for (auto h : constraint_handles)
			{
				constraints[h].resolve();
			}
		}

		for (std::size_t gi = 0; gi < groups.size(); ++gi)
		{
			for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
			{
				groups[gi].dynamic_states[i].velocity += (groups[gi].static_states[i].position - groups[gi].static_states_temp[i].position) / dt;
				groups[gi].dynamic_states[i].angular_velocity += (groups[gi].static_states[i].rotation - groups[gi].static_states_temp[i].rotation) / dt;
			}
		}
	}

	void engine::impl::gather_collisions()
	{
		profile_function;

		for (std::size_t gi = 0; gi < groups.size(); ++gi)
		{
			groups[gi].contacts_by_velocity.resize(groups[gi].infos.size());
			groups[gi].contacts_by_penetration.resize(groups[gi].infos.size());
			for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
			{
				groups[gi].contacts_by_velocity[i].clear();
				groups[gi].contacts_by_penetration[i].clear();
			}
		}

		contacts_by_velocity.clear();
		contacts_by_penetration.clear();

		auto test_objects = [&](std::size_t gi, std::size_t i, std::size_t gj, std::size_t j)
		{
			auto shi = groups[gi].infos[i].shape;
			auto shj = groups[gj].infos[j].shape;

			auto & sti = groups[gi].static_states[i];
			auto & stj = groups[gj].static_states[j];

			auto c = shapes.visit([&](auto const & si, auto const & sj){
				return shape_collision(si.shape, sti, sj.shape, stj);
			}, shi, shj);

			if (!c) return;

			if (c->penetration == geom::vector<float, 2>::zero()) return;

			auto ri = c->position - sti.position;
			auto rj = c->position - stj.position;

			auto & dsi = groups[gi].dynamic_states[i];
			auto & dsj = groups[gj].dynamic_states[j];

			auto ui = dsi.velocity + geom::ort(ri) * dsi.angular_velocity;
			auto uj = dsj.velocity + geom::ort(rj) * dsj.angular_velocity;

			auto const u = uj - ui;

			contact ct;
			ct.gi = gi;
			ct.gj = gj;
			ct.i = i;
			ct.j = j;
			ct.ri = ri;
			ct.rj = rj;
			ct.penetration = c->penetration;
			ct.normal = geom::normalized(ct.penetration);
			ct.penetration_depth = geom::length(ct.penetration);
			ct.velocity = u;
			ct.velocity_projection = geom::dot(u, ct.normal);

			auto res = contacts_by_velocity.insert(ct);
			assert(res.second);
			groups[gi].contacts_by_velocity[i].push_back(res.first);
			groups[gj].contacts_by_velocity[j].push_back(res.first);
		};

		struct bbox_data
		{
			geom::box<float, 2> box;
			std::size_t g, i;
		};

		std::vector<bbox_data> bboxes;
		for (std::size_t gi = 0; gi < groups.size(); ++gi)
		{
			for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
			{
				bbox_data data;
				data.box = shapes.visit([&](auto const & s){ return shape_bbox(s.shape, groups[gi].static_states[i]); }, groups[gi].infos[i].shape);
				data.g = gi;
				data.i = i;
				bboxes.push_back(data);
			}
		}

		std::vector<std::size_t> min, max;
		min.reserve(bboxes.size());
		max.reserve(bboxes.size());

		for (std::size_t i = 0; i < bboxes.size(); ++i)
		{
			min.push_back(i);
			max.push_back(i);
		}

		std::sort(min.begin(), min.end(), [&](auto i, auto j){ return bboxes[i].box[0].min < bboxes[j].box[0].min; });
		std::sort(max.begin(), max.end(), [&](auto i, auto j){ return bboxes[i].box[0].max < bboxes[j].box[0].max; });

		std::set<std::size_t> current;

		for (std::size_t mini = 0, maxi = 0; mini < bboxes.size();)
		{
			if (bboxes[min[mini]].box[0].min < bboxes[max[maxi]].box[0].max)
			{
				std::size_t i = min[mini];
				for (auto j : current)
				{
					if (!(bboxes[i].box[1] & bboxes[j].box[1]).empty())
						test_objects(bboxes[i].g, bboxes[i].i, bboxes[j].g, bboxes[j].i);
				}
				current.insert(i);

				++mini;
			}
			else
			{
				current.erase(max[maxi]);
				++maxi;
			}
		}
	}

	void engine::impl::resolve_impulses()
	{
		profile_function;

		std::size_t const iterations = contacts_by_velocity.size() * 2;

		for (std::size_t iteration = 0; iteration < iterations; ++iteration)
		{
			contact const & c = *contacts_by_velocity.begin();

			if (c.velocity_projection > 0.f)
			{
				break;
			}

			auto const & infoi = groups[c.gi].infos[c.i];
			auto const & infoj = groups[c.gj].infos[c.j];

			auto const & mati = materials[infoi.material];
			auto const & matj = materials[infoj.material];

			geom::matrix<float, 2, 2> K;
			K[0][0] = infoi.inv_mass + infoj.inv_mass;
			K[1][1] = K[0][0];
			K[0][1] = 0.f;
			K[1][0] = 0.f;

			K[0][0] += c.ri[1] * c.ri[1] * infoi.inv_inertia;
			K[1][1] += c.ri[0] * c.ri[0] * infoi.inv_inertia;
			K[0][1] -= c.ri[0] * c.ri[1] * infoi.inv_inertia;

			K[0][0] += c.rj[1] * c.rj[1] * infoj.inv_inertia;
			K[1][1] += c.rj[0] * c.rj[0] * infoj.inv_inertia;
			K[0][1] -= c.rj[0] * c.rj[1] * infoj.inv_inertia;

			K[1][0] = K[0][1];

			auto const & n = c.normal;

			// Plastic sliding impulse
			// Normal relative velocity -> 0
			// Tangential relative velocity -> unchanged
			auto const J1 = - n * c.velocity_projection / geom::dot(n, K * n);
			// Plastic sticking impulse
			// Normal relative velocity -> 0
			// Tangential relative velocity -> 0
			auto const J2 = - *geom::solve(K, c.velocity);

			float const e = std::sqrt(mati.elasticity * matj.elasticity);
			float const f = std::sqrt(mati.friction * matj.friction);
			float const mu = f;

			auto J = (1.f + e) * J1 + f * (J2 - J1);

			// Test for friction cone
			float q = geom::length(J - n * geom::dot(J, n));
			if (q > mu * geom::dot(J, n))
			{
				float k = (mu * (1.f + e) * geom::dot(J1, n)) / (geom::length(J2 - n * geom::dot(J2, n)) - mu * geom::dot(n, J2 - J1));
				J = (1.f + e) * J1 + k * (J2 - J1);
			}

			auto dvi = -J * infoi.inv_mass;
			auto dvj =  J * infoj.inv_mass;

			auto dai = -geom::det(c.ri, J) * infoi.inv_inertia;
			auto daj =  geom::det(c.rj, J) * infoj.inv_inertia;

			groups[c.gi].dynamic_states[c.i].velocity += dvi;
			groups[c.gj].dynamic_states[c.j].velocity += dvj;

			groups[c.gi].dynamic_states[c.i].angular_velocity += dai;
			groups[c.gj].dynamic_states[c.j].angular_velocity += daj;

			auto ct = contacts_by_velocity.begin();

			std::size_t cti = -1, ctj = -1;

			auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & dv, float da)
			{
				contact const & c = *old_it;

				std::size_t OG, OI;
				bool flip;

				if (c.gi == G && c.i == I)
				{
					OG = c.gj;
					OI = c.j;
					flip = false;
				}
				else
				{
					OG = c.gi;
					OI = c.i;
					flip = true;
				}

				std::size_t oi;
				for (oi = 0; oi < groups[OG].contacts_by_velocity[OI].size(); ++oi)
				{
					if (groups[OG].contacts_by_velocity[OI][oi] == old_it)
						break;
				}

				auto node = contacts_by_velocity.extract(old_it);
				contact & cc = node.value();
				cc.velocity += (flip ? 1.f : -1.f) * (dv + geom::ort(flip ? cc.rj : cc.ri) * da);
				cc.velocity_projection = geom::dot(cc.velocity, cc.normal);

				auto res = contacts_by_velocity.insert(std::move(node));
				assert(res.inserted);
				auto new_it = res.position;

				groups[G].contacts_by_velocity[I][i] = new_it;
				groups[OG].contacts_by_velocity[OI][oi] = new_it;
			};

			for (std::size_t i = 0; i < groups[c.gi].contacts_by_velocity[c.i].size(); ++i)
			{
				auto old_it = groups[c.gi].contacts_by_velocity[c.i][i];
				if (old_it == ct)
				{
					cti = i;
					continue;
				}

				update_contact(old_it, c.gi, c.i, i, dvi, dai);
			}

			for (std::size_t i = 0; i < groups[c.gj].contacts_by_velocity[c.j].size(); ++i)
			{
				auto old_it = groups[c.gj].contacts_by_velocity[c.j][i];
				if (old_it == ct)
				{
					ctj = i;
					continue;
				}

				update_contact(old_it, c.gj, c.j, i, dvj, daj);
			}

			{
				auto node = contacts_by_velocity.extract(ct);
				contact & cc = node.value();
				cc.velocity -= dvi + geom::ort(cc.ri) * dai;
				cc.velocity += dvj + geom::ort(cc.rj) * daj;
				cc.velocity_projection = geom::dot(cc.velocity, cc.normal);

				auto res = contacts_by_velocity.insert(std::move(node));
				assert(res.inserted);
				auto new_it = res.position;

				auto & ccc = *new_it;

				groups[ccc.gi].contacts_by_velocity[ccc.i][cti] = new_it;
				groups[ccc.gj].contacts_by_velocity[ccc.j][ctj] = new_it;
			}
		}
	}

	void engine::impl::resolve_penetrations()
	{
		profile_function;

		for (auto it = contacts_by_velocity.begin(); it != contacts_by_velocity.end();)
		{
			auto jt = it++;
			auto node = contacts_by_velocity.extract(jt);
			auto res = contacts_by_penetration.insert(std::move(node));
			assert(res.inserted);
			auto new_it = res.position;
			contact const & c = *new_it;
			groups[c.gi].contacts_by_penetration[c.i].push_back(new_it);
			groups[c.gj].contacts_by_penetration[c.j].push_back(new_it);
		}

		std::size_t const iterations = contacts_by_penetration.size() * 2;

		for (std::size_t iteration = 0; iteration < iterations; ++iteration)
		{
			contact const & c = *contacts_by_penetration.begin();

			if (c.penetration_depth < 0.f)
				break;

			auto const & infoi = groups[c.gi].infos[c.i];
			auto const & infoj = groups[c.gj].infos[c.j];

			auto di = - c.penetration * infoi.inv_mass / (infoi.inv_mass + infoj.inv_mass);
			auto dj =   c.penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass);

			groups[c.gi].static_states[c.i].position += di;
			groups[c.gj].static_states[c.j].position += dj;

			auto ct = contacts_by_penetration.begin();

			std::size_t cti = -1, ctj = -1;

			auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & d)
			{
				contact const & c = *old_it;

				std::size_t OG, OI;
				bool flip;

				if (c.gi == G && c.i == I)
				{
					OG = c.gj;
					OI = c.j;
					flip = false;
				}
				else
				{
					OG = c.gi;
					OI = c.i;
					flip = true;
				}

				std::size_t oi;
				for (oi = 0; oi < groups[OG].contacts_by_penetration[OI].size(); ++oi)
				{
					if (groups[OG].contacts_by_penetration[OI][oi] == old_it)
						break;
				}

				auto node = contacts_by_penetration.extract(old_it);
				contact & cc = node.value();
				cc.penetration += (flip ? -1.f : 1.f) * d;
				cc.penetration_depth = geom::dot(cc.penetration, cc.normal);

				auto res = contacts_by_penetration.insert(std::move(node));
				assert(res.inserted);
				auto new_it = res.position;

				groups[G].contacts_by_penetration[I][i] = new_it;
				groups[OG].contacts_by_penetration[OI][oi] = new_it;
			};

			for (std::size_t i = 0; i < groups[c.gi].contacts_by_penetration[c.i].size(); ++i)
			{
				auto old_it = groups[c.gi].contacts_by_penetration[c.i][i];
				if (old_it == ct)
				{
					cti = i;
					continue;
				}

				update_contact(old_it, c.gi, c.i, i, di);
			}

			for (std::size_t i = 0; i < groups[c.gj].contacts_by_penetration[c.j].size(); ++i)
			{
				auto old_it = groups[c.gj].contacts_by_penetration[c.j][i];
				if (old_it == ct)
				{
					ctj = i;
					continue;
				}

				update_contact(old_it, c.gj, c.j, i, dj);
			}

			{
				auto node = contacts_by_penetration.extract(ct);
				contact & cc = node.value();
				cc.penetration += di;
				cc.penetration -= dj;
				cc.penetration_depth = geom::dot(cc.penetration, cc.normal);

				auto res = contacts_by_penetration.insert(std::move(node));
				assert(res.inserted);
				auto new_it = res.position;

				auto & ccc = *new_it;

				groups[ccc.gi].contacts_by_penetration[ccc.i][cti] = new_it;
				groups[ccc.gj].contacts_by_penetration[ccc.j][ctj] = new_it;
			}
		}
	}

	void engine::impl::apply_air_friction(float dt)
	{
		if (air_friction != 0.f)
		{
			float const factor = 1.f - std::min(1.f, air_friction * dt);

			for (auto & g : groups)
			{
				for (std::size_t i = 0; i < g.infos.size(); ++i)
				{
					if (g.infos[i].inv_mass == 0.f) continue;

					g.dynamic_states[i].velocity *= factor;
					g.dynamic_states[i].angular_velocity *= factor;
				}
			}
		}
	}

	float engine::impl::energy()
	{
		float Eg = 0.f;
		float Er = 0.f;
		float Ek = 0.f;

		for (auto & g : groups)
		{
			for (std::size_t i = 0; i < g.infos.size(); ++i)
			{
				if (g.infos[i].inv_mass == 0.f) continue;

				Eg -= geom::dot(gravity, g.static_states[i].position - geom::point<float, 2>::zero()) / g.infos[i].inv_mass;

				Ek += geom::length_sqr(g.dynamic_states[i].velocity) / g.infos[i].inv_mass / 2.f;
				Er += geom::sqr(g.dynamic_states[i].angular_velocity) / g.infos[i].inv_inertia / 2.f;
			}
		}

		return Eg + Ek + Er;
	}

	engine::engine()
		: pimpl_{make_impl()}
	{}

	engine::~engine() = default;

	engine::shape_handle engine::add_shape(ball const & b)
	{
		return impl().shapes.insert(wrap(b));
	}

	engine::shape_handle engine::add_shape(half_space const & s)
	{
		return impl().shapes.insert(wrap(s));
	}

	engine::shape_handle engine::add_shape(box const & b)
	{
		return impl().shapes.insert(wrap(b));
	}

	std::variant<ball const *, half_space const *, box const *> engine::get_shape(shape_handle handle) const
	{
		using result_type = std::variant<ball const *, half_space const *, box const *>;
		return impl().shapes.visit([](auto const & w) -> result_type {
			return &(w.shape);
		}, handle);
	}

	void engine::remove_shape(shape_handle handle)
	{
		impl().shapes.erase(handle);
	}

	engine::material_handle engine::add_material(material const & m)
	{
		return impl().materials.insert(m);
	}

	material const & engine::get_material(material_handle handle) const
	{
		return impl().materials[handle];
	}

	material & engine::get_material(material_handle handle)
	{
		return impl().materials[handle];
	}

	void engine::remove_material(material_handle handle)
	{
		impl().materials.erase(handle);
	}

	engine::group_handle engine::create_group()
	{
		auto h = static_cast<group_handle>(impl().groups.size());
		impl().groups.emplace_back();
		return h;
	}

	void engine::remove_group(group_handle handle)
	{
		auto & g = impl().groups[handle];
		g.infos.clear();
		g.static_states.clear();
		g.dynamic_states.clear();
	}

	std::size_t engine::group_size(group_handle handle) const
	{
		return impl().groups[handle].infos.size();
	}

	static_state const * engine::group_static_state(group_handle handle) const
	{
		return impl().groups[handle].static_states.data();
	}

	dynamic_state const * engine::group_dynamic_state(group_handle handle) const
	{
		return impl().groups[handle].dynamic_states.data();
	}

	static_state * engine::group_static_state(group_handle handle)
	{
		return impl().groups[handle].static_states.data();
	}

	dynamic_state * engine::group_dynamic_state(group_handle handle)
	{
		return impl().groups[handle].dynamic_states.data();
	}

	std::size_t engine::add_object(group_handle group, shape_handle shape, material_handle material, static_state const & ss, dynamic_state const & ds)
	{
		float const density = impl().materials[material].density;
		float const area = impl().shapes.visit([](auto const & s){ return s.area; }, shape);
		float const inertia = impl().shapes.visit([](auto const & s){ return s.inertia; }, shape);

		auto & g = impl().groups[group];
		g.infos.push_back({shape, material, 1.f / (area * density), 1.f / (inertia * density)});
		g.constraints.emplace_back();
		g.static_states.push_back(ss);
		g.static_states_temp.emplace_back();
		g.dynamic_states.push_back(ds);

		return g.infos.size() - 1;
	}

	void engine::remove_object(group_handle group, std::size_t index)
	{
		auto & g = impl().groups[group];

		for (auto h : g.constraints[index])
			remove_constraint(h);

		g.infos.erase(g.infos.begin() + index);
		g.constraints.erase(g.constraints.begin() + index);
		g.static_states.erase(g.static_states.begin() + index);
		g.static_states_temp.erase(g.static_states_temp.begin() + index);
		g.dynamic_states.erase(g.dynamic_states.begin() + index);
	}

	engine::object_info const & engine::get_object(group_handle group, std::size_t index)
	{
		return impl().groups[group].infos[index];
	}

	engine::constraint_handle engine::add_constraint(group_handle g, std::size_t index, fixed_constraint const & c)
	{
		impl::constraint constraint;
		constraint.objects.push_back({g, index});
		constraint.resolve = [=, this]{
			auto & ss = impl().groups[g].static_states[index];

			geom::vector<float, 2> t;
			t[0] = c.target[0] - ss.position[0] - c.point[0] * std::cos(ss.rotation) + c.point[1] * std::sin(ss.rotation);
			t[1] = c.target[1] - ss.position[1] - c.point[0] * std::sin(ss.rotation) - c.point[1] * std::cos(ss.rotation);

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

			auto delta = geom::least_squares(J, -t);

			if (delta)
			{
				ss.position[0] += (*delta)[0];
				ss.position[1] += (*delta)[1];
				ss.rotation += (*delta)[2];
			}
		};

		auto handle = impl().constraints.insert(std::move(constraint));
		impl().constraint_handles.push_back(handle);
		impl().groups[g].constraints[index].push_back(handle);
		return handle;
	}

	engine::constraint_handle engine::add_constraint(group_handle g1, std::size_t index1, group_handle g2, std::size_t index2, joint_constraint const & c)
	{
		impl::constraint constraint;
		constraint.objects.push_back({g1, index1});
		constraint.objects.push_back({g2, index2});
		constraint.resolve = [=, this]{
			auto & ss1 = impl().groups[g1].static_states[index1];
			auto & ss2 = impl().groups[g2].static_states[index2];

			geom::vector<float, 2> t;
			t[0] = ss2.position[0] + c.point2[0] * std::cos(ss2.rotation) - c.point2[1] * std::sin(ss2.rotation) - ss1.position[0] - c.point1[0] * std::cos(ss1.rotation) + c.point1[1] * std::sin(ss1.rotation);
			t[1] = ss2.position[1] + c.point2[0] * std::sin(ss2.rotation) + c.point2[1] * std::cos(ss2.rotation) - ss1.position[1] - c.point1[0] * std::sin(ss1.rotation) - c.point1[1] * std::cos(ss1.rotation);

			geom::matrix<float, 2, 6> J;
			J[0][0] = -1.f;
			J[0][1] = 0.f;
			J[0][2] = c.point1[0] * std::sin(ss1.rotation) + c.point1[1] * std::cos(ss1.rotation);
			J[0][3] = 1.f;
			J[0][4] = 0.f;
			J[0][5] = - c.point2[0] * std::sin(ss2.rotation) - c.point2[1] * std::cos(ss2.rotation);
			J[1][0] = 0.f;
			J[1][1] = -1.f;
			J[1][2] = - c.point1[0] * std::cos(ss1.rotation) + c.point1[1] * std::sin(ss1.rotation);
			J[1][3] = 0.f;
			J[1][4] = 1.f;
			J[1][5] = c.point2[0] * std::cos(ss2.rotation) - c.point2[1] * std::sin(ss2.rotation);

			auto delta = geom::least_squares(J, -t);

			if (delta)
			{
				ss1.position[0] += (*delta)[0];
				ss1.position[1] += (*delta)[1];
				ss1.rotation += (*delta)[2];
				ss2.position[0] += (*delta)[3];
				ss2.position[1] += (*delta)[4];
				ss2.rotation += (*delta)[5];
			}
		};

		auto handle = impl().constraints.insert(std::move(constraint));
		impl().constraint_handles.push_back(handle);
		impl().groups[g1].constraints[index1].push_back(handle);
		impl().groups[g2].constraints[index2].push_back(handle);
		return handle;
	}

	void engine::remove_constraint(constraint_handle handle)
	{
		auto const & constraint = impl().constraints[handle];
		for (auto [g, i] : constraint.objects)
		{
			auto & cs = impl().groups[g].constraints[i];
			if (auto it = std::find(cs.begin(), cs.end(), handle); it != cs.end())
				cs.erase(it);
		}
		impl().constraints.erase(handle);
		impl().constraint_handles.erase(std::find(impl().constraint_handles.begin(), impl().constraint_handles.end(), handle));
	}

	void engine::set_gravity(geom::vector<float, 2> const & g)
	{
		impl().gravity = g;
	}

	void engine::set_air_friction(float friction)
	{
		impl().air_friction = friction;
	}

	void engine::explode(geom::point<float, 2> const & center, float strength, float attenuation)
	{
		impl().explode(center, strength, attenuation);
	}

	void engine::update(float dt)
	{
		impl().update(dt);
	}

}