#include "Universe.hpp"
#include "../data/math.hpp"

using namespace world;
using namespace geometry;

bool Universe::move(glm::ifvec3 &pos, const glm::vec3 &vel, int density, float radius) const {
    const auto dir = glm::normalize(vel);
    const auto velocity = vel * glm::vec3(density);
    const auto from = pos * density + dir;
    const auto result = raycast(Ray(from, dir, glm::length(velocity) + radius));
    if (auto target = std::get_if<ray_target>(&result)) {
        const auto target_dist = from.dist(glm::ifvec3(target->offset + target->pos.second, density)) - radius;
        pos += vel * glm::vec3(target_dist / glm::length(vel));
        return true;
    } else if(auto out = std::get_if<ray_out_of_range>(&result)) {
        const auto out_dist = from.dist(glm::ifvec3(out->offset + glm::multiply(out->pos.second), density)) - radius - CHUNK_LENGTH;
        pos += vel * glm::vec3(out_dist / glm::length(vel));
        return true;
    }

    pos += vel;
    return false;
}
bool Universe::collide(const glm::ifvec3 &pos, const glm::vec3 &vel, int density, float radius) const {
    const auto dir = glm::normalize(vel);
    const auto velocity = vel * glm::vec3(density);
    const auto from = pos * density + dir;
    return !std::holds_alternative<std::monostate>(raycast(Ray(from, dir, glm::length(velocity) + radius)));
}