#include "Poseable.h"

#include <glm/gtc/matrix_transform.hpp>

namespace charcoal
{
	Poseable::Poseable(
		const vec3& position,
		const vec3& forward,
		const vec3& up,
		const vec3& right
	)
		: m_orientation_matrix(
			vec4(right, 0.0f),
			vec4(up, 0.0f),
			vec4(forward, 0.0f),
			vec4(position, 1.0f)
		)
	{}

	void Poseable::reset_orientation()
	{
		m_orientation_matrix[0][0] = 1.0f;
		m_orientation_matrix[0][1] = 0.0f;
		m_orientation_matrix[0][2] = 0.0f;

		m_orientation_matrix[1][0] = 0.0f;
		m_orientation_matrix[1][1] = 1.0f;
		m_orientation_matrix[1][2] = 0.0f;

		m_orientation_matrix[2][0] = 0.0f;
		m_orientation_matrix[2][1] = 0.0f;
		m_orientation_matrix[2][2] = 1.0f;
	}

	void Poseable::update_position(const vec3& position)
	{
		m_orientation_matrix[3][0] = position.x;
		m_orientation_matrix[3][1] = position.y;
		m_orientation_matrix[3][2] = position.z;
	}

	void Poseable::update_orientation(const vec3& forward, const vec3& up, const vec3& right)
	{
		m_orientation_matrix[0][0] = right.x;
		m_orientation_matrix[0][1] = right.y;
		m_orientation_matrix[0][2] = right.z;

		m_orientation_matrix[1][0] = up.x;
		m_orientation_matrix[1][1] = up.y;
		m_orientation_matrix[1][2] = up.z;

		m_orientation_matrix[2][0] = forward.x;
		m_orientation_matrix[2][1] = forward.y;
		m_orientation_matrix[2][2] = forward.z;
	}

	void Poseable::translate(const vec3& translation)
	{
		m_orientation_matrix[3][0] += translation.x;
		m_orientation_matrix[3][1] += translation.y;
		m_orientation_matrix[3][2] += translation.z;
	}

	void Poseable::rotate(const vec3& axis, float angle)
	{
		mat4 rotation_matrix = glm::rotate(glm::mat4(1.0f), angle, axis);
		m_orientation_matrix[0] = rotation_matrix * m_orientation_matrix[0];
		m_orientation_matrix[1] = rotation_matrix * m_orientation_matrix[1];
		m_orientation_matrix[2] = rotation_matrix * m_orientation_matrix[2];
	}
}