#include "assets/data_loaders/kitti_pose_loader.hpp"

#include "assets/dynamic_read_buffers/dynamic_pose_buffer.hpp"
#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_data_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)
	auto pose_buffer = dynamic_pose_buffer{};

	pose_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.txt";
		preprocess_file();
	}

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

	for (const auto filename : processed_filenames)
	{
		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, pose_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(pose_buffer);

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

			id_lookup.poses.emplace(directory, i, id);
		}

		in.close();
	}
}

void kitti_pose_loader::load(
	const ztu::string_list& directories,
	dynamic_pose_store& store,
	pose_prefetch_lookup& id_lookup
) {

	auto filename_buffer = std::filesystem::path{};
	auto in = std::ifstream{}; // TODO disable exceptions (for other loaders as well)
	auto pose_buffer = dynamic_pose_buffer{};

	pose_buffer = glm::identity<glm::mat4>();

	for (const auto directory : directories)
	{
		filename_buffer = directory;
		filename_buffer /= "pose.txt";

		in.open(filename_buffer);
		if (not in.is_open())
		{
			ztu::logger::error("Cannot open kitti pose file %", filename_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, pose_buffer))
			{
				ztu::logger::error(
					"Error occurred while parsing kitti pose % in file %: [%] %",
					i,
					filename_buffer,
					error.category().name(),
					error.message()
				);
				continue;
			}

			const auto id = store.add(pose_buffer);
			id_lookup.emplace(directory, i, id);
		}

		in.close();
	}
}