/*
 * Copyright (C) 2023  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 <engine/ai/navmesh.hpp>
#include <engine/geom/closest-point.hpp>
#include <engine/geom/coordinates.hpp>
#include <engine/math/quaternion.hpp>
#include <iterator>
#include <engine/debug/log.hpp>

namespace ai {

navmesh_traversal traverse_navmesh(const geom::brep_mesh& mesh, geom::brep_face* face, geom::ray<float, 3> ray, float distance)
{
	// Get vertex positions and face normals
	const auto& vertex_positions = mesh.vertices().attributes().at<math::fvec3>("position");
	const auto& face_normals = mesh.faces().attributes().at<math::fvec3>("normal");
	
	// Init traversal result
	navmesh_traversal traversal;
	traversal.edge = nullptr;
	
	geom::triangle_region region;
	
	auto target_point = ray.extrapolate(distance);
	
	math::fvec3 closest_point;
	math::fvec3 traversal_direction = ray.direction;
	
	geom::brep_edge* previous_closest_edge{};
	
	do
	{
		// Get vertex positions of face
		auto loop_it = face->loops().begin();
		const auto& a = vertex_positions[loop_it->vertex()->index()];
		const auto& b = vertex_positions[(++loop_it)->vertex()->index()];
		const auto& c = vertex_positions[(++loop_it)->vertex()->index()];
		
		// Find closest point on face to target point
		std::tie(closest_point, region) = geom::closest_point(a, b, c, target_point);
		
		// If point is on the face
		if (geom::is_face_region(region))
		{
			// Traversal complete
			break;
		}
		
		geom::brep_loop* closest_loop;
		
		// If point is on an edge
		if (geom::is_edge_region(region))
		{
			// Get index of the edge
			const auto edge_index = geom::edge_index(region);
			
			// Get pointer to the edge's loop
			auto loop_it = face->loops().begin();
			std::advance(loop_it, edge_index);
			closest_loop = *loop_it;
			
			// If edge is a boundary edge
			if (closest_loop->edge()->loops().size() == 1)
			{
				// Abort traversal
				traversal.edge = closest_loop->edge();
				break;
			}
		}
		else
		{
			// Point is on a vertex, get index of vertex on which point lies
			const auto vertex_index = geom::vertex_index(region);
			
			// Get pointer to loop originating at the vertex
			auto loop_it = face->loops().begin();
			std::advance(loop_it, vertex_index);
			geom::brep_loop* loop = *loop_it;
			
			// If previous loop edge is a boundary edge
			if (loop->previous()->edge()->loops().size() == 1)
			{
				// If current loop edge is also a boundary edge
				if (loop->edge()->loops().size() == 1)
				{
					// Abort traversal
					traversal.edge = loop->edge();
					break;
				}
				
				// Select current loop
				closest_loop = loop;
			}
			// If current loop edge is a boundary edge
			else if (loop->edge()->loops().size() == 1)
			{
				// Select previous loop
				closest_loop = loop->previous();
			}
			else
			// Neither loop edge is a boundary edge
			{
				// Calculate direction of current loop edge
				const auto current_direction = math::normalize
				(
					vertex_positions[loop->next()->vertex()->index()] -
					vertex_positions[loop->vertex()->index()]
				);
				
				// Calculate direction of previous loop edge
				const auto previous_direction = math::normalize
				(
					vertex_positions[loop->vertex()->index()] -
					vertex_positions[loop->previous()->vertex()->index()]
				);
				
				// Select loop with minimal angle between edge and traversal direction
				if (std::abs(math::dot(traversal_direction, current_direction)) <
					std::abs(math::dot(traversal_direction, previous_direction)))
				{
					closest_loop = loop;
				}
				else
				{
					closest_loop = loop->previous();
				}
			}
		}
		
		// Get edge of closest loop
		geom::brep_edge* closest_edge = closest_loop->edge();
		
		// If closest edge is previous closest edge
		if (closest_edge == previous_closest_edge)
		{
			// Abort traversal
			traversal.edge = closest_edge;
			break;
		}
		
		// Remember closest edge to prevent infinite loops
		previous_closest_edge = closest_edge;
		
		// Find a loop and face that shares the closest edge
		geom::brep_loop* symmetric_loop = closest_edge->loops().front();
		if (symmetric_loop == closest_loop)
		{
			symmetric_loop = closest_edge->loops().back();
		}
		geom::brep_face* symmetric_face = symmetric_loop->face();
		
		// Find quaternion representing rotation from normal of first face to normal of second face
		const auto& n0 = face_normals[face->index()];
		const auto& n1 = face_normals[symmetric_face->index()];
		const auto rotation = math::rotation(n0, n1);
		
		// Rotate target point
		target_point = rotation * (target_point - closest_point) + closest_point;
		
		// Rotate traversal direction
		traversal_direction = rotation * traversal_direction;
		
		// Move to next face
		face = symmetric_face;
	}
	while (true);
	
	traversal.face = face;
	traversal.target_point = target_point;
	traversal.closest_point = closest_point;
	
	auto loop_it = face->loops().begin();
	const auto& a = vertex_positions[loop_it->vertex()->index()];
	const auto& b = vertex_positions[(++loop_it)->vertex()->index()];
	const auto& c = vertex_positions[(++loop_it)->vertex()->index()];
	traversal.barycentric = geom::cartesian_to_barycentric(traversal.closest_point, a, b, c);
	
	return traversal;
}

} // namespace ai