#include "main_window.hh"

#include <QVector3D>
#include <QRandomGenerator>


struct Fish {
	QVector3D pos;
	QVector3D vel;
	float size;
	float dist = 10;
	float sqdist;

	OpenGLMesh mesh;

	Fish(QVector3D pos, OpenGLMesh mesh)
		:pos(pos),
		 sqdist(dist * dist),
		 mesh(mesh) {}

	void step() {
		pos += vel;
		mesh.mat.setToIdentity();
		QMatrix4x4 rot;
		rot.lookAt(pos, vel, {0, 1, 0});
		mesh.mat.translate(pos);
		// mesh.mat = rot * mesh.mat;
	}

	bool in_neighborhood(const QVector3D &p) const {
		return (p - pos).lengthSquared() < sqdist;
	}
};


struct Banc {
	QVector<Fish> fishes;

	Banc() {}

	Banc(size_t nfishes) {
		OpenGLMesh fish_mesh = OpenGLMesh({
				0, .1, 0,
				-.0866, -.05, 0,
				.0866, -.05, 0,
			});
		QRandomGenerator *rng = QRandomGenerator::global();
		for (size_t i = 0; i < nfishes; i++) {
			Fish fish({
					(float) (rng->generateDouble() * 3 - 1.5),
					(float) (rng->generateDouble() * 3 - 1.5),
					(float) (rng->generateDouble() * 3 - 1.5)
				}, fish_mesh);
			fishes.append(fish);
		}
	}

	void step() {
		QVector<Fish> neighbors;
		for (Fish &fish : fishes) {
			QVector3D neighbors_cog;
			QVector3D neighbors_vel;
			size_t nneighbors = 0;
			for (Fish &fish_b : fishes) {
				if (fish_b.in_neighborhood(fish.pos)) {
					nneighbors++;
					neighbors_cog += fish_b.pos;
					neighbors_vel += fish_b.vel;
				}
			}
			neighbors_cog /= nneighbors;
			neighbors_vel /= nneighbors;
			QVector3D v1 = -(fish.pos - neighbors_cog);
			QVector3D v2 = fish.vel - neighbors_vel;
			QVector3D v3 = neighbors_cog - fish.pos;
			QVector3D v = 1.5 * v1 + v2 + v3;
			float L = .001;
			fish.vel = (1-L) * fish.vel + L * v;
			fish.step();
		}
	}
};


Banc banc;

void MainWindow::init() {
	banc = Banc(10);
	for (Fish &fish : banc.fishes) {
		glw.meshes.append(&fish.mesh);
	}
}


void MainWindow::step() {
	banc.step();
	OpenGLWidget::instance->update();
}


void OpenGLWidget::paintGL() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	for (const Fish &fish : banc.fishes) {
		glUniformMatrix4fv(model_attr, 1, GL_FALSE, fish.mesh.mat.data());
		glBindVertexArray(fish.mesh.vao);
		glDrawArrays(GL_TRIANGLES, 0, fish.mesh.nverts);
	}
}