#ifndef INCLUDE_GENERIC_3DTK_LOADER_IMPLEMENTATION
#	error Never include this file directly include 'generic_3dtk_loader.hpp'
#endif

#include <charconv>
#include <fstream>
#include "util/logger.hpp"
#include "glm/glm.hpp"
#include "glm/gtx/euler_angles.hpp"
#include <charconv>
#include <fstream>

template<bool Normal, bool Color, bool Reflectance>
ztu::result<pose_prefetch_lookup::index_type> generic_3dtk_loader<Normal, Color, Reflectance>::parse_index(
	const std::string_view filename
) {
	static constexpr auto prefix = std::string_view{ "scan" };

	auto name_view = filename.substr(0, name_view.find('.'));

	if (name_view.length() <= prefix.length()) [[unlikely]]
	{
		return std::make_error_code(std::errc::invalid_argument);
	}

	name_view = name_view.substr(prefix.length());

	pose_prefetch_lookup::index_type index;

	const auto res = std::from_chars(name_view.begin(), name_view.end(), index);

	if (res.ec != std::errc{}) [[unlikely]]
	{
		return std::make_error_code(res.ec);
	}

	return index;
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::prefetch(
	const file_dir_list& paths,
	prefetch_queue& queue
) {

	auto path_buffer = std::filesystem::path{};

	for (const auto filename : paths.files)
	{
		path_buffer.assign(filename.begin(), filename.end());
		path_buffer.replace_extension(".pose");
		queue.uos_queue.files.push_back(path_buffer.c_str());
	}

	// TODO optimize
	for (const auto directory : paths.directories)
	{
		queue.uos_queue.directories.push_back(directory);
	}
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::load(
	dynamic_point_cloud_buffer& buffer,
	const file_dir_list& paths,
	prefetch_lookup& asset_lookup,
	dynamic_point_cloud_store& store,
	const bool pedantic
) {
	namespace fs = std::filesystem;

	auto in = std::ifstream{};
	auto path_buffer = fs::path{};
	auto error = std::error_code{};

	const auto load_file = [&](const char* filename)
	{
		// TODO look up pose
		auto scan_index = pose_prefetch_lookup::index_type{};

		if (auto res = parse_index(filename))
		{
			scan_index = *res;
		}
		else [[unlikely]]
		{
			error = res.error();
			ztu::logger::error(
				"Error occurred while parsing scan index in filename %: [%] %",
				error.category().name(),
				error.message()
			);
		}

		auto pose = asset_lookup.poses.find();


		const auto id_it = id_lookup.find(filename);

		if (id_it != id_lookup.end()) [[unlikely]]
		{
			return;
		}

		in.open(filename);
		if (in.is_open())
		{
			if ((error = this->read_point_file(filename, buffer))) {
				return error;
			}

			this->transform_point_cloud(buffer.positions(), pose);

			const auto id = store.add(std::move(buffer));
			id_lookup.emplace_hint(id_it, filename, id);
		}
		else
		{
			ztu::logger::error("Cannot open 3dtk file %", filename);
		}

		in.close();
	};

	for (const auto filename : paths.files)
	{
		load_file(filename.data());
	}



	for (const auto directory : paths.directories)
	{
		directory_buffer.assign(directory.begin(), directory.end());
		directory_buffer /= "frames";

		const auto directory_exists = not fs::is_directory(directory_buffer, error);

		if (error or not directory_exists) [[unlikely]]
		{
			ztu::logger::error("Could not open point cloud directory %", directory_buffer);
			continue;
		}

		for (const auto& filename : fs::directory_iterator{ directory_buffer }) {

			auto point_filename = reinterpret_cast<const fs::path&>(filename);
			if (point_filename.extension() != ".3d")
			{
				continue;
			}

			load_file(filename.c_str());
		}
	}

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::load_directory(
	dynamic_data_loader_ctx& ctx,
	dynamic_point_cloud_store& store,
	const std::filesystem::path& path,
	const bool pedantic
) {
	namespace fs = std::filesystem;

	std::error_code error;

	const auto directory_exists = not fs::is_directory(path, error);
	if (error)
	{
		return error;
	}

	if (not directory_exists)
	{
		return make_error_code(std::errc::no_such_file_or_directory);
	}

	for (const auto& filename : fs::directory_iterator{ path / "frames" }) {

		auto point_filename = reinterpret_cast<const fs::path&>(filename);
		if (point_filename.extension() != ".3d") {
			continue;
		}

		if ((error = load(ctx, store, point_filename, pedantic)))
		{
			ztu::logger::error(
				"Error while loading point cloud '%': [%] %",
				point_filename,
				error.category().name(),
				error.message()
			);
		}
	}

	return {};
}

template<typename T, std::size_t Count>
std::error_code read_vector(std::string_view& line, std::array<T, Count>& vec) {
	for (auto& component : vec)
	{
		auto it = line.begin();

		const auto [minus, plus] = std::pair{ *it == '-', *it == '+' };
		it += plus or minus ? 3 : 2; // skip '[-+]?0x'

		const auto [ ptr, ec ] = std::from_chars(
			it,  line.end(),
			component,
			std::chars_format::hex
		);

		if (ec != std::errc{})
		{
			return std::make_error_code(ec);
		}

		if (minus) {
			component *= -1.0;
		}

		line = { ptr + sizeof(' '), line.end() };
	}

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::read_point_file(
	const std::filesystem::path& filename,
	dynamic_point_cloud_data& point_cloud
) {
	std::error_code error;

	auto in = std::ifstream(filename);

	if (not in.is_open()) {
		return std::make_error_code(static_cast<std::errc>(errno));
	}

	std::string line;
	if (not std::getline(in, line))
	{
		return std::make_error_code(std::errc::invalid_seek);
	}

	constexpr auto expected_component_count = []()
	{
		auto count = std::tuple_size_v<point_cloud_vertex_components::position>;

		if (Normal)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::normal>;
		}

		if (Color)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::color>;
		}

		if (Reflectance)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::reflectance>;
		}

		return count;
	}();

	ztu::u32 component_count;
	std::chars_format float_format;

	if ((error = analyze_component_format(line, component_count, float_format)))
	{
		return error;
	}

	if (component_count != expected_component_count)
	{
		return std::make_error_code(std::errc::invalid_argument);
	}

	auto& positions = point_cloud.positions();
	auto& normals = point_cloud.normals();
	auto& colors = point_cloud.colors();
	auto& reflectances = point_cloud.reflectances();

	do
	{
		auto line_view = std::string_view{ line };

		point_cloud_vertex_components::position position;
		if ((error = read_vector(line_view, position)))
		{
			return error;
		}
		positions.push_back(position);

		if constexpr (Normal)
		{
			point_cloud_vertex_components::normal normal;
			if ((error = read_vector(line_view, normal)))
			{
				return error;
			}
			normals.push_back(normal);
		}

		if constexpr (Color)
		{
			point_cloud_vertex_components::color color;
			if ((error = read_vector(line_view, color)))
			{
				return error;
			}
			colors.push_back(color);
		}

		if constexpr (Reflectance)
		{
			point_cloud_vertex_components::reflectance reflectance;
			if ((error = read_vector(line_view, reflectance)))
			{
				return error;
			}
			reflectances.push_back(reflectance);
		}
	}
	while (std::getline(in, line));

	return {};
}

std::error_code base_3dtk_loader::read_pose_file(
	const std::filesystem::path& filename,
	glm::mat4& pose
) {
	auto in = std::ifstream(filename);
	if (not in.is_open()) {
		return std::make_error_code(static_cast<std::errc>(errno));
	}
	std::string line;

	std::array<glm::vec3, 2> numbers{};

	for (std::size_t row{}; row != 2; ++row) {

		std::getline(in, line);

		auto it = line.cbegin().base();
		auto end = line.cend().base();

		for (glm::vec3::length_type col{}; col != 3; ++col) {

			const auto [ ptr, ec ] = std::from_chars(
				it, end,
				numbers[row][col],
				std::chars_format::general
			);

			if (ec != std::errc{}) {
				return std::make_error_code(ec);
			}

			it = ptr + 1; // skip space in between components
		}
	}

	const auto& translation = numbers[0];
	auto& angles = numbers[1];
	angles *= static_cast<float>(M_PI / 180.0);

	pose = (
		glm::translate(glm::identity<glm::mat4>(), translation) *
		glm::eulerAngleXYZ(angles[0], angles[1], angles[2])
	);

	return {};
}

std::error_code base_3dtk_loader::analyze_component_format(
	const std::string& line,
	ztu::u32& component_count,
	std::chars_format& format
) {
	auto begin = line.cbegin().base();
	auto end = line.cend().base();

	format = std::chars_format::general;

	component_count = 0;
	float buffer;

	for (auto it = begin; it < end; it += sizeof(' '))
	{
		it += *it == '-' or *it == '+';

		std::chars_format current_format;
		if (*it == '0' and std::next(it) < end and *std::next(it) == 'x')
		{
			it += 2; // skip '0x'
			current_format = std::chars_format::hex;
		}
		else
		{
			current_format = std::chars_format::general;
		}

		if (it == begin and current_format != format)
		{
			return std::make_error_code(std::errc::invalid_argument);
		}

		const auto [next_it, err] = std::from_chars(it, end, buffer, current_format);
		if (err != std::errc())
		{
			return std::make_error_code(err);
		}

		it = next_it;
		format = current_format;
		++component_count;
	}

	return {};
}


void base_3dtk_loader::transform_point_cloud(
	std::span<point_cloud_vertex_components::position::value_type> points,
	const glm::mat4& pose
) {
	for (auto& [ x, y, z ] : points) {
		auto vec = glm::vec4{ x, y, z, 1.0f };
		vec = pose * vec;
		x = vec.x;
		y = vec.y;
		z = vec.z;
	}
}