#include "../../../include/assets/data_parsers"

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

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

	return {};
}


std::error_code kitti_pose_loader::prefetch(
	const file_dir_list& paths,
	prefetch_queue& queue
) {
	// Nothing to be done here
}

std::error_code kitti_pose_loader::load(
	dynamic_pose_buffer& buffer,
	const file_dir_list& paths,
	prefetch_lookup& id_lookup,
	dynamic_shader_source_store& store,
	bool pedantic
) {
	namespace fs = std::filesystem;

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

	// Needed to initialize 4th row and col
	buffer = glm::identity<glm::mat4>();

	auto processed_filenames = ztu::string_list{};

	processed_filenames.reserve(
	paths.files.character_count() +
		paths.directories.character_count() +
		paths.directories.size() * pose_filename.size(),
	paths.files.size() + paths.directories.size()
	);

	const auto preprocess_file = [&]()
	{
		if (not fs::is_regular_file(path_buffer))
		{
			ztu::logger::error("Kitti pose file does not exist: %", path_buffer);
			return;
		}
		processed_filenames.push_back(path_buffer.c_str());
	};

	for (const auto directory : paths.directories) {
		path_buffer.assign(directory.begin(), directory.end());
		path_buffer /= pose_filename;
		preprocess_file();
	}

	for (const auto file : paths.files) {
		path_buffer.assign(file.begin(), file.end());
		preprocess_file();
	}

	for (const auto filename : processed_filenames)
	{
		// TODO if (not) removing the path separator creates issues.
		const auto directory = filename.substr(0, filename.length() - pose_filename.length());

		auto [ dir_it, dir_match ] = id_lookup.poses.find_directory(directory);

		if (not dir_match) [[unlikely]]
		{
			dir_it = id_lookup.poses.emplace_dir(dir_it, directory);
		}

		in.open(filename.data()); // Safe because string list adds null terminator
		if (not in.is_open())
		{
			ztu::logger::error("Cannot open kitti pose file %", path_buffer);
			continue;
		}

		in >> std::skipws;

		for (auto i = pose_prefetch_lookup::index_type{}; in.peek() != std::ifstream::traits_type::eof(); ++i)
		{
			if (const auto error = parse_pose(in, buffer))
			{
				ztu::logger::error(
					"Error occurred while parsing kitti pose % in file %: [%] %",
					i,
					path_buffer,
					error.category().name(),
					error.message()
				);
				continue;
			}

			const auto id = store.poses.add(buffer);
			id_lookup.poses.emplace_hint_dir(dir_it, i, id);
		}

		in.close();
	}
}