#include "ObjectInstance.hpp"
#include <glm/gtc/matrix_transform.hpp>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/transform.hpp>
#include <glm/gtx/norm.hpp>
#include <iostream>

/*was glm/gtc/before */
#include <glm/gtx/quaternion.hpp>

/* aarch64-linux-gnu/include/math.h already defines it */
#ifndef M_PI
#define M_PI	3.14159265358979323846264338327950288
#endif

glm::mat4 WorldInstanceable::getLocalTransform() {
	glm::mat4 transform = glm::mat4(1.0);
	transform *= scaleMat;
	transform *= rotation;
	return transform;
}

glm::mat4 WorldInstanceable::getGlobalTransform() {
	glm::mat4 transform = getLocalTransform();

	//transform = glm::translate(glm::mat4(transform), position);
	
	return glm::translate(glm::mat4(1.0), position) * transform;
}



void WorldInstanceable::alignTo(glm::vec3 direction) {
	glm::vec3 localZ;

	direction = glm::normalize(direction);
	localZ = localFront;

	glm::quat a = glm::rotation(localZ, direction);

	localFront = glm::toMat4(a) * glm::vec4(localFront, 0.0f);

	glm::quat b = glm::rotation(localZ, direction);

	localUp = glm::toMat4(b) * glm::vec4(localUp, 0.0f);

	//++++++++++++++++++++++++++
	//glm::vec3 right = glm::normalize(cross(localFront, localUp));
	glm::vec3 right = glm::normalize(cross(localUp, localFront));
	right.y = 0;
	glm::vec3 stabilizedR = glm::normalize(right);
	
	glm::quat c = glm::rotation(right, stabilizedR);
	localUp = glm::toMat4(c) * glm::vec4(localUp, 0);
	localUp = -glm::cross(stabilizedR, localFront);
	rotation = glm::mat4(glm::mat3(stabilizedR, localUp, localFront));
	
}

void WorldInstanceable::setPosition(glm::vec3 position) {
	this->position = position;
}

void WorldInstanceable::rotate(glm::vec3 axis, float angle) {
	rotation *= glm::rotate(glm::radians(angle), axis);
}

void WorldInstanceable::scale(float factor) {
	scaleMat *= glm::scale(glm::mat4(1.0), glm::vec3(factor));
}

void WorldInstanceable::scale(glm::vec3 factors) {
	scaleMat *= glm::scale(glm::mat4(1.0), factors);
}

void WorldInstanceable::setup(glm::vec3 position) {
	this->position = position;
	localFront = glm::vec3( 0, 0, 1);
	localUp = glm::vec3( 0, 1, 0);
	rotation = glm::mat4(1.0f);
	scaleMat = glm::mat4(1.0f);
}

WorldInstanceable::WorldInstanceable(glm::vec3 position) {
	setup(position);
}

WorldInstanceable::WorldInstanceable(glm::vec3 position, glm::vec3 scale) {
	setup(position);
	this->scale(scale);
}

WorldInstanceable::WorldInstanceable(glm::vec3 position, glm::vec3 axis, float angle) {
	setup(position);
	this->rotate(axis, angle);
}

WorldInstanceable::WorldInstanceable(glm::vec3 position, glm::vec3 scale, glm::vec3 axis, float angle) {
	setup(position);
	this->scale(scale);
	this->rotate(axis, angle);
}