#include "fish_schooling.h"

#include <QRandomGenerator>


static double generateDoubleBetween(QRandomGenerator *rng,
									double min, double max) {
	return rng->generateDouble() * (max - min) + min;
}


FishSchooling::FishSchooling(size_t count, QVector3D bounds)
	:bounds(bounds) {
	QRandomGenerator *rng = QRandomGenerator::global();
	for (size_t i = 0; i < count; i++) {
		QVector3D position(
			generateDoubleBetween(rng, -bounds.x()/2, bounds.x()/2),
			generateDoubleBetween(rng, -bounds.y()/2, bounds.y()/2),
			generateDoubleBetween(rng, -bounds.z()/2, bounds.z()/2));
		QVector3D velocity(
			generateDoubleBetween(rng, -5, 5),
			generateDoubleBetween(rng, -5, 5),
			generateDoubleBetween(rng, -5, 5));
		float size = 10;
		fishes.push_back({position, velocity, size, size * 10});
	}
}


void FishSchooling::animate(float dt) {
	for (Fish &fish : fishes) {
		size_t n_neighbors = 0;
		QVector3D distance;
		QVector3D cog;
		QVector3D velocity;
		for (Fish &fish2 : fishes) {
			if (&fish2 != &fish && fish.isCloseTo(fish2.position)) {
				distance += fish2.position - fish.position;
				cog += fish2.position;
				velocity += fish2.velocity;
				n_neighbors++;
			}
		}
		if (n_neighbors != 0) {
			distance /= n_neighbors;
			cog /= n_neighbors;
			velocity /= n_neighbors;
			QVector3D separation = -distance;
			QVector3D alignment = fish.velocity - velocity;
			QVector3D cohesion = cog - fish.position;
			QVector3D velocity_tgt = .8 * separation + 1.5 * alignment + cohesion;
			const float L = .1;
			fish.velocity = L * velocity_tgt + (1 - L) * fish.velocity;
		}
		float speed = fish.velocity.length();
		if (speed > 10) {
			fish.velocity = fish.velocity / speed * 10;
		}
		fish.position += fish.velocity * dt;
	}
}