#include "assets/file_parsers/kitti_pose_parser.hpp"

#include <fstream>
#include <execution>
#include <glm/ext/matrix_transform.hpp>
#include "util/logger.hpp"


assets::kitti_pose_parser::parser_context::parser_context(
	store_type& m_store
) : m_store{ &m_store }
{
	m_buffer.reserve(128);
}

void assets::kitti_pose_parser::parser_context::reset()
{
	m_buffer.clear();
}

void assets::kitti_pose_parser::parser_context::operator()(lookup_type::const_pointer entry) noexcept
{
	const auto& [ filename, id ] = *entry;

	if (const auto e = parse_file(filename, m_buffer))
	{
		ztu::logger::warn("Could not load pose_list %: %.", filename, e.message());
		return;
	}

	m_store->insert(id, m_buffer);
}


std::error_code assets::kitti_pose_parser::prefetch(
	path_id_lookups& lookups
) {
	return {};
}

std::error_code assets::kitti_pose_parser::parse(
	const path_id_lookups& lookups,
	store_type& store,
	bool
) {
	namespace fs = std::filesystem;

	auto path_buffer = fs::path{};
	auto in = std::ifstream{}; // TODO disable exceptions (for other loaders as well)

	m_path_buffer.clear();
	lookups.pose_lists.by_extension(".glsl", m_path_buffer);

	std::for_each(
		std::execution::parallel_unsequenced_policy{},
		m_path_buffer.begin(),
		m_path_buffer.end(),
		parser_context{ store }
	);

	return {};
}


std::error_code assets::kitti_pose_parser::parse_file(
	const std::filesystem::path& filename,
	data_type &poses
) {

	auto in = std::ifstream{};

	in.open(filename);
	if (not in.is_open())
	{
		return std::make_error_code(std::errc::no_such_file_or_directory);
	}

	in >> std::skipws;

	while (in.peek() != std::ifstream::traits_type::eof())
	{
		auto pose = glm::identity<pose_data>();

		if (const auto error = parse_pose(in, pose))
		{
			ztu::logger::error(
				"Error occurred while parsing kitti pose file %: [%] %",
				filename,
				error.category().name(),
				error.message()
			);
			continue;
		}

		poses.emplace_back(pose);
	}

	in.close();

	return {};
}

std::error_code assets::kitti_pose_parser::parse_pose(
	std::ifstream& in,
	pose_data& pose
) {
	for (pose_data::length_type row{}; row != 3; ++row)
	{
		for (pose_data::length_type col{}; col != 4; ++col)
		{
			if (not (in >> pose[row][col]))
			{
				return std::make_error_code(std::errc::result_out_of_range);
			}
		}
	}

	return {};
}