#include "drone_controller.hh"
#include "opengl_widget.hh"
#include "load_obj.hh"

#include <QJsonArray>
#include <QDebug>
#include <QOpenGLShaderProgram>
#include <QPainter>


const unsigned char DroneController::sphere_neutral[] = {
	166, 166, 166
};

OpenGLMesh *DroneController::sphere = nullptr;


DroneController::DroneController(const QJsonObject &json)
	:framerate(json["framerate"].toInt()),
	 sphere_timer(this) {
	sphere_timer.setSingleShot(true);
	connect(&sphere_timer, &QTimer::timeout, [&]() {
		draw_spheres = false;
		OpenGLWidget::instance->update();
	});
	if (sphere == nullptr) {
		OpenGLWidget::instance->makeCurrent();
		sphere = new OpenGLMesh(load_obj(":/mdl/sphere.obj", LOAD_OBJ_NORMALS | LOAD_OBJ_UVS),
								new QOpenGLTexture(QImage(sphere_neutral, 1, 1, QImage::Format_RGB888)),
								OpenGLWidget::instance->getMainProgram());
		OpenGLWidget::instance->makeCurrent();
	}
	QJsonArray ja = json["drones"].toArray();
	drones.reserve(ja.size());
	for (const QJsonValue &o : ja) {
		drones.append(Drone(o.toObject()));
	}
	for (const Drone &d : drones) {
		for (const Waypoint &wp : d.getWaypoints()) {
			if (wp.frame > duration) duration = wp.frame;
		}
	}

	connect(&timer, &QTimer::timeout, this, &DroneController::step);
}


void DroneController::drawTrajectory(OpenGLWidget *glw, const Drone &d) const {
	glw->getLineProgram()->bind();
	glw->getLineProgram()->setUniformValue("color", 1, 0, .532);
	size_t trajectory_len = 1;
	for (const Waypoint &wp : d.getWaypoints()) {
		if (wp.frame > frame) break;
		trajectory_len++;
	}
	GLfloat trajectory[trajectory_len * 3] = {0};
	size_t i = 0;
	for (const Waypoint &wp : d.getWaypoints()) {
		if (wp.frame > frame) break;
		trajectory[i] = wp.pos.x();
		trajectory[i + 1] = wp.pos.y();
		trajectory[i + 2] = wp.pos.z();
		i += 3;
	}
	trajectory[i] = d.getPos().x();
	trajectory[i + 1] = d.getPos().y();
	trajectory[i + 2] = d.getPos().z();
	glw->glEnableVertexAttribArray(0);
	glw->glBindBuffer(GL_ARRAY_BUFFER, 0);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, trajectory);
	glLineWidth(2);
	glw->glDisable(GL_CULL_FACE);
	glw->glDrawArrays(GL_LINE_STRIP, 0, trajectory_len);
	glw->glEnable(GL_CULL_FACE);
	glw->getLineProgram()->release();
	glw->getMainProgram()->bind();
}


void DroneController::drawGuide(OpenGLWidget *glw, const Drone &d) const {
	glw->getLineProgram()->bind();
	glw->glEnableVertexAttribArray(0);
	glw->glBindBuffer(GL_ARRAY_BUFFER, 0);
	glw->glDisable(GL_CULL_FACE);

	QVector<GLfloat> support_line {
		d.getPos().x(), 0, d.getPos().z(),
		d.getPos().x(), d.getPos().y(), d.getPos().z(),
	};
	glLineWidth(2);
	glw->getLineProgram()->setUniformValue("color", .2, .2, .4);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, support_line.constData());
	glw->glDrawArrays(GL_LINES, 0, 2);

	QVector<GLfloat> grid;
	const int size = 100;
	const float offset = 0;
	for (int i = -size; i < size; i += 10) {
		grid.append(i);
		grid.append(offset);
		grid.append(-size);
		grid.append(i);
		grid.append(offset);
		grid.append(size);
		grid.append(-size);
		grid.append(offset);
		grid.append(i);
		grid.append(size);
		grid.append(offset);
		grid.append(i);
	}
	glLineWidth(1);
	glw->getLineProgram()->setUniformValue("color", .2, .2, .2);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, grid.constData());
	glw->glDrawArrays(GL_LINES, 0, grid.size() / 3);

	glDisable(GL_DEPTH_TEST); // pro-gamer move
	QVector<GLfloat> axes {
		0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 1,
	};
	glLineWidth(2);
	glw->getLineProgram()->setUniformValue("color", 1, 0, 0);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData());
	glw->glDrawArrays(GL_LINES, 0, 2);
	glw->getLineProgram()->setUniformValue("color", 0, 1, 0);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData() + 6);
	glw->glDrawArrays(GL_LINES, 0, 2);
	glw->getLineProgram()->setUniformValue("color", 0, 0, 1);
	glw->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData() + 12);
	glw->glDrawArrays(GL_LINES, 0, 2);
	glDisable(GL_DEPTH_TEST);

	glw->glEnable(GL_CULL_FACE);
	glw->getLineProgram()->release();
	glw->getMainProgram()->bind();

	QVector3D text_pos = d.getPos();
	text_pos.setY(text_pos.y() + .5);
	QPoint screen_pos;
	if (!glw->project(text_pos, screen_pos)) return;
	QPainter painter(glw);
	painter.endNativePainting();
	painter.setRenderHint(QPainter::Antialiasing);
	painter.setPen(Qt::green);
	QRect rect(0, 0, 200, 100);
	rect.moveCenter({screen_pos.x(), screen_pos.y()});
	painter.drawText(rect, Qt::AlignCenter, QString::number(d.getId()));
	painter.beginNativePainting();
	painter.end();
}


void DroneController::draw(OpenGLWidget *glw) const {
	const QVector<QPair<int, int>> &col = collisions[frame];
	for (const Drone &d : drones) {
		glw->getMainProgram()->bind();
		QMatrix4x4 mat;
		mat.translate(d.getPos());
		for (const QPair<int, int> &p : col) {
			if (d.getId() == p.first || d.getId() == p.second) {
				glw->getMainProgram()->setUniformValue("highlight", true);
			}
		}
		d.getMesh()->draw(glw, mat);
		if (draw_spheres) {
			mat.scale(sphere_radius);
			sphere->draw(glw, mat);
		}
		glw->getMainProgram()->bind();
		glw->getMainProgram()->setUniformValue("highlight", false);
		glw->getMainProgram()->release();
		if (draw_trajectories) {
			drawTrajectory(glw, d);
		}
		if (draw_guides) {
			drawGuide(glw, d);
		}
	}
}


int DroneController::getDuration() const {
	return duration;
}


void DroneController::step() {
	for (Drone &d : drones) {
		d.setTo(frame);
	}
	OpenGLWidget::instance->update();
	emit frameChanged(frame);
	if (frame == duration) {
		pause();
	} else {
		frame++;
	}
}


void DroneController::play() {
	if (!paused) return;
	paused = false;
	timer.start(1000. / framerate);
	emit playing();
}


void DroneController::pause() {
	if (paused) return;
	paused = true;
	timer.stop();
	emit pausing();
}


void DroneController::suspend() {
	if (!paused) {
		pause();
		paused = false;
	}
}


void DroneController::resume() {
	if (!paused) {
		paused = true;
		play();
	}
}


void DroneController::seek(int frame) {
	if (this->frame == frame) return;
	this->frame = frame;
	step();
}


void DroneController::computeCollisions(double sphere_radius) {
	collisions.clear();
	double sqDist = sphere_radius * sphere_radius * 4;
	for (int i = 0; i < duration; i++) {
		for (Drone &a : drones) {
			a.setTo(i);
			for (Drone &b : drones) {
				b.setTo(i);
				if (&a == &b) continue;
				if (collides(a, b, sqDist)) {
					collisions[i].append(QPair<int, int>(a.getId(), b.getId()));
					emit collision(a.getId(), b.getId(), i);
				}
			}
		}
	}
	for (Drone &d : drones) {
		d.setTo(frame);
	}
}


void DroneController::computeSpeedingViolations(double speed) {
	speed_limit = speed;
	for (int i = 0; i < duration; i++) {
		for (Drone &d : drones) {
			d.setTo(i);
			if (d.getSpeed() > speed) {
				emit speedViolation(d.getId(), d.getSpeed(), i);
			}
		}
	}
	for (Drone &d : drones) {
		d.setTo(frame);
	}
}


void DroneController::displaySpheres(double sphere_radius) {
	draw_spheres = true;
	this->sphere_radius = sphere_radius;
	sphere_timer.start(1000);
	OpenGLWidget::instance->update();
}


bool DroneController::collides(const Drone &a, const Drone &b, double sqDist) {
	return (b.getPos() - a.getPos()).lengthSquared() < sqDist;
}


void DroneController::setDrawTrajectories(bool enable) {
	draw_trajectories = enable;
	OpenGLWidget::instance->update();
}


void DroneController::setDrawGuides(bool enable) {
	draw_guides = enable;
	OpenGLWidget::instance->update();
}