#include "drone.hh"

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

#include <QJsonArray>
#include <QDebug>


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

Drone::Drone(int id)
	:id(id) {
	OpenGLWidget *glw = OpenGLWidget::instance;
	if (!mesh_initialized) {
		glw->makeCurrent();
		QVector<GLfloat> verts = load_obj(":/mdl/dji600.obj", LOAD_OBJ_NORMALS | LOAD_OBJ_UVS);
		QOpenGLTexture *texture = new QOpenGLTexture(QImage(":/img/dji600.jpg").mirrored());
		mesh = new OpenGLMesh(verts, texture, glw->getMainProgram());
		mesh_initialized = true;
		glw->doneCurrent();
	}
}


Drone::Drone(const QJsonObject &json)
	:Drone(json["id"].toInt()) {
	QJsonArray ja = json["waypoints"].toArray();
	waypoints.reserve(ja.size());
	for (const QJsonValue &o : ja) {
		waypoints.append(Waypoint(o.toObject()));
	}
	for (int i = 0; i < waypoints.size() - 1; i++) {
		waypoints[i].computed_speed = (waypoints[i + 1].pos - waypoints[i].pos).length()
			/ (waypoints[i + 1].frame - waypoints[i].frame);
	}
	setTo(0);
}


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;
		}
	}
	if (next > -1 && prev == -1) {
		pos = next_wp->pos;
		speed = next_wp->computed_speed;
	} else if (prev > -1 && next == -1) {
		pos = prev_wp->pos;
		speed = prev_wp->computed_speed;
	} else {
		pos = lerp(prev_wp->pos, next_wp->pos, (double) (frame-prev) / (next-prev));
		speed = prev_wp->computed_speed;
	}
}


const QVector3D Drone::getPos() const {
	return pos;
}


int Drone::getId() const {
	return id;
}


double Drone::getSpeed() const {
	return speed;
}


const OpenGLMesh *Drone::getMesh() const {
	return mesh;
}