#pragma once

#include "../Common/Casts.hpp"
#include "../Math/Ray.hpp"
#include "Position.hpp"

namespace MC::World::VoxelTraversal {

// Amanatides, John, and Andrew Woo. 1987.
// "A Fast Voxel Traversal Algorithm for Ray Tracing."
// https://doi.org/10.2312/EGTP.19871000.
template <typename P>
Position::BlockWorld traverse(Ray ray, Real max_distance, P&& predicate) {
    // Find the voxel grid cell containing the origin of the ray.
    Position::BlockWorld block_pos = Position::World(ray.origin).round_to_block();
    Position::BlockWorldOffset const step = {
        Math::sign(ray.direction.x()),
        Math::sign(ray.direction.y()),
        Math::sign(ray.direction.z()),
    };

    Position::WorldOffset t_max = {
        (TO(Real, block_pos.x()) + TO(Real, step.x() > 0) - ray.origin.x()) / ray.direction.x(),
        (TO(Real, block_pos.y()) + TO(Real, step.y() > 0) - ray.origin.y()) / ray.direction.y(),
        (TO(Real, block_pos.z()) + TO(Real, step.z() > 0) - ray.origin.z()) / ray.direction.z()
    };

    Position::WorldOffset const t_delta = {
        TO(Real, step.x()) / ray.direction.x(),
        TO(Real, step.y()) / ray.direction.y(),
        TO(Real, step.z()) / ray.direction.z()
    };

    while (!predicate(block_pos)) {
        // TODO: Calculate distance exactly.
        if (ray.origin.distance(Vec3(block_pos)) > max_distance) return {};

        if (t_max.x() < t_max.y()) {
            if (t_max.x() < t_max.z()) {
                block_pos.x() += step.x();
                t_max.x() += t_delta.x();
            } else {
                block_pos.z() += step.z();
                t_max.z() += t_delta.z();
            }
        } else {
            if (t_max.y() < t_max.z()) {
                block_pos.y() += step.y();
                t_max.y() += t_delta.y();
            } else {
                block_pos.z() += step.z();
                t_max.z() += t_delta.z();
            }
        }
    }

    return block_pos;
}

}