#include "simulation_v2.h"
#include "SFML/Graphics/Text.hpp"
#include "SFML/Graphics/VertexArray.hpp"
#include <SFML/Graphics/CircleShape.hpp>
#include <array>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <mutex>
#include <random>
#include <ranges>
#include <set>

#define HASH_BUCKETS 255
#define HASH_GRID_DIVISIONS 16

namespace v2 {
typedef uint8_t PointHash;

struct Particle {
	bool active{true};
	sf::CircleShape shape;
	sf::Vector2f velocity;
	sf::Vector2f solveMotion;
	sf::Vector2f acceleration;
	PointHash spatialHash;
};

sf::FloatRect worldBounds{};
std::array<std::set<Particle *>, HASH_BUCKETS> buckets{}; //!< Hash buckets; grid of 16x16 cells stretched over the bounding area
std::vector<Particle> particles{};
std::mutex mtx{};

// hashing strategy:
// A 2 grid of rectangles divided evenly over the world's bounding box up to 16 * 16 buckets.
// A point is hashed by converting from world space coordinates to floored grid coordinate.
// To look it up, simply use the pointhash to index into the `buckets` array.
PointHash hashPoint(sf::Vector2f vec, sf::FloatRect bounds) {
	// least- and most- significant four bits
	uint8_t ls4(uint8_t(std::floor(((vec.y - bounds.position.y) / bounds.size.y) * HASH_GRID_DIVISIONS)) & 0xF);
	uint8_t ms4(uint8_t(std::floor(((vec.x - bounds.position.x) / bounds.size.x) * HASH_GRID_DIVISIONS)) & 0xF);
	return (ls4 | (ms4 << 4u));
}

sf::Vector2f particleTopLeft(Particle &particle) {
	return particle.shape.getPosition() - sf::Vector2f{particle.shape.getRadius(), particle.shape.getRadius()};
}

PointHash hashParticlePosition(Particle &particle) {
	return hashPoint(particleTopLeft(particle), worldBounds);
}

void updateParticleHash(Particle &particle) {
	PointHash previous{particle.spatialHash};
	particle.spatialHash = hashParticlePosition(particle);
	if (particle.spatialHash != previous) {
		buckets.at(previous % HASH_BUCKETS).erase(&particle);
		buckets.at(particle.spatialHash % HASH_BUCKETS).emplace(&particle);
	}
}

void initialize(size_t particleCount, sf::FloatRect bounds) {
	worldBounds = bounds;

	std::mt19937 rng{std::mt19937()};
	std::uniform_real_distribution<float> xPositionDist{std::uniform_real_distribution<float>(bounds.position.x, bounds.position.x + bounds.size.x)};
	std::uniform_real_distribution<float> yPositionDist{std::uniform_real_distribution<float>(bounds.position.y, bounds.position.y + bounds.size.y)};
	std::uniform_real_distribution<float> velocityDist{std::uniform_real_distribution<float>(-200.0f, 200.0f)};
	std::uniform_int_distribution<uint8_t> colorDist{std::uniform_int_distribution<uint8_t>(0, 255)};
	std::uniform_real_distribution<float> radiusDist{std::uniform_real_distribution<float>(2.5f, 2.5f)};
	particles.reserve(particleCount);
	for (size_t _ : std::ranges::views::iota(0zu, particleCount)) {
		particles.emplace_back(true,
							   sf::CircleShape(radiusDist(rng)),
							   sf::Vector2f{0, 0});
		Particle &particle{particles[particles.size() - 1]};
		particle.shape.setPosition({xPositionDist(rng), yPositionDist(rng)});
		particle.velocity = {velocityDist(rng), velocityDist(rng)};
		particle.spatialHash = hashParticlePosition(particle);
		particle.shape.setFillColor({colorDist(rng), colorDist(rng), colorDist(rng), 255});
		particle.shape.setOrigin({particle.shape.getRadius(), particle.shape.getRadius()});
	}
}

sf::Vector2f reflect(sf::Vector2f x, sf::Vector2f normal, float bounce = 1.0) {
	float const dot{x.dot(normal)};
	if (dot > 0)
		return x;
	return x - (normal * -std::abs(dot) * (1 + bounce));
}

void particleSolveOverlap(Particle &self, Particle const &other) {
	sf::Vector2f const difference{self.shape.getPosition() - other.shape.getPosition()};
	float const targetDistance{self.shape.getRadius() + other.shape.getRadius()};
	float const currentDistance{difference.length()};
	if (currentDistance < targetDistance && currentDistance > 0) {
		sf::Vector2f const collisionNormal{difference.normalized()};
		float const requiredMotion{targetDistance - currentDistance};
		self.solveMotion += requiredMotion * collisionNormal * 0.5f;
		self.acceleration += reflect(self.velocity, collisionNormal) - self.velocity;
	}
}

void particleSolveOverlaps(Particle &self) {
	sf::Vector2f position{self.shape.getPosition()};
	sf::Vector2f offset{worldBounds.size.x / HASH_GRID_DIVISIONS, worldBounds.size.y / HASH_GRID_DIVISIONS};
	PointHash neighbours[4]{
		self.spatialHash,
		hashPoint(particleTopLeft(self) + sf::Vector2f{offset.x, 0}, worldBounds),
		hashPoint(particleTopLeft(self) + sf::Vector2f{offset.x, offset.y}, worldBounds),
		hashPoint(particleTopLeft(self) + sf::Vector2f{0, offset.y}, worldBounds)};
	for (PointHash neighbour : neighbours) {
		for (Particle *possible : buckets[neighbour % HASH_BUCKETS]) {
			if (possible == &self) {
				continue;
			} else {
				particleSolveOverlap(self, *possible);
			}
		}
	}
	if (position.x - self.shape.getRadius() < worldBounds.position.x) {
		self.solveMotion.x += worldBounds.position.x - (position.x - self.shape.getRadius());
		self.acceleration += reflect(self.velocity, {1, 0}) - self.velocity;
	}
	if (position.x + self.shape.getRadius() > worldBounds.position.x + worldBounds.size.x) {
		self.solveMotion.x += (worldBounds.position.x + worldBounds.size.x) - (position.x + self.shape.getRadius());
		self.acceleration += reflect(self.velocity, {-1, 0}) - self.velocity;
	}
	if (position.y - self.shape.getRadius() < worldBounds.position.y) {
		self.solveMotion.y += worldBounds.position.y - (position.y - self.shape.getRadius());
		self.acceleration += reflect(self.velocity, {0, 1}) - self.velocity;
	}
	if (position.y + self.shape.getRadius() > worldBounds.position.y + worldBounds.size.y) {
		self.solveMotion.y += (worldBounds.position.y + worldBounds.size.y) - (position.y + self.shape.getRadius());
		self.acceleration += reflect(self.velocity, {0, -1}) - self.velocity;
	}
}

void tick(double delta) {
	for (Particle &particle : particles) {
		if (!particle.active) {
			return;
		}
		particleSolveOverlaps(particle);
	}

	for (Particle &particle : particles) {
		if (particle.active) {
			// apply velocity and combined solve motion
			particle.shape.move(particle.solveMotion);
			particle.shape.move(particle.velocity * float(delta));
			particle.velocity += particle.acceleration;
			particle.acceleration = particle.solveMotion = {0, 0}; // reset solve motion and acceleration
			updateParticleHash(particle);						   // update spatial hash
		}
	}
}

void draw(sf::RenderTarget *target) {
#if 0
	sf::RectangleShape rect{worldBounds.size};
	rect.setPosition(worldBounds.position);
	rect.setFillColor(sf::Color::Transparent);
	rect.setOutlineColor(sf::Color::White);
	rect.setOutlineThickness(3);
	target->draw(rect);
	sf::Vertex polygon[2];
#endif
	for (Particle &particle : particles) {
		if (particle.active) {
			target->draw(particle.shape);
#if 0
			polygon[0].position = particle.shape.getPosition();
			polygon[1].position = particle.shape.getPosition() + particle.velocity;
			target->draw(polygon, 2, sf::PrimitiveType::Lines);
#endif
		}
	}
}
} // namespace v2