#include "dut/Transform.h"


using namespace Dut;

glm::vec3 Transform::GetEulerAnglesFromRotationMatrix(const glm::mat4& rotationMatrix)
{

	float c = sqrtf(1.0f - rotationMatrix[2][1] * rotationMatrix[2][1]);
	if (isnan(c))
	{
		c = 0.f;
	}
	glm::vec3 rotation;

	rotation.z = glm::atan(rotationMatrix[0][1], rotationMatrix[1][1]);
	rotation.x = glm::atan(-rotationMatrix[2][1], c);
	rotation.y = glm::atan(rotationMatrix[2][0], rotationMatrix[2][2]);

	// theta phi pitch
	// ˳�򲻶�
	return rotation * 180.0f / glm::pi<float>();
}

Transform::Transform()
{
	position = glm::vec3(0.f, 0.f, 0.f);
	rotation = glm::vec3(0.0f, 0.0f, 0.0f);
	scale = glm::vec3(1.f, 1.0f, 1.0f);
}


glm::vec3 Transform::Right()
{
	glm::mat4 rMat = GetRotationMatrix();
	return -rMat[0];
}

glm::vec3 Transform::Forward()
{
	glm::mat4 rMat = GetRotationMatrix();
	return -rMat[2];
}

glm::vec3 Transform::Up()
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[1];
}

glm::mat4 Transform::GetViewMatrix()
{
	return glm::lookAt(position, position + Forward(), Up());
}

glm::mat4 Transform::GetLocalMatrix() const
{
	glm::mat4 model(1.0f);

	model = glm::translate(model, position);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));

	model = glm::scale(model, scale);

	return model;
}

glm::mat4 Dut::Transform::GetPosRotMatrix() const
{
	glm::mat4 model(1.0f);
	model = glm::translate(model, position);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;
}

void Dut::Transform::Decompose(glm::mat4 local)
{
	glm::vec3 pos, scale, skew;
	glm::vec4 pers;
	glm::quat rot;
	glm::decompose(local, scale, rot, pos, skew, pers);

	this->position = pos;
	this->scale = scale;
	this->SetRotation(glm::conjugate(rot));
}

void Dut::Transform::SetPosition(const glm::vec3& pos)
{
	this->position = pos;

}

void Transform::RotateX(float d)
{
	rotation.x += d;
}

void Transform::RotateY(float d)
{
	rotation.y += d;
}

void Transform::RotateZ(float d)
{
	rotation.z += d;
}

void Transform::SetRotation(const glm::quat& q)
{
	glm::vec3 rot = glm::eulerAngles(q);

	rotation = rot;
	this->rotation *= 180.0f / glm::pi<float>();
}

void Transform::BackAt(glm::vec3 target, glm::vec3 worldUp)
{
	if (target == this->position)
	{
		return;
	}
	glm::mat4 view = glm::lookAtLH(position, target, worldUp);
	rotation = GetEulerAnglesFromRotationMatrix(glm::inverse(view));
}

void Transform::LookAt(glm::vec3 target, glm::vec3 worldUp)
{
	BackAt(target, worldUp);
}

glm::mat4 Transform::GetRotationMatrix()
{
	glm::mat4 model(1.0f);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;

}

glm::quat Dut::Transform::GetRoatationQuat()
{
	glm::mat4 local = GetRotationMatrix();
	glm::vec3 pos, scale, skew;
	glm::vec4 pers;
	glm::quat rot;
	glm::decompose(local, scale, rot, pos, skew, pers);

	return rot;
}

void Transform::Move(float dist, glm::vec3 f)
{
	position += dist * glm::normalize(f);
}

void Transform::Translate(float dist)
{
	this->position += this->Forward() * dist;
}

void Transform::MoveRight(float dist)
{
	this->position += this->Right() * dist;
}


void Transform::Rotate(glm::vec3 axis)
{
	RotateZ(axis.z);
	RotateX(axis.y);
	RotateY(axis.z);
}

Transform Dut::Transform::Slerp(const Transform& t1, const Transform& t2, float axis)
{
	Transform res;
	res.position = glm::mix(t1.position, t2.position, axis);
	res.rotation = glm::mix(t1.rotation, t2.rotation, axis);
	res.scale = glm::mix(t1.scale, t2.scale, axis);

	return res;
}