#include <psemek/app/application_base.hpp>
#include <psemek/app/default_application_factory.hpp>
#include <psemek/gfx/painter.hpp>
#include <psemek/gfx/gl.hpp>
#include <psemek/math/scale.hpp>
#include <psemek/math/camera.hpp>
#include <psemek/math/constants.hpp>
#include <psemek/util/clock.hpp>
#include <psemek/util/to_string.hpp>
#include <psemek/prof/profiler.hpp>
#include <psemek/util/moving_average.hpp>
#include <psemek/log/log.hpp>

#include <random>
#include <set>

/*
No optimizations, 125:
[2020 Aug 29  09:54:27.348][ main][   info] Avg forces time:    0.000641577
[2020 Aug 29  09:54:27.348][ main][   info] Avg collision time: 0.000789326

No optimizations, 250:
[2020 Aug 29  09:49:48.476][ main][   info] Avg forces time:    0.00257929
[2020 Aug 29  09:49:48.476][ main][   info] Avg collision time: 0.00314664

No optimizations, 500:
[2020 Aug 29  09:44:02.200][ main][   info] Avg forces time:    0.00299429
[2020 Aug 29  09:44:02.200][ main][   info] Avg collision time: 0.00362124

No optimizations, 1000:
[2020 Aug 29  09:42:34.143][ main][   info] Avg forces time:    0.0072538
[2020 Aug 29  09:42:34.143][ main][   info] Avg collision time: 0.00873274

No optimizations, 2000:
[2020 Aug 29  09:44:35.334][ main][   info] Avg forces time:    0.029108
[2020 Aug 29  09:44:35.334][ main][   info] Avg collision time: 0.0345713

No optimizations, 4000:
[2020 Aug 29  09:50:15.647][ main][   info] Avg forces time:    0.118145
[2020 Aug 29  09:50:15.647][ main][   info] Avg collision time: 0.1402





*/

using namespace psemek;

struct particle
{
	math::point<float, 2> pos;
	math::vector<float, 2> vel;

	float angle;
	float angle_vel;

	float radius;
	float mass;
	float density;

	math::vector<float, 2> delta_pos{0.f, 0.f};
	math::vector<float, 2> delta_vel{0.f, 0.f};

	math::point<float, 2> old_pos{0.f, 0.f};

	math::vector<float, 2> acc{0.f, 0.f};

	float T = 0.f;
};

float const G = 50.f;
float const GG = 0*1000.f;
float const GC = 0*100000.f;
float const K = 10000.f;
float const FR = 0.99f;

float const dt = 0.01f;

float const world_size = 10000000.f;
math::point world_center{0.f, 0.f};

int const SOLVE_ITERATIONS = 16;
float const BIAS = 1.f / 8.f;

struct myapp
	: app::application_base
{
	myapp(options const &, context const & context)
		: rng_{std::random_device{}()}
	{
		gl::ClearColor(1.f, 1.f, 1.f, 1.f);

		context.vsync(true);

		std::uniform_real_distribution<float> d{-50.f, 50.f};
		std::uniform_real_distribution<float> rr{0.5f, 2.f};
		std::uniform_real_distribution<float> rden{0.25f, 1.f};
		std::uniform_real_distribution<float> ra{0.f, 2.f * math::pi};

		float min_R = 0.f;
		float max_R = 100.f;
		std::uniform_real_distribution<float> rR{0.f, 1.f};

		bool star = false;

		if (star)
			particles_.push_back({{0.f, 0.f}, {0.f, 0.f}, 0.f, 0.f, 10.f, math::pi * 10000.f, 1.f});

//		particles_.push_back({{-2.f, 0.f}, {0.f,  0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
//		particles_.push_back({{ 2.f, 0.f}, {0.f,  0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
//		particles_.push_back({{ 0.f, 3.f}, {0.f,  0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
//		particles_.push_back({{ 0.f, -3.f}, {0.f,  0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});

//		float planet_R[] = {200.f, 300.f, 400.f};
//		float planet_a[] = {0.f, math::rad(120.f), math::rad(240.f)};

//		if(false)
		for (int i = 0; i < 500; ++i)
		{
			math::vector v{0.f, 0.f};

			math::point<float, 2> p;
			float m;
			float r;
			float den;
			while (true)
			{
//				p = {((i % 2) ? -200.f : 200.f) + d(rng_), d(rng_)};

				r = rr(rng_);
				den = rden(rng_);
				m = math::pi * r * r * den;

				auto a = ra(rng_);
				float R = std::sqrt(rR(rng_)) * (max_R - min_R) + min_R;
//				if (std::uniform_int_distribution<int>(0, 1)(rng_) == 0)
//					R += 200.f;

//				auto pl = std::uniform_int_distribution<int>(0, std::size(planet_R) - 1)(rng_);
//				a = planet_a[pl];
//				R = planet_R[pl];

//				R += std::uniform_real_distribution<float>(-10.f, 10.f)(rng_);
//				a += math::rad(std::uniform_real_distribution<float>(-2.f, 2.f)(rng_));

				p =
				{
					R * std::cos(a),
					R * std::sin(a),
				};
//				p += math::vector{((i % 2) ? -1000.f : 1000.f), 0.f};

//				v[0] = {(i % 2) ? 100.f : -100.f};

				if (std::all_of(particles_.begin(), particles_.end(), [&](particle const & q){ return math::distance(q.pos, p) > q.radius + r; }))
					break;
			}
			particles_.push_back({p, v, 0.f, 0.f, r, m, den});
		}

		float total_M = 0.f;
		if (star)
			total_M += particles_[0].mass;
		for (std::size_t i = star ? 1 : 0; i < particles_.size(); ++i)
			total_M += particles_[i].mass / 2.f;

		if(false)
		for (std::size_t i = star ? 1 : 0; i < particles_.size(); ++i)
		{
			auto r = particles_[i].pos - math::point{0.f, 0.f};
			auto R = math::length(r);
			float V = std::sqrt(G * total_M / R);
			particles_[i].vel = math::ort(r / R) * V;
			(void)V;
		}

		for (auto & p : particles_)
			p.old_pos = p.pos;
	}

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

		gl::Viewport(0, 0, event.size[0], event.size[1]);

		camera_ratio_ = static_cast<float>(event.size[0]) / event.size[1];
	}

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

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

	void on_event(app::mouse_button_event const & event) override
	{
		if (event.button == app::mouse_button::left && event.down)
		{
			math::scale<float, 2> const flip_y({1.f, -1.f});

			float const scale = camera_size_ / state().size[1];

			math::point<int, 2> const screen_center { state().size[0] / 2,state().size[1] / 2 };
			auto target = camera_center_ + flip_y(math::cast<float>(state().mouse - screen_center) * scale);
			force_target_ = target;

//			math::point<float, 2> pos{100.f, 0.f};

//			math::vector<float, 2> vel = math::normalized(target - pos) * 40.f;

//			particles_.push_back({pos, vel, 1.f, 1.f});

//			for (auto & p : particles_)
//			{
//				auto r = p.pos - target;
//				p.vel += 4000.f * r / math::length_sqr(r) / p.mass;
//			}
		}

		if (event.button == app::mouse_button::left && !event.down)
		{
			force_target_ = std::nullopt;
		}

		if (event.button == app::mouse_button::right && event.down)
		{
			camera_drag_ = state().mouse;
		}

		if (event.button == app::mouse_button::right && !event.down)
		{
			camera_drag_ = std::nullopt;
		}
	}

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

		if (camera_drag_)
		{
			math::scale<float, 2> const flip_y({1.f, -1.f});

			float const scale = camera_size_ / state().size[1];

			camera_center_ += flip_y(math::cast<float>(*camera_drag_ - event.position) * scale);
			camera_drag_ = event.position;
		}

		if (force_target_)
		{
			math::scale<float, 2> const flip_y({1.f, -1.f});
			float const scale = camera_size_ / state().size[1];
			math::point<int, 2> const screen_center { state().size[0] / 2, state().size[1] / 2 };
			auto target = camera_center_ + flip_y(math::cast<float>(state().mouse - screen_center) * scale);
			force_target_ = target;
		}
	}

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

		if (event.key == app::keycode::SPACE && event.down)
		{
			paused_ = !paused_;
		}

		if (event.key == app::keycode::C && event.down)
		{
			particles_.push_back({{200.f, 0.f}, {-5000.f, 0.f}, 0.f, 0.f, 1.f, 100.f, 1.f});
			particles_.back().old_pos = particles_.back().pos;
		}
	}

	void update() override
	{
		if (paused_)
			return;

		for (std::size_t step = 0; step < 1; ++step)
		{
			{
				util::clock<> clock;

				for (auto & p : particles_)
					p.acc = {0.f, 0.f};

				for (auto & p : particles_)
					p.acc += math::vector{0.f, -GG};

				for (auto & p : particles_)
				{
					auto r = (p.pos - p.pos.zero());
					p.acc -= GC * r / std::pow(1.f + math::length(r), 3.f);
				}

//				for (std::size_t i = 0; i < particles_.size(); ++i)
//				{
//					log::info() << "Start: #" << i << " pos = " << std::setprecision(10) << particles_[i].pos << ", vel = " << particles_[i].vel
//								<< ", |vel| = " << math::length(particles_[i].vel);
//				}

				for (std::size_t i = 0; i < particles_.size(); ++i)
				{
					for (std::size_t j = i + 1; j < particles_.size(); ++j)
					{
						auto const r = particles_[i].pos - particles_[j].pos;

//						float const l = std::max(particles_[i].radius + particles_[j].radius, length(r));
						float const l = length(r);

						auto const f = G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);

//						log::info() << "Force: #" << i << "," << j << " = " << std::setprecision(10) << f;

						particles_[i].acc -= f / particles_[i].mass;
						particles_[j].acc += f / particles_[j].mass;
					}
				}

				// Force-based collisions
//				if (false)
				for (std::size_t i = 0; i < particles_.size(); ++i)
				{
					for (std::size_t j = i + 1; j < particles_.size(); ++j)
					{
						auto const r = particles_[i].pos - particles_[j].pos;
						float const l = length(r);

						float R = particles_[i].radius + particles_[j].radius;
						auto n = r / l;

						if (l < R)
						{
							auto f = K * n * (R - l);
							(void)n;

//							auto const f = -2.f * G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);
//							f -= 2.f * G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);

							particles_[i].acc += f / particles_[i].mass;
							particles_[j].acc -= f / particles_[j].mass;
						}

//						auto vij = particles_[i].vel - particles_[j].vel;

//						auto vn = math::dot(vij, n) * n;

//						particles_[i].acc -= vn * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
//						particles_[j].acc += vn * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
					}
				}

				total_forces_ += clock.count();
			}

//			for (std::size_t i = 0; i < particles_.size(); ++i)
//			{
//				log::info() << "Force: #" << i << " = " << std::setprecision(10) << particles_[i].acc;
//			}

			if (state().key_down.contains(app::keycode::M))
			{
				for (auto & p : particles_)
					p.vel *= std::exp(-100.f*dt);
			}

			if (force_target_ && false)
			{
				for (auto & p : particles_)
				{
					auto r = p.pos - *force_target_;
					p.acc += 100000.f * r / math::length_sqr(r) / p.mass;
				}
			}

//			for (auto & p : particles_)
//			{
//				auto old = p.pos;
//				p.pos += (p.pos - p.old_pos) + p.acc * dt * dt;
//				p.old_pos = old;
//			}

//			for (std::size_t i = 0; i < particles_.size(); ++i)
//			{
//				log::info() << "Integrate: #" << i << " new pos = " << std::setprecision(10) << particles_[i].pos;
//			}

			if (false)
			{
				float s = 100.f;
				world_center[0] += dt * std::uniform_real_distribution<float>(-s, s)(rng_);
				world_center[1] += dt * std::uniform_real_distribution<float>(-s, s)(rng_);
			}

			if (force_target_)
				world_center = *force_target_;

//			if (false)
			for (auto & p : particles_)
			{
				p.vel += p.acc * dt;
				p.vel *= FR;
				p.pos += p.vel * dt;
			}

			// Verlet
			if (false)
			for (auto & p : particles_)
			{
				auto old = p.pos;
				p.pos += (p.pos - p.old_pos) + p.acc * dt * dt;
				p.old_pos = old;
			}

			// New iterative algorithm
			if (false)
			{
				std::vector<math::point<float, 2>> old_pos(particles_.size());
				for (std::size_t i = 0; i < particles_.size(); ++i)
					old_pos[i] = particles_[i].pos;

				struct collision
				{
					std::size_t i, j;
					math::vector<float, 2> n; // i -> j
				};

				std::vector<collision> collisions;

				for (std::size_t i = 0; i < particles_.size(); ++i)
				{
					for (std::size_t j = i + 1; j < particles_.size(); ++j)
					{
						auto const r = particles_[i].pos - particles_[j].pos;

						float const R = particles_[i].radius + particles_[j].radius;
						float const l = length(r);

						if (l < R)
							collisions.push_back({i, j, - r / l});
					}
				}

				for (std::size_t iteration = 0; iteration < 16; ++iteration)
				{
					for (auto const & c : collisions)
					{
						auto const r = particles_[c.j].pos - particles_[c.i].pos;
						auto const R = particles_[c.j].radius + particles_[c.i].radius;
						auto const l = math::dot(r, c.n);

						if (l < R)
						{
							auto const M = particles_[c.j].mass + particles_[c.i].mass;

							auto const d = (l - R) * c.n / M;

							particles_[c.i].pos += d * particles_[c.j].mass;
							particles_[c.j].pos -= d * particles_[c.i].mass;
						}
					}
				}

				for (std::size_t i = 0; i < particles_.size(); ++i)
					particles_[i].vel += (particles_[i].pos - old_pos[i]) / dt;
			}

			// First-event algorithm
			if (false)
			{
//				std::vector<float> particle_time(particles_.size(), 0.f);

				for (std::size_t i = 0; i < particles_.size(); ++i)
				{
					bool collided = false;

					for (std::size_t j = 0; j < particles_.size(); ++j)
					{
						if (i == j) continue;

//						auto dp = (particles_[i].pos - particles_[i].vel * particle_time[i]) - (particles_[j].pos - particles_[j].vel * particle_time[j]);
						auto dp = particles_[i].pos - particles_[j].pos;
						auto dv = particles_[i].vel - particles_[j].vel;
						auto R = particles_[i].radius + particles_[j].radius;

						float A = math::dot(dv, dv);
						float B = math::dot(dp, dv);
						float C = math::dot(dp, dp) - math::sqr(R);

						if (B >= 0.f) return;

						float collision_time;

						if (C <= 0.f)
						{
							collision_time = 0.f;
						}
						else
						{
							float D = B * B - A * C;

							if (D <= 0.f)
								continue;

							collision_time = C / (- B + std::sqrt(D));
						}

						if (collision_time >= dt)
							continue;

						collided = true;
						particles_[i].pos += particles_[i].vel * collision_time;

//						particles_[i].pos += particles_[i].vel * (e.t - particle_time[i]);
//						particles_[j].pos += particles_[j].vel * (e.t - particle_time[j]);

//						particle_time[i] = e.t;
//						particle_time[j] = e.t;

						auto n = math::normalized(particles_[i].pos - particles_[j].pos);

						float S = 0.5f * (1.f / particles_[i].mass + 1.f / particles_[j].mass);
						float T = dot(n, particles_[i].vel - particles_[j].vel);

						auto np = n * (- T / S);

						np *= std::exp(-10.f * dt);

						particles_[i].vel += np / particles_[i].mass;
						particles_[j].vel -= np / particles_[j].mass;

						break;
					}

					if (!collided)
						particles_[i].pos += particles_[i].vel * dt;
				}
			}

			// Event-based algorithm
			if (false)
			{
//				log::info() << "===== ITERATION =====";
				collisions_ = 0;
				struct collision_event
				{
					std::uint32_t i0, i1;
					float t;
					bool erased = false;
				};

				struct comparator
				{
					bool operator()(collision_event const & e1, collision_event const & e2) const
					{
						return std::tie(e1.t, e1.i0, e1.i1) < std::tie(e2.t, e2.i0, e2.i1);
					}
				};

				using events_container = std::set<collision_event, comparator>;
				events_container events;
				std::vector<events_container::node_type> erased_events;
				std::vector<std::vector<events_container::iterator>> event_list(particles_.size());

				std::vector<float> particle_time(particles_.size(), 0.f);

				std::unordered_map<math::vector<int, 2>, std::vector<std::size_t>> cells;

				float time = 0.f;

				auto check_collision = [&](std::size_t i, std::size_t j)
				{
					if (i > j)
						std::swap(i, j);

					// (pi + vi * (t - ti) - pj - vj * (t - tj))^2 = (ri+rj)^2

					auto dp = (particles_[i].pos - particles_[i].vel * particle_time[i]) - (particles_[j].pos - particles_[j].vel * particle_time[j]);
					auto dv = particles_[i].vel - particles_[j].vel;
					auto R = particles_[i].radius + particles_[j].radius;

					float A = math::dot(dv, dv);
					float B = math::dot(dp, dv);
					float C = math::dot(dp, dp) - math::sqr(R);

//					log::info() << "DV = " << dv;
//					log::info() << "DP = " << dp;
//					log::info() << "B = " << B;

//					if (C < 0.f)
//						log::info() << "DEFINITELY A COLLISION!";

//					if (B > 0.f) return;

//					auto result = math::solve_quadratic(A, B, C);
//					if (!result) return;

//					log::info() << result->first << "   " << result->second;

//					float min_time = 0.f;
//					min_time = std::max(particle_time[i], particle_time[j]);

//					std::optional<float> collision_time;

//					if (result->first >= min_time && result->first <= dt)
//						collision_time = result->first;
//					else if (result->second >= min_time && result->second <= dt)
//						collision_time = result->second;

//					if (result->first >= 0.f && result->first <= dt)
//					if (result->first <= dt)
//						collision_time = result->first;
//						collision_time = std::max(min_time, result->first);
//					(void)min_time;

//					if (!collision_time) return;

					if (B >= -0.01f * math::length(dv) * math::length(dp)) return;

					float collision_time;

					if (C <= 0.f)
					{
						collision_time = time;
					}
					else
					{
						float D = B * B - A * C;

						if (D <= 0.f)
							return;

						collision_time = C / (- B + std::sqrt(D));
					}

					if (collision_time >= dt) return;

					auto dpn = (particles_[i].pos + particles_[i].vel * (collision_time - particle_time[i])) - (particles_[j].pos + particles_[j].vel * (collision_time - particle_time[j]));
					if (math::dot(dv, dpn) >= -0.01f * math::length(dv) * math::length(dpn)) return;

					auto insert_result = events.insert(collision_event{i, j, collision_time});
					if (insert_result.second)
					{
						event_list[i].push_back(insert_result.first);
						event_list[j].push_back(insert_result.first);
					}
				};

				auto erase = [&](events_container::iterator it)
				{
					if (!it->erased)
					{
						auto n = events.extract(it);
						n.value().erased = true;
						erased_events.push_back(std::move(n));
					}
				};

				auto clear_collisions = [&](std::size_t i)
				{
					for (auto it : event_list[i])
						erase(it);
					event_list[i].clear();
				};

				auto foreach_cells = [&](std::size_t i, auto && callback)
				{
					math::box<float, 2> bbox;

					bbox |= particles_[i].pos - math::vector{1.f, 1.f} * particles_[i].radius;
					bbox |= particles_[i].pos + math::vector{1.f, 1.f} * particles_[i].radius;

					auto npos = particles_[i].pos + particles_[i].vel * dt;//(dt - particle_time[i]);

					bbox |= npos - math::vector{1.f, 1.f} * particles_[i].radius;
					bbox |= npos + math::vector{1.f, 1.f} * particles_[i].radius;

					int xmin = std::floor(bbox[0].min);
					int xmax = std::floor(bbox[0].max);
					int ymin = std::floor(bbox[1].min);
					int ymax = std::floor(bbox[1].max);

					for (int x = xmin; x <= xmax; ++x)
					{
						for (int y = ymin; y <= ymax; ++y)
						{
							callback(cells[{x, y}]);
						}
					}
				};

				auto clear_cells = [&](std::size_t i)
				{
					foreach_cells(i, [i](auto & cell){
						auto it = std::find(cell.begin(), cell.end(), i);
						if (it != cell.end())
							cell.erase(it);
					});
				};

				auto fill_cells = [&](std::size_t i)
				{
					foreach_cells(i, [i](auto & cell){ cell.push_back(i); });
				};

				auto find_collisions = [&](std::size_t i)
				{
					foreach_cells(i, [&, i](auto & cell){
						for (auto j : cell)
							if (i != j) check_collision(i, j);
					});
				};

				auto handle_collision = [&](collision_event const & e)
				{
					clear_cells(e.i0);
					clear_cells(e.i1);

					particles_[e.i0].pos += particles_[e.i0].vel * (e.t - particle_time[e.i0]);
					particles_[e.i1].pos += particles_[e.i1].vel * (e.t - particle_time[e.i1]);

//					log::info() << "Collision " << e.i0 << " " << e.i1 << " " << math::distance(particles_[e.i0].pos, particles_[e.i1].pos);

					particle_time[e.i0] = e.t;
					particle_time[e.i1] = e.t;

					auto n = math::normalized(particles_[e.i0].pos - particles_[e.i1].pos);

					float A = 0.5f * (1.f / particles_[e.i0].mass + 1.f / particles_[e.i1].mass);
					float B = dot(n, particles_[e.i0].vel - particles_[e.i1].vel);

					auto BB = B / math::length(particles_[e.i0].vel - particles_[e.i1].vel);
					(void)BB;
					auto np = n * (- B / A);

					np *= std::exp(-10.f * dt);

					particles_[e.i0].vel += np / particles_[e.i0].mass;
					particles_[e.i1].vel -= np / particles_[e.i1].mass;

					fill_cells(e.i0);
					fill_cells(e.i1);
				};

				// 0 - naive
				// 1 - optimized
				int algorithm = 1;

				if (algorithm == 0)
				{
					collisions_ = 0;

					while (time < dt)
					{
						particle_time.assign(particles_.size(), 0.f);

						collision_event next{0, 0, std::numeric_limits<float>::infinity()};

						for (std::size_t i = 0; i < particles_.size(); ++i)
						{
							for (std::size_t j = i + 1; j < particles_.size(); ++j)
							{
								auto rij = particles_[i].pos - particles_[j].pos;
								auto vij = particles_[i].vel - particles_[j].vel;
								auto R = particles_[i].radius + particles_[j].radius;

								float B = math::dot(rij, vij);

								if (B >= 0.f) continue;

								std::optional<collision_event> e;

								float C = math::dot(rij, rij) - math::sqr(R);

								if (C <= 0.f)
								{
									e = collision_event{i, j, 0.f};
								}
								else
								{
									float A = math::dot(vij, vij);

									float D = B * B - A * C;

									if (D <= 0.f)
										continue;

									float t = C / (- B + std::sqrt(D));
									e = collision_event{i, j, t};
								}

								if (e && e->t < next.t)
									next = *e;
							}
						}

						if (time + next.t < dt)
						{
							handle_collision(next);
							++collisions_;

							for (std::size_t i = 0; i < particles_.size(); ++i)
								particles_[i].pos += particles_[i].vel * (next.t - particle_time[i]);

							time += next.t;
						}
						else
							break;
					}

					for (std::size_t i = 0; i < particles_.size(); ++i)
						particles_[i].pos += particles_[i].vel * (dt - time);
				}
				else if (algorithm == 1)
				{
					for (std::size_t i = 0; i < particles_.size(); ++i)
						fill_cells(i);

					for (std::size_t i = 0; i < particles_.size(); ++i)
						find_collisions(i);

					while (!events.empty())
					{
						collision_event e = *events.begin();
						time = e.t;

//						if (e.t < time)
//						{
//							erase(events.begin());
//							continue;
//						}

//						time = e.t;

//						if (e.t < particle_time[e.i0])
//							log::info() << e.t << " < " << particle_time[e.i0];
//						if (e.t < particle_time[e.i1])
//							log::info() << e.t << " < " << particle_time[e.i1];

						handle_collision(e);
						clear_collisions(e.i0);
						clear_collisions(e.i1);

						find_collisions(e.i0);
						find_collisions(e.i1);

						++collisions_;
					}

					for (std::size_t i = 0; i < particles_.size(); ++i)
					{
						particles_[i].pos += particles_[i].vel * (dt - particle_time[i]);
					}
				}
			}

			if (false)
			{
				util::clock<> clock;

				for (std::size_t iteration = 0; iteration < 1; ++iteration)
				{
					for (auto & p : particles_)
					{
						auto r = p.pos - world_center;
						auto l = math::length(r);

						if (l + p.radius > world_size)
						{
							auto n = r / l;

							auto t = math::ort(n);

							auto vn = n * math::dot(p.vel, n);

							auto vt = t * math::dot(p.vel + t * p.angle_vel * p.radius, t);

							auto pt = - vt * (1.f - std::exp(- 10.f * dt));
							float I = 0.5f * p.mass * math::sqr(p.radius);

							p.pos -= n * (l + p.radius - world_size);
							p.vel -= 1.75f * vn;

							p.vel += pt / p.mass;
							p.angle_vel += math::det(n * p.radius, pt) / I;
						}
					}

					for (std::size_t i = 0; i < particles_.size(); ++i)
					{
						for (std::size_t j = i + 1; j < particles_.size(); ++j)
						{
							auto const r = particles_[i].pos - particles_[j].pos;

							float const l = length(r);

							float const R = particles_[i].radius + particles_[j].radius;

							auto const n = r / l;
/*
							if (l < R)
							{
								auto f = E * n * std::pow(l / R, -2.f);
								particles_[i].vel += f * dt / particles_[i].mass;
								particles_[j].vel -= f * dt / particles_[j].mass;

								auto const vij = particles_[i].vel - particles_[j].vel;
								auto dp = - K * dt * n * dot(n, vij) * 2.f / (1.f / particles_[i].mass + 1.f / particles_[j].mass);

								float Ei0 = math::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
								particles_[i].vel += dp / particles_[i].mass;
								float Ei1 = math::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
								particles_[i].T += Ei0 - Ei1;

								float Ej0 = math::length_sqr(particles_[j].vel) * particles_[j].mass * 0.5f;
								particles_[j].vel -= dp / particles_[j].mass;
								float Ej1 = math::length_sqr(particles_[j].vel) * particles_[j].mass * 0.5f;
								particles_[j].T += Ej0 - Ej1;

								float dT = particles_[i].T - particles_[j].T;
								particles_[i].T -= C * dT * dt;
								particles_[j].T += C * dT * dt;
							}
*/

							auto const vij = particles_[i].vel - particles_[j].vel;

							// merging collision
							if (l < R && false)
							{
								particle p;

								auto M = (particles_[i].mass + particles_[j].mass);
								p.pos = particles_[i].pos + (particles_[j].pos - particles_[i].pos) * particles_[j].mass / M;
								p.vel = (particles_[i].vel * particles_[i].mass + particles_[j].vel * particles_[j].mass) / M;
								p.mass = M;
								p.radius = std::sqrt(math::sqr(particles_[i].radius) + math::sqr(particles_[j].radius));
								p.density = p.mass / (math::pi * math::sqr(p.radius));

								particles_[i] = p;
								particles_.erase(particles_.begin() + j);
								break;
							}

							// inelastic collision
//							 if (l < R && dot(vij, n) < 0.f)
							if (l < R && false)
							{
								float D = R - l;

								float A = 0.5f * (1.f / particles_[i].mass + 1.f / particles_[j].mass);
								float B = dot(n, vij);
								auto np = n * (- B / A);
//								np *= 0.5f + 0.5f * std::exp(- 0.f * dt);

								auto p = particles_[j].pos + (particles_[j].radius - D / 2.f) * n;

								auto ri = p - particles_[i].pos;
								auto rj = p - particles_[j].pos;

								auto t = math::ort(n);

								auto vri = math::dot(t, particles_[i].vel + math::ort(ri) * particles_[i].angle_vel);
								auto vrj = math::dot(t, particles_[j].vel + math::ort(rj) * particles_[j].angle_vel);

								auto vrij = vri - vrj;

								auto tp = - t * vrij * (1.f - std::exp(- 10.f * dt));;

								particles_[i].vel += (np + tp) / particles_[i].mass;
								particles_[j].vel -= (np + tp) / particles_[j].mass;

								float Ii = 0.5f * particles_[i].mass * math::sqr(particles_[i].radius);
								float Ij = 0.5f * particles_[j].mass * math::sqr(particles_[j].radius);

								auto ti = math::det(ri,  tp);
								auto tj = math::det(rj, -tp);

								particles_[i].angle_vel += ti / Ii;
								particles_[j].angle_vel += tj / Ij;

								float ki =   D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
								float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);

								particles_[i].pos += ki * n;
								particles_[j].pos += kj * n;
							}

							// inelastic collision without rotation
							 if (l < R && dot(vij, n) < 0.f && false)
//							if (l < R)
							{
								float D = R - l;

								float elasticity = 0.25f;

								float A = (1.f / particles_[i].mass + 1.f / particles_[j].mass);
								float B = dot(n, vij);
								auto np = n * (- B / A) * (1.f + elasticity);

								particles_[i].vel += np / particles_[i].mass;
								particles_[j].vel -= np / particles_[j].mass;

								float ki =   D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
								float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);

								particles_[i].pos += ki * n;
								particles_[j].pos += kj * n;
							}

							// collision replaced by force
							if (l < R && false)
							{
								auto f = 10000.f * n * (R - l);
								particles_[i].vel += dt * f / particles_[i].mass;
								particles_[j].vel -= dt * f / particles_[j].mass;

								auto vn = math::dot(vij, n) * n;
//								vn *= 1.f - std::exp(- (1.f - l / R));
//								vn *= 0.5f;

								particles_[i].vel -= vn * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
								particles_[j].vel += vn * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
							}

							// Verlet collision
							if (l < R)
							{
								float D = (R - l) * 1.f;
								float ki =   D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
								float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);

								auto di = ki * n;
								auto dj = kj * n;

								particles_[i].pos += di;
								particles_[j].pos += dj;
							}

							// impulse-based collision
							if (l < R && dot(n, particles_[i].vel - particles_[j].vel) < 0.f && false)
							{
								float bias = 0.5f;
								float constraint = l - R;
								float reduced_mass = 1.f / (1.f / particles_[i].mass + 1.f / particles_[j].mass);
								float lambda = (- math::dot(n, particles_[i].vel - particles_[j].vel) - bias / dt * constraint) * reduced_mass;
								auto impulse = lambda * n;

								particles_[i].vel += impulse / particles_[i].mass;
								particles_[j].vel -= impulse / particles_[j].mass;
							}
						}
					}

					for (auto & p : particles_)
					{
						p.vel += p.delta_vel;
						p.pos += p.delta_pos;
						p.delta_pos = {0.f, 0.f};
						p.delta_vel = {0.f, 0.f};
					}
				}

				total_collisions_ += clock.count();
			}

			// Verlet finalization
			if(false)
			for (auto & p : particles_)
				p.vel = (p.pos - p.old_pos) / dt;

			// Iterative impulse-based collision
			if (false)
			{
				struct collision
				{
					std::size_t i, j;
					math::vector<float, 2> normal;
					float value;
					float impulse = 0.f;
				};

				std::vector<collision> collisions;

				for (std::size_t i = 0; i < particles_.size(); ++i)
				{
					for (std::size_t j = i + 1; j < particles_.size(); ++j)
					{
						auto const r = particles_[i].pos - particles_[j].pos;

						float const l = length(r);

						float const R = particles_[i].radius + particles_[j].radius;

						auto const n = r / l;

						if (l < R)
						{
							collisions.push_back({i, j, n, l - R});
						}
					}
				}

				for (std::size_t iteration = 0; iteration < SOLVE_ITERATIONS; ++iteration)
				{
					for (auto & c : collisions)
					{
						float bias = BIAS;

						float reduced_mass = 1.f / (1.f / particles_[c.i].mass + 1.f / particles_[c.j].mass);

						auto dv = math::dot(c.normal, particles_[c.i].vel - particles_[c.j].vel);
						float elasticity = 0.75f;

						float lambda = (- dv - bias / dt * c.value - dv * elasticity) * reduced_mass;
						float new_impulse = std::max(0.f, c.impulse + lambda);
						auto delta = new_impulse - c.impulse;
						c.impulse = new_impulse;

						particles_[c.i].vel += delta * c.normal / particles_[c.i].mass;
						particles_[c.j].vel -= delta * c.normal / particles_[c.j].mass;
					}
				}
			}

			if (false)
			for (auto & p : particles_)
			{
				p.pos += p.vel * dt;
			}

			float Ep = 0.f;
			float Ek = 0.f;

			for (std::size_t i = 0; i < particles_.size(); ++i)
			{
				Ek += math::length_sqr(particles_[i].vel) * particles_[i].mass / 2.f;

				for (std::size_t j = i + 1; j < particles_.size(); ++j)
				{
					Ep -= G * particles_[i].mass * particles_[j].mass / distance(particles_[i].pos, particles_[j].pos);
				}
			}

			float omega = 0.f;
			for (std::size_t i = 0; i < particles_.size(); ++i)
			{
				omega += math::det(particles_[i].mass * particles_[i].vel, particles_[i].pos - math::point{0.f, 0.f});
			}

			energy_ = Ek + Ep;

//			log::info() << "Angular velocity: " << omega;

//			log::info() << "Energy: " << Ek << " - " << (-Ep) << " = " << (Ek + Ep);

			++frame_count_;

//			std::reverse(particles_.begin(), particles_.end());
		}
	}

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

		painter_.circle(world_center, world_size, {255, 255, 255, 255}, 72);

//		for (auto const & c : cells_)
//		{
//			float a = 0.25f;
//			if (!c.second.empty())
//				a = 0.5f;
//			painter_.rect({{{c.first[0], c.first[0] + 1.f}, {c.first[1], c.first[1] + 1.f}}}, gfx::to_coloru8(gfx::color_4f{1.f, 0.f, 0.f, a}));
//		}

		for (auto & p : particles_)
		{
//			float c = 2.f / (std::exp(-p.T / 100000.f) + 1.f) - 1.f;
			float c = 1.f - p.density;
			auto x = static_cast<std::uint8_t>(c * 255.f);

			float s = state().size[1] / camera_size_;

			float r = std::max(p.radius * s, 1.5f) / s;
			painter_.circle(p.pos, r, {x, x, x, 191});
//			painter_.line(p.pos, p.pos + r * math::direction(p.angle), r / 16.f, {255, 0, 0, 255}, false);
//			painter_.circle(p.pos, r * 0.75f, {255, 255, 255, 255});
		}

		math::orthographic_camera camera;
		camera.box[0].min = camera_center_[0] - camera_size_ * camera_ratio_ / 2.f;
		camera.box[0].max = camera_center_[0] + camera_size_ * camera_ratio_ / 2.f;
		camera.box[1].min = camera_center_[1] - camera_size_ / 2.f;
		camera.box[1].max = camera_center_[1] + camera_size_ / 2.f;
		camera.box[2].min = -1.f;
		camera.box[2].max =  1.f;

		painter_.render(camera.transform());

		float rotation = 0.f;
		for (auto const & p : particles_)
			rotation += math::det(p.pos - p.pos.zero(), p.vel) * p.mass;
		rotation_.push(rotation);

		{
			gfx::painter::text_options opts;
			opts.scale = 3.f;
			opts.c = {0, 0, 0, 255};
			opts.x = gfx::painter::x_align::center;
			opts.y = gfx::painter::y_align::top;

			auto put = [&](math::point<float, 2> const & pos, std::string const & str)
			{
				return;
				painter_.text(pos, str, opts);
				painter_.text(pos + math::vector{1.f, 0.f}, str, opts);
			};

			put({state().size[0] / 2.f, 30.f}, util::to_string("ITERATIONS: ", SOLVE_ITERATIONS));
			put({state().size[0] / 2.f, 60.f}, BIAS == 1.f ? "BIAS: 1" : util::to_string("BIAS: 1/", std::round(1.f / BIAS)));

//			painter_.text({10.f, 10.f}, util::to_string("Angular velocity: ", rotation_.average()), opts);
//			painter_.text({10.f, 30.f}, util::to_string("Collisions: ", collisions_, " = ", (collisions_ * 1.f / particles_.size() / particles_.size()), " N^2"), opts);
//			painter_.text({10.f, 50.f}, util::to_string("Energy: ", energy_), opts);
		}

		painter_.render(math::window_camera{state().size[0], state().size[1]}.transform());
	}

	~myapp()
	{
		log::info() << "Avg forces time:    " << (total_forces_ / frame_count_);
		log::info() << "Avg collision time: " << (total_collisions_ / frame_count_);

		prof::dump();
	}

private:
	std::default_random_engine rng_;

	std::optional<math::point<float, 2>> force_target_;

	math::point<float, 2> camera_center_ { 0.f, 0.f };
	float camera_size_ = 150.f;
	float camera_ratio_ = 1.f;

	std::optional<math::point<int, 2>> camera_drag_;

	gfx::painter painter_;

	std::vector<particle> particles_;

	int frame_count_ = 0;
	float total_forces_ = 0.f;
	float total_collisions_ = 0.f;

	util::moving_average<float> rotation_{300};
	int collisions_ = 0;
	float energy_ = 0.f;

	bool paused_ = true;
};

namespace psemek::app
{

	std::unique_ptr<application::factory> make_application_factory()
	{
		return default_application_factory<myapp>({.name = "Gravity example"});
	}

}