#include "precompile.h"
#include "handle.h"
#include "primitive.h"

//public

void Handle::Reset()
{
	m_rotation_angle_2d = 0;
	m_rotation_primal_quat_3d.setIdentity();
	m_translation.setZero();
}

bool Handle::Select(std::vector<glm::vec3> ray)
{
	if (m_mode == MODE_2D)
	{
		glm::vec3 p = ray[0]; p.z = 0.0f;
		if (insidebb(p))
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		// TODO: 3d mode
	}

	return false;
}

ScalarType Handle::BoundingBoxSize()
{
	glm::vec3 diff = m_bounding_box_max-m_bounding_box_min;

	return (diff.x*diff.y + diff.x*diff.z + diff.y*diff.z);
}

glm::vec3 Handle::GetCoM()
{
	VectorX t = m_original_centor_of_mass + m_translation;
	if (m_mode == MODE_2D)
	{
		return glm::vec3(t.x(), t.y(), 0.0f);
	}
	else
	{
		return glm::vec3(t.x(), t.y(), t.z());
	}
}

glm::vec3 Handle::GetLocalCoM()
{
	if (m_mode == MODE_2D)
	{
		return glm::vec3(m_original_centor_of_mass.x(), m_original_centor_of_mass.y(), 0.0f);
	}
	else
	{
		return glm::vec3(m_original_centor_of_mass.x(), m_original_centor_of_mass.y(), m_original_centor_of_mass.z());
	}
}

void Handle::MoveTemporary(const glm::vec3& trans)
{
	if (m_mode == MODE_2D)
	{
		m_temp_translation = EigenVector2(trans.x, trans.y);
	}
	else
	{
		m_temp_translation = GLM2Eigen(trans);
	}
	update();
}

void Handle::MoveFinalize()
{
	m_translation = m_temp_translation;
	m_temp_translation.setZero();
	update();
}

void Handle::RotateToValue(ScalarType theta)
{
	if (m_mode == MODE_2D)
	{
		m_rotation_angle_2d = theta;
	}
	update();
}

void Handle::RotateTemporary(const glm::vec3& axis, const float& theta)
{
	if (m_mode == MODE_2D)
	{
		m_temp_rotation_angle_2d = theta;
	}
	else
	{
		// TODO: 3d mode
	}
	update();
}

void Handle::RotateFinalize()
{
	if (m_mode == MODE_2D)
	{
		m_rotation_angle_2d = m_temp_rotation_angle_2d;
		m_temp_rotation_angle_2d = 0.0;
	}
	else
	{

		// TODO: 3d mode
	}
	update();
}

void Handle::ChangeTranslationOnXAxis(ScalarType t)
{
	m_translation(0) += t;

	update();
}

void Handle::RotateExtra(ScalarType theta)
{
	if (m_mode == MODE_2D)
	{
		m_rotation_angle_2d += theta;
	}
	else
	{

		// TODO: 3d mode
	}
	update();
}

void Handle::Draw(const VBO& vbos, bool selected)
{
	Cube cube(DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS);
	if (selected)
	{
		cube.change_color(glm::vec3(1.2f, 1.2f, 0.0f)); // highlight using yellow
	}
	else
	{
		cube.change_color(m_color);
	}
	glm::vec3 m_x_i;
	for (unsigned int i = 0; i * 2 < m_original_x.size(); ++i)
	{
		m_x_i.x = m_x(2*i);
		m_x_i.y = m_x(2*i+1);
		m_x_i.z = 0;
		cube.move_to(m_x_i);
		cube.Draw(vbos);
	}
}

ScalarType Handle::HandleHandleDifference(const Handle& other)
{
	VectorX t1 = this->m_translation;
	VectorX t2 = other.m_translation;

	if (m_mode == MODE_2D)
	{
		ScalarType theta1 = this->m_rotation_angle_2d;
		ScalarType theta2 = other.m_rotation_angle_2d;

		ScalarType d1 = (t1-t2).norm();
		ScalarType d2 = std::abs(std::tan(0.5*(theta1-theta2)));

		return d1 + d2;
	}
	else
	{
		// TODO: 3d mode

		return 0;
	}
}

// protected:
void Handle::init()
{
	// dimension
	m_dim = (m_mode == MODE_2D) ? 2: 3;

	// vertices
	m_x = m_original_x;

	// transformation
	m_rotation_angle_2d = 0.0;
	m_temp_rotation_angle_2d = 0.0;
	m_rotation_primal_quat_3d.setIdentity();
	m_temp_rotation_primal_quat_3d.setIdentity();
	m_translation.resize(m_dim); m_translation.setZero();
	m_temp_translation.resize(m_dim); m_temp_translation.setZero();
	m_T.resize(m_dim+1, m_dim+1); m_T.setIdentity();

	// CoM
	m_original_centor_of_mass.resize(m_dim); m_original_centor_of_mass.setZero();
	unsigned int size = m_original_x.size()/m_dim;
	if (size != 0)
	{
		for (unsigned int i = 0; i != size; ++i)
		{
			for (unsigned int j = 0; j != m_dim; ++j)
			{
				m_original_centor_of_mass(j) += m_original_x(m_dim*i+j);
			}
		}
		m_original_centor_of_mass = m_original_centor_of_mass / size;
	}

	// update transformation matrix
	update();
}

void Handle::cleanup()
{
	m_indices.clear();
}

bool Handle::insidebb(const glm::vec3& p)
{
	if (p.x < m_bounding_box_max.x && p.y < m_bounding_box_max.y && p.z < m_bounding_box_max.z &&
		p.x > m_bounding_box_min.x && p.y > m_bounding_box_min.y && p.z > m_bounding_box_min.z)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void Handle::updateTransformationMatrix()
{
	if (m_mode == MODE_2D)
	{
		float theta;
		if (abs(m_temp_rotation_angle_2d) > EPSILON)
		{
			theta = m_temp_rotation_angle_2d;
		}
		else
		{
			theta = m_rotation_angle_2d;
		}
		VectorX translation = m_translation;
		if (m_temp_translation.squaredNorm() > EPSILON)
		{
			translation = m_temp_translation;
		}

		EigenMatrix3 R;
		R.setIdentity();
		R(0, 0) = cos(theta); R(0, 1) = -sin(theta); R(1, 0) = sin(theta); R(1, 1) = cos(theta);
		EigenMatrix3 T_to_origin, T_to_center, T_translate;
		T_to_origin.setIdentity(); T_to_center.setIdentity(); T_translate.setIdentity();
		T_to_origin.block<2, 1>(0, 2) = -m_original_centor_of_mass;
		T_to_center.block<2, 1>(0, 2) = m_original_centor_of_mass;
		T_translate.block<2, 1>(0, 2) = translation;
		//T_to_origin(0, 2) = -m_center_of_mass.x;
		//T_to_origin(1, 2) = -m_center_of_mass.y;
		//T_to_center(0, 2) = m_center_of_mass.x;
		//T_to_center(1, 2) = m_center_of_mass.y;
		//T_translate(0, 2) = m_translation_2d.x();
		//T_translate(1, 2) = m_translation_2d.y();

		m_T = T_translate * T_to_center * R * T_to_origin;
	}
	else
	{
		// TODO: 3d case update transformation matrix;
	}
}

void Handle::update()
{
	updateTransformationMatrix();

	if (m_mode == MODE_2D)
	{
		for (unsigned int i = 0; i * 2 < m_original_x.size(); ++i)
		{
			EigenVector3 v(m_original_x(2*i), m_original_x(2*i+1), 1.0);
			v = m_T * v;

			m_x.block<2, 1>(2*i, 0) = v.block<2, 1>(0, 0);
		}

		// update bounding box
		m_bounding_box_min = glm::vec3(INFINITY, INFINITY, INFINITY);
		m_bounding_box_max = -m_bounding_box_min;
		glm::vec3 m_x_i;
		for (unsigned int i = 0; i * 2 < m_original_x.size(); ++i)
		{
			m_x_i.x = m_x(2*i);
			m_x_i.y = m_x(2*i+1);
			m_x_i.z = 0;
			m_bounding_box_min = glm::min(m_bounding_box_min, m_x_i);
			m_bounding_box_max = glm::max(m_bounding_box_max, m_x_i);
		}
		// selection slackness
		m_bounding_box_min -= glm::vec3(DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS);
		m_bounding_box_max += glm::vec3(DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS, DEFAULT_SELECTION_RADIUS);
	}
	else
	{
		// TODO: 3D mode
	}
}
