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

#define TINYOBJLOADER_IMPLEMENTATION
#include "tiny_obj_loader.h"

#include <QJsonArray>
#include <QDebug>
#include <QFile>


Waypoint::Waypoint(unsigned frame, QVector3D pos)
	:frame(frame),
	 pos(pos) {}


Waypoint::Waypoint(const QJsonObject &json)
	:Waypoint(json["frame"].toInt(),
			  QVector3D(json["position"]["lng_X"].toInt() / 100.0,
						json["position"]["alt_Y"].toInt() / 100.0,
						json["position"]["lat_Z"].toInt() / 100.0)) {}


bool Drone::mesh_initialized = false;
OpenGLMesh *Drone::mesh = nullptr;

Drone::Drone() {
	if (!mesh_initialized) {
		QFile obj_file(":/mdl/dji600.obj");
		QFile mtl_file(":/mdl/dji600.mtl");
		obj_file.open(QIODevice::ReadOnly | QIODevice::Text);
		mtl_file.open(QIODevice::ReadOnly | QIODevice::Text);
		std::string obj = obj_file.readAll().toStdString();
		std::string mtl = mtl_file.readAll().toStdString();
		tinyobj::ObjReaderConfig cfg;
		cfg.triangulate = true;
		cfg.vertex_color = false;
		tinyobj::ObjReader reader;
		if (!reader.ParseFromString(obj, mtl, cfg)) {
			if (!reader.Error().empty()) {
				qWarning() << "Erreur lors de la lecture de du modèle";
			}
			exit(1);
		}
		// if (!reader.Warning().empty()) {
		// 	qWarning() << "TinyObjReader: " << reader.Warning();
		// }
		auto& attrib = reader.GetAttrib();
		auto& shapes = reader.GetShapes();

		QVector<float> verts;

		for (size_t s = 0; s < shapes.size(); s++) {
			// Loop over faces(polygon)
			size_t index_offset = 0;
			for (size_t f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++) {
				size_t fv = shapes[s].mesh.num_face_vertices[f];

				// Loop over vertices in the face.
				for (size_t v = 0; v < fv; v++) {
					tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v];
					tinyobj::real_t vx = attrib.vertices[3*idx.vertex_index+0];
					tinyobj::real_t vy = attrib.vertices[3*idx.vertex_index+1];
					tinyobj::real_t vz = attrib.vertices[3*idx.vertex_index+2];
					tinyobj::real_t ts = attrib.texcoords[2*idx.texcoord_index+0];
					tinyobj::real_t tt = attrib.texcoords[2*idx.texcoord_index+1];
					verts.append(vx);
					verts.append(vy);
					verts.append(vz);
					verts.append(ts);
					verts.append(tt);
					// qDebug() << "vert" << vx << vy << vz << "tex" << ts << tt;
				}
				index_offset += fv;
			}
		}

		QOpenGLTexture *texture = new QOpenGLTexture(QImage(":/mdl/dji600.jpg").mirrored());
		// texture->setMinificationFilter(QOpenGLTexture::LinearMipMapLinear);
		// texture->setMagnificationFilter(QOpenGLTexture::Linear);
		mesh = new OpenGLMesh(verts, texture);
		mesh_initialized = true;
	}
	OpenGLWidget::instance->meshes.append(*mesh);
	mesh_id = OpenGLWidget::instance->meshes.size() - 1;
}


Drone::Drone(const QJsonObject &json)
	:Drone() {
	QJsonArray ja = json["waypoints"].toArray();
	waypoints.reserve(ja.size());
	for (const QJsonValue &o : ja) {
		waypoints.append(Waypoint(o.toObject()));
	}
}


const QVector<Waypoint> Drone::getWaypoints() const {
	return waypoints;
}


void Drone::setTo(int frame) {
	int prev = -1, next = -1;
	const Waypoint *prev_wp, *next_wp;
	for (const Waypoint &wp : waypoints) { // TODO: this can be optimized
		if (wp.frame < frame) {
			prev = wp.frame;
			prev_wp = &wp;
		} else {
			next = wp.frame;
			next_wp = &wp;
			break;
		}
	}
	OpenGLMesh &mesh = OpenGLWidget::instance->meshes[mesh_id];
	mesh.mat = QMatrix4x4();
	if (next > -1 && prev == -1) {
		mesh.mat.translate(next_wp->pos);
	} else if (prev > -1 && next == -1) {
		mesh.mat.translate(prev_wp->pos);
	} else {
		mesh.mat.translate(lerp(prev_wp->pos, next_wp->pos, (double) (frame-prev) / (next-prev)));
	}
}


DroneController::DroneController(const QJsonObject &json)
	:framerate(json["framerate"].toInt()) {

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

	OpenGLWidget::instance->makeCurrent();
	QOpenGLTexture *ground_tex = new QOpenGLTexture(QImage(":/mdl/ground.jpg").mirrored());
	OpenGLMesh *ground = new OpenGLMesh({
			-100, 0, -100, 0, 0,
			100, 0, -100, 1, 0,
			-100, 0, 100, 0, 1,
			100, 0, -100, 1, 0,
			-100, 0, 100, 0, 1,
			100, 0, 100, 1, 1,
		}, ground_tex);
	OpenGLWidget::instance->meshes.append(*ground);
	OpenGLWidget::instance->doneCurrent();
	connect(&timer, &QTimer::timeout, this, &DroneController::step);
}


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