/*
 * Copyright (C) 2021  Christopher J. Howard
 *
 * This file is part of Antkeeper source code.
 *
 * Antkeeper source code is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Antkeeper source code is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "constraint.hpp"
#include "entity/components/constraint-stack.hpp"
#include "entity/components/constraints/constraints.hpp"
#include "entity/components/transform.hpp"

namespace entity {
namespace system {

constraint::constraint(entity::registry& registry):
	updatable(registry)
{}

void constraint::update(double t, double dt)
{
	// For each entity with transform and constraint stack components
	registry.view<component::transform, component::constraint_stack>().each(
		[&](entity::id transform_eid, auto& transform, auto& stack)
		{
			// Get entity ID of first constraint
			entity::id constraint_eid = stack.head;
			
			// Consecutively apply constraints
			while (constraint_eid != entt::null)
			{
				// Invalid constraint
				if (!registry.has<component::constraint_stack_node>(constraint_eid))
					break;
				
				// Get constraint stack node of this constraint
				const auto& node = registry.get<component::constraint_stack_node>(constraint_eid);
				
				// Apply constraint if enabled
				if (node.active)
					handle_constraint(transform, constraint_eid, static_cast<float>(dt));
				
				// Get entity ID of next constraint
				constraint_eid = node.next;
			}
		});
}

void constraint::handle_constraint(component::transform& transform, entity::id constraint_eid, float dt)
{
	if (registry.has<component::constraint::copy_translation>(constraint_eid))
		handle_copy_translation_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::copy_rotation>(constraint_eid))
		handle_copy_rotation_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::copy_scale>(constraint_eid))
		handle_copy_scale_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::copy_transform>(constraint_eid))
		handle_copy_transform_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::track_to>(constraint_eid))
		handle_track_to_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::three_dof>(constraint_eid))
		handle_three_dof_constraint(transform, constraint_eid);
	else if (registry.has<component::constraint::spring_to>(constraint_eid))
		handle_spring_to_constraint(transform, constraint_eid, dt);
}

void constraint::handle_copy_translation_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::copy_translation>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		const auto& target_translation = registry.get<component::transform>(params.target).world.translation;
		
		if (params.offset)
		{
			if (params.copy_x)
				transform.world.translation.x += (params.invert_x) ? -target_translation.x : target_translation.x;
			if (params.copy_y)
				transform.world.translation.y += (params.invert_y) ? -target_translation.y : target_translation.y;
			if (params.copy_z)
				transform.world.translation.z += (params.invert_z) ? -target_translation.z : target_translation.z;
		}
		else
		{
			if (params.copy_x)
				transform.world.translation.x = (params.invert_x) ? -target_translation.x : target_translation.x;
			if (params.copy_y)
				transform.world.translation.y = (params.invert_y) ? -target_translation.y : target_translation.y;
			if (params.copy_z)
				transform.world.translation.z = (params.invert_z) ? -target_translation.z : target_translation.z;
		}
		
	}
}

void constraint::handle_copy_rotation_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::copy_rotation>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		const auto& target_rotation = registry.get<component::transform>(params.target).world.rotation;
		transform.world.rotation = target_rotation;
	}
}

void constraint::handle_copy_scale_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::copy_scale>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		const auto& target_scale = registry.get<component::transform>(params.target).world.scale;
		
		if (params.copy_x)
			transform.world.scale.x = target_scale.x;
		if (params.copy_y)
			transform.world.scale.y = target_scale.y;
		if (params.copy_z)
			transform.world.scale.z = target_scale.z;
	}
}

void constraint::handle_copy_transform_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::copy_transform>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		const auto& target_transform = registry.get<component::transform>(params.target).world;
		transform.world = target_transform;
	}
}

void constraint::handle_track_to_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::track_to>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		const auto& target_transform = registry.get<component::transform>(params.target).world;
		transform.world.rotation = math::look_rotation(math::normalize(math::sub(target_transform.translation, transform.world.translation)), params.up);
	}
}

void constraint::handle_three_dof_constraint(component::transform& transform, entity::id constraint_eid)
{
	const auto& params = registry.get<component::constraint::three_dof>(constraint_eid);
	
	const math::quaternion<float> yaw = math::angle_axis(params.yaw, {0.0f, 1.0f, 0.0f});
	const math::quaternion<float> pitch = math::angle_axis(params.pitch, {-1.0f, 0.0f, 0.0f});
	const math::quaternion<float> roll = math::angle_axis(params.roll, {0.0f, 0.0f, -1.0f});
	transform.local.rotation = math::normalize(yaw * pitch * roll);
}

void constraint::handle_spring_to_constraint(component::transform& transform, entity::id constraint_eid, float dt)
{
	auto& params = registry.get<component::constraint::spring_to>(constraint_eid);
	
	if (this->registry.has<component::transform>(params.target))
	{
		// Get transform of target entity
		const auto& target_transform = registry.get<component::transform>(params.target).world;
		
		// Spring translation
		if (params.spring_translation)
		{
			// Update translation spring target
			params.translation.x1 = target_transform.translation;
			
			// Solve translation spring
			solve_numeric_spring<float3, float>(params.translation, dt);
			
			// Update transform translation
			transform.world.translation = params.translation.x0;
		}
		
		// Spring rotation
		if (params.spring_rotation)
		{
			// Update rotation spring target
			params.rotation.x1 = {target_transform.rotation.w, target_transform.rotation.x, target_transform.rotation.y, target_transform.rotation.z};
			
			// Solve rotation spring
			solve_numeric_spring<float4, float>(params.rotation, dt);
			
			// Update transform rotation
			transform.world.rotation = math::normalize(math::quaternion<float>{params.rotation.x0[0], params.rotation.x0[1], params.rotation.x0[2], params.rotation.x0[3]});
		}
	}
}

} // namespace system
} // namespace entity