#include <psemek/app/app.hpp>
#include <psemek/app/main.hpp>
#include <psemek/gfx/painter.hpp>
#include <psemek/gfx/gl.hpp>
#include <psemek/geom/scale.hpp>
#include <psemek/geom/camera.hpp>
#include <psemek/geom/constants.hpp>
#include <psemek/util/clock.hpp>
#include <psemek/audio/engine.hpp>
#include <psemek/audio/constants.hpp>
#include <psemek/audio/oscillator.hpp>
#include <psemek/audio/effect/compressor.hpp>
#include <psemek/prof/profiler.hpp>

#include <random>

/*
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
{
	geom::point<float, 2> pos;
	geom::vector<float, 2> vel;

	float angle;
	float angle_vel;

	float radius;
	float mass;
	float density;

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

	geom::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 dt = 0.01f;

float const world_size = 10000.f;
geom::point world_center{0.f, 0.f};

struct sound_stream
	: audio::stream
{
	sound_stream(std::vector<particle> const & particles)
		: particles_(particles)
	{}

	std::optional<std::size_t> length() const override
	{
		return std::nullopt;
	}

	std::size_t read(float * data, std::size_t sample_count) override
	{
		std::size_t time = played_.load() / 2;

		if (velocity_.size() < particles_.size())
		{
			velocity_.resize(particles_.size(), 0.f);
			amplitude_.resize(particles_.size(), 0.f);
			oscillator_.resize(particles_.size(), 0.f);
		}

		float mean_v = 0.f;
		for (std::size_t i = 0; i < particles_.size(); ++i)
		{
			float v = geom::length(particles_[i].vel);
			velocity_[i] = geom::lerp(velocity_[i], v, 0.1f);
			mean_v += velocity_[i];
		}
		mean_v /= particles_.size();

		for (std::size_t i = 0; i < particles_.size(); ++i)
		{
//			oscillator_[i].frequency(500.f * velocity_[i] / mean_v);
//			oscillator_[i].frequency(geom::lerp(100.f, 10000.f, velocity_[i] / 1000.f));
			oscillator_[i].frequency(10.f * velocity_[i]);
//			oscillator_[i].frequency(0.1f * geom::sqr(velocity_[i]));
		}


		for (std::size_t i = 0; i < sample_count; i += 2)
		{
			float v = 0.f;
			for (std::size_t j = 0; j < particles_.size(); ++j)
				v += oscillator_[j].next().imag();

			if (i >= 2)
			{
				v = (v + data[i - 1]) / 2.f;
			}

			data[i + 0] = v;
			data[i + 1] = v;

			time += 1;
		}

		played_.fetch_add(sample_count);

		return sample_count;
	}

	// The number of samples already played from this stream
	std::size_t played() const override
	{
		return played_.load();
	}

private:
	std::vector<particle> const & particles_;
	std::vector<float> velocity_;
	std::vector<float> amplitude_;
	std::vector<audio::oscillator> oscillator_;
	std::atomic<std::size_t> played_;
};

struct myapp : app::app
{
	myapp()
		: app("Test app", 4)
		, rng_{std::random_device{}()}
	{
		gl::ClearColor(1.f, 1.f, 1.f, 1.f);

		vsync(true);

		audio_.output()->stream(audio::compressor(std::make_shared<sound_stream>(particles_), audio::from_db(-4.f), 0.95f));

		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 * geom::pi};

		float min_R = 0.f;
		float max_R = 50.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, geom::pi * 10000.f, 1.f});

//		particles_.push_back({{-1.f, 0.f}, {0.f, -1.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
//		particles_.push_back({{ 1.f, 0.f}, {0.f,  1.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, geom::rad(120.f), geom::rad(240.f)};

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

			geom::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 = geom::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 += geom::rad(std::uniform_real_distribution<float>(-2.f, 2.f)(rng_));

				p =
				{
					R * std::cos(a),
					R * std::sin(a),
				};
				p += geom::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 geom::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 - geom::point{0.f, 0.f};
			auto R = geom::length(r);
			float V = std::sqrt(G * total_M / R);
			particles_[i].vel = geom::ort(r / R) * V;
			(void)V;
		}
	}

	void on_resize(int width, int height) override
	{
		gl::Viewport(0, 0, width, height);

		window_size_ = {width, height};

		camera_ratio_ = static_cast<float>(width) / height;
	}

	void on_mouse_wheel(int delta) override
	{
		camera_size_ *= std::pow(0.8f, delta);
	}

	void on_left_button_down() override
	{
		if (mouse_)
		{
			geom::scale<float, 2> const flip_y({1.f, -1.f});

			float const scale = camera_size_ / window_size_[1];

			geom::point<int, 2> const screen_center { window_size_[0] / 2, window_size_[1] / 2 };
			auto target = camera_center_ + flip_y(geom::cast<float>(*mouse_ - screen_center) * scale);
			force_target_ = target;

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

//			geom::vector<float, 2> vel = geom::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 / geom::length_sqr(r) / p.mass;
//			}
		}
	}

	void on_left_button_up() override
	{
		force_target_ = std::nullopt;
	}

	void on_right_button_down() override
	{
		camera_drag_ = mouse_;
	}

	void on_right_button_up() override
	{
		camera_drag_ = std::nullopt;
	}

	void on_mouse_move(int x, int y, int, int) override
	{
		mouse_ = {x, y};

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

			float const scale = camera_size_ / window_size_[1];

			camera_center_ += flip_y(geom::cast<float>(*camera_drag_ - *mouse_) * scale);
			camera_drag_ = mouse_;
		}

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

	void on_key_down(SDL_Keycode key) override
	{
		app::app::on_key_down(key);

		if (key == SDLK_SPACE)
		{
			particles_.push_back({{200.f, 20.f}, {-1000.f, 0.f}, 0.f, 0.f, 1.f, 100.f, 1.f});
		}
	}

	void update() override
	{
		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 += geom::vector{0.f, -GG};

				for (auto & p : particles_)
				{
					auto r = (p.pos - p.pos.zero());
					p.acc -= GC * r / std::pow(1.f + geom::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| = " << geom::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;
					}
				}

				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 (force_target_ && false)
			{
				for (auto & p : particles_)
				{
					auto r = p.pos - *force_target_;
					p.acc += 100000.f * r / geom::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_;

			for (auto & p : particles_)
			{
				p.vel += p.acc * dt;
				p.pos += p.vel * dt;
				p.angle += p.angle_vel * dt;
			}

			{
				util::clock<> clock;

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

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

							auto t = geom::ort(n);

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

							auto vt = t * geom::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 * geom::sqr(p.radius);

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

							p.vel += pt / p.mass;
							p.angle_vel += geom::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 = geom::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
								particles_[i].vel += dp / particles_[i].mass;
								float Ei1 = geom::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
								particles_[i].T += Ei0 - Ei1;

								float Ej0 = geom::length_sqr(particles_[j].vel) * particles_[j].mass * 0.5f;
								particles_[j].vel -= dp / particles_[j].mass;
								float Ej1 = geom::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(geom::sqr(particles_[i].radius) + geom::sqr(particles_[j].radius));
								p.density = p.mass / (geom::pi * geom::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 = geom::ort(n);

								auto vri = geom::dot(t, particles_[i].vel + geom::ort(ri) * particles_[i].angle_vel);
								auto vrj = geom::dot(t, particles_[j].vel + geom::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 * geom::sqr(particles_[i].radius);
								float Ij = 0.5f * particles_[j].mass * geom::sqr(particles_[j].radius);

								auto ti = geom::det(ri,  tp);
								auto tj = geom::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;
							}

							// collision replaced by force
							if (l < R)
							{
								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 = geom::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 && false)
							{
								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;
							}
						}
					}

					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();
			}


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

			for (std::size_t i = 0; i < particles_.size(); ++i)
			{
				Ek += geom::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 += geom::det(particles_[i].mass * particles_[i].vel, particles_[i].pos - geom::point{0.f, 0.f});
			}

//			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 & 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 = window_size_[1] / camera_size_;

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

		geom::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());
	}

	~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_;

	geom::vector<int, 2> window_size_;

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

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

	std::optional<geom::point<int, 2>> mouse_;
	std::optional<geom::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;

	audio::engine audio_;
};

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