#include "shared/movement.hh"

namespace shared {
namespace movement {

// Move a player into the correct neighbour chunk if they go over a border.
static void shift_chunk(shared::player& player) noexcept {
    const bool g_x = (player.local_pos.x >= shared::world::chunk::WIDTH);
    const bool l_x = (player.local_pos.x < 0.0f);
    const bool g_z = (player.local_pos.z >= shared::world::chunk::WIDTH);
    const bool l_z = (player.local_pos.z < 0.0f);

    player.chunk_pos.x += g_x - l_x;
    player.local_pos.x += shared::world::chunk::WIDTH * (l_x - g_x);
    player.chunk_pos.z += g_z - l_z;
    player.local_pos.z += shared::world::chunk::WIDTH * (l_z - g_z);
}

constexpr float epsilon = 0.001f; // counteract arithmetic errors
constexpr float epsilon2 = epsilon + 2 * epsilon * epsilon;

bool intersect_aabbs(const aabb& a, const aabb& b) noexcept {
    if (a.max.x < b.min.x || a.min.x > b.max.x) {
        return false;
    }
    if (a.max.y < b.min.y || a.min.y > b.max.y) {
        return false;
    }
    if (a.max.z < b.min.z || a.min.z > b.max.z) {
        return false;
    }
    return true;
}

static glm::vec3 plane_to_normal(const int plane) {
    glm::vec3 normal{0.0f, 0.0f, 0.0f};
    normal[std::abs(plane) - 1] = std::signbit(plane) ? -1.0f : 1.0f;
    return normal;
}

std::optional<ray_aabb_ret> intersect_ray_aabb(const line& line,
                                               const aabb& aabb) noexcept {
    float tmin = -std::numeric_limits<float>::max();
    float tmax = std::numeric_limits<float>::max();

    int p = 0;
    for (int i = 0; i < 3; ++i) {
        if (std::abs(line.dir[i]) < epsilon) {

            // Ray is parallel to slab, no hit if origin not within slab.
            if (line.origin[i] < aabb.min[i] || line.origin[i] > aabb.max[i]) {
                return std::nullopt;
            }

        } else {
            // Intersection t value of ray with near and far plane of slab.
            const float ood = 1.0f / line.dir[i];
            float t1 = (aabb.min[i] - line.origin[i]) * ood;
            float t2 = (aabb.max[i] - line.origin[i]) * ood;

            if (t1 > t2) {
                std::swap(t1, t2);
            }
            auto old = tmin;
            tmin = std::max(tmin, t1);
            if (tmin != old) {
                p = (i + 1);
            }
            tmax = std::min(tmax, t2);
            if (tmin > tmax) {
                return std::nullopt;
            }
        }
    }

    if (tmin <= 0.0f) {
        return std::nullopt;
    }

    return ray_aabb_ret{.position = line.origin + line.dir * tmin,
                        .time = tmin,
                        .normal = plane_to_normal(p)};
}

std::optional<moving_aabb_ret>
intersect_moving_aabbs(const moving_aabb& a, const moving_aabb& b) noexcept {

    if (intersect_aabbs(a.aabb, b.aabb)) {
        return std::nullopt;
    }

    const glm::vec3 velocity = b.velocity - a.velocity;
    float tfirst = 0.0f;
    float tlast = 10.0f;
    int p = 0;

    for (int i = 0; i < 3; ++i) {
        if (velocity[i] < 0.0f) {
            if (b.aabb.max[i] < a.aabb.min[i]) {
                return std::nullopt;
            }
            if (a.aabb.max[i] < b.aabb.min[i]) {
                const auto old = tfirst;
                tfirst = std::max((a.aabb.max[i] - b.aabb.min[i]) / velocity[i],
                                  tfirst);
                if (tfirst != old) {
                    p = (i + 1);
                }
            }
            if (b.aabb.max[i] > a.aabb.min[i]) {
                tlast = std::min((a.aabb.min[i] - b.aabb.max[i]) / velocity[i],
                                 tlast);
            }
        } else if (velocity[i] > 0.0f) {
            if (b.aabb.min[i] > a.aabb.max[i]) {
                return std::nullopt;
            }
            if (b.aabb.max[i] < a.aabb.min[i]) {
                const auto old = tfirst;
                tfirst = std::max((a.aabb.min[i] - b.aabb.max[i]) / velocity[i],
                                  tfirst);
                if (tfirst != old) {
                    p = -(i + 1);
                }
            }
            if (a.aabb.max[i] > b.aabb.min[i]) {
                tlast = std::min((a.aabb.max[i] - b.aabb.min[i]) / velocity[i],
                                 tlast);
            }
        } else {
            if (b.aabb.max[i] < a.aabb.min[i] ||
                b.aabb.min[i] > a.aabb.max[i]) {
                return std::nullopt;
            }
        }
        if (tfirst > tlast) {
            return std::nullopt;
        }
    }
    /*
    if (tfirst < 0.0f || tfirst > 1.0f) {
        return std::nullopt;
    }
    */

    return moving_aabb_ret{.time = tfirst, .normal = plane_to_normal(p)};
}

static std::optional<shared::world::block>
get_ground(const aabb& player_aabb, const std::vector<blockinfo> blockinfos,
           bool (*delimit)(const enum shared::world::block::type) =
               shared::world::block::is_tangible) noexcept {

    const moving_aabb player_moving_aabb{.aabb = player_aabb,
                                         .velocity = {0.0f, -1.0f, 0.0f}};
    for (const auto& block_aabb : blockinfos) {

        if (!delimit(block_aabb.block)) {
            continue;
        }

        const struct moving_aabb block_moving_aabb {
            .aabb = block_aabb.aabb, .velocity = { 0.0f, 0.0f, 0.0f }
        };

        if (const auto intersect =
                intersect_moving_aabbs(player_moving_aabb, block_moving_aabb);
            intersect.has_value()) {

            if (intersect->time >= 1.0f) {
                continue;
            }

            if (intersect->time <= epsilon2) {
                return block_aabb.block;
            }
        }
    }
    return std::nullopt;
}

// Only returns ground if we can collide with it.
static std::optional<shared::world::block>
get_collidable_ground(const aabb& player_aabb,
                      const std::vector<blockinfo> blockinfo) noexcept {

    return get_ground(player_aabb, blockinfo,
                      shared::world::block::is_collidable);
}

// Recursively resolve collisions against blocks, up to n times.
static void resolve_collisions(shared::player& player, aabb player_aabb,
                               const std::vector<blockinfo>& blockinfos,
                               const float deltatime,
                               const int n = 3) noexcept {

    const moving_aabb player_moving_aabb{
        .aabb = player_aabb, .velocity = player.velocity * deltatime};

    std::optional<std::pair<moving_aabb_ret, shared::world::block>> collision;
    for (const auto& block_aabb : blockinfos) {
        if (!shared::world::block::is_collidable(block_aabb.block)) {
            continue;
        }

        const struct moving_aabb block_moving_aabb {
            .aabb = block_aabb.aabb, .velocity = { 0.0f, 0.0f, 0.0f }
        };
        const auto intersect =
            intersect_moving_aabbs(player_moving_aabb, block_moving_aabb);

        if (!intersect.has_value() || intersect->time > 1.0f) {
            continue;
        }

        // Update collision if it doesn't exist or if this one is closer.
        if (!collision.has_value() || intersect->time < collision->first.time) {

            collision = std::make_pair(intersect.value(), block_aabb.block);
        }
    }

    // No more collisions :)
    if (!collision.has_value()) {
        player.local_pos += player.velocity * deltatime;
        return;
    }

    const glm::vec3 collision_pos =
        collision->first.time * player.velocity * deltatime;
    const glm::vec3 offset = collision->first.normal * epsilon;
    const float backoff = glm::dot(player.velocity, collision->first.normal);

#ifndef NDEBUG
    // clang-format off
    shared::print::debug("Collision with n = " + std::to_string(n) + "\n");
    shared::print::message("    Unresolved player position: {" + std::to_string(player.local_pos.x) + ", " + std::to_string(player.local_pos.y) + ", " + std::to_string(player.local_pos.z) + "}\n", false);
    shared::print::message("    Unresolved player velocity: {" + std::to_string(player.velocity.x) + ", " + std::to_string(player.velocity.y) + ", " + std::to_string(player.velocity.z) + "}\n", false);
    shared::print::message("    Collision delta vector: {" + std::to_string(collision_pos.x) + ", " + std::to_string(collision_pos.y) + ", " + std::to_string(collision_pos.z) + "}\n", false);
    shared::print::message("    Backoff: " + std::to_string(backoff) + "\n", false);
    shared::print::message("    On ground before collision: " + std::to_string(get_collidable_ground(player_aabb, blockinfos).has_value()) + "\n", false);
    shared::print::message("    Collision normal: {" + std::to_string(collision->first.normal.x) + ", " + std::to_string(collision->first.normal.y) + ", " + std::to_string(collision->first.normal.z) + "}\n", false);
    // clang-format on
#endif

    // Quake engine's reflect velocity.
    for (int i = 0; i < 3; ++i) {
        player.velocity[i] -= collision->first.normal[i] * backoff;
        if (std::abs(player.velocity[i]) <= epsilon) {
            player.velocity[i] = 0.0f;
        }
        if (std::abs(collision_pos[i]) <= epsilon2) {

#ifndef NDEBUG
            shared::print::message(
                "    Ignored axis: " + std::to_string(i) + "\n", false);
#endif

            continue;
        }
        player.local_pos[i] += collision_pos[i] - offset[i];
        player_aabb.min[i] += collision_pos[i] - offset[i];
        player_aabb.max[i] += collision_pos[i] - offset[i];
    }

#ifndef NDEBUG
    // clang-format off
    shared::print::message("    Resolved player position: {" + std::to_string(player.local_pos.x) + ", " + std::to_string(player.local_pos.y) + ", " + std::to_string(player.local_pos.z) + "}\n", false);
    shared::print::message("    Resolved player velocity: {" + std::to_string(player.velocity.x) + ", " + std::to_string(player.velocity.y) + ", " + std::to_string(player.velocity.z) + "}\n", false);
    shared::print::message("    On ground after collision: " + std::to_string(get_collidable_ground(player_aabb, blockinfos).has_value()) + "\n", false);
    // clang-format on
#endif

    if (n <= 0) {
        return;
    }
    resolve_collisions(player, player_aabb, blockinfos,
                       deltatime - collision->first.time * deltatime, n - 1);
}

// Returns the highest friction value of a block that intersects an aabb.
static std::optional<float>
get_intersect_friction(const aabb& aabb,
                       const std::vector<blockinfo>& blockinfos) noexcept {
    std::optional<float> greatest_friction;
    for (const auto& block_aabb : blockinfos) {
        if (!intersect_aabbs(aabb, block_aabb.aabb)) {
            continue;
        }
        const float friction =
            shared::world::block::get_friction(block_aabb.block);

        if (!greatest_friction.has_value() ||
            friction > greatest_friction.value()) {

            greatest_friction = friction;
        }
    }

    return greatest_friction;
}

// In air is slightly more complicated becase of edge cases involving water.
static bool is_in_air(
    const aabb& aabb, const std::vector<blockinfo>& blockinfos,
    const std::optional<shared::world::block>& collidable_ground) noexcept {
    if (!std::all_of(std::begin(blockinfos), std::end(blockinfos),
                     [&aabb](const auto& ba) {
                         if (!intersect_aabbs(aabb, ba.aabb)) {
                             return true;
                         }
                         return !shared::world::block::is_tangible(ba.block);
                     })) {
        return false;
    }
    if (collidable_ground.has_value()) {
        return false;
    }
    return true;
}

static bool is_in_liquid(const aabb& aabb,
                         const std::vector<blockinfo>& blockinfos) noexcept {
    return std::any_of(std::begin(blockinfos), std::end(blockinfos),
                       [&aabb](const auto& ba) {
                           if (!intersect_aabbs(aabb, ba.aabb)) {
                               return false;
                           }
                           return shared::world::block::is_liquid(ba.block);
                       });
}

static bool
can_jump(const std::optional<shared::world::block>& collidable_ground,
         const bool in_liquid) noexcept {
    if (collidable_ground.has_value()) {
        return true;
    }
    if (in_liquid) {
        return true;
    }
    return false;
}

// Move state is a cache of expensive operations that we use more than once.
struct move_state {
    std::optional<shared::world::block> ground;
    std::optional<shared::world::block> collidable_ground;
    bool is_in_air;
    bool is_in_liquid;
    bool can_jump;
};
static move_state
make_move_state(const aabb& player_aabb,
                const std::vector<blockinfo>& blockinfos) noexcept {

    const auto get_ground_ret = get_ground(player_aabb, blockinfos);
    const auto get_collidable_ground_ret =
        get_collidable_ground(player_aabb, blockinfos);
    const bool is_in_air_ret =
        is_in_air(player_aabb, blockinfos, get_collidable_ground_ret);
    const bool is_in_liquid_ret = is_in_liquid(player_aabb, blockinfos);
    const bool can_jump_ret =
        can_jump(get_collidable_ground_ret, is_in_liquid_ret);

    return {.ground = get_ground_ret,
            .collidable_ground = get_collidable_ground_ret,
            .is_in_air = is_in_air_ret,
            .is_in_liquid = is_in_liquid_ret,
            .can_jump = can_jump_ret};
}

static float get_jump_magnitude(const shared::player& player, const aabb& aabb,
                                const std::vector<blockinfo>& blockinfos,
                                const struct move_state& move_state) noexcept {
    if (!move_state.can_jump) {
        return 0.0f;
    }
    if (move_state.is_in_liquid) {
        const struct aabb jump_aabb = {
            .min = glm::vec3{0.0f, aabb.min.y + aabb.max.y / 5.0f, 0.0f},
            .max = aabb.max};
        if (player.velocity.y > 0.0f &&
            is_in_air(jump_aabb, blockinfos,
                      get_collidable_ground(jump_aabb, blockinfos))) {
            return 1.0f;
        }
        return 0.5f;
    }
    if (player.velocity.y < 0.0f) {
        return 0.0f;
    }
    return 8.0f;
}

void move(shared::player& player, const std::vector<blockinfo>& blockinfos,
          const float deltatime) noexcept {

    constexpr glm::vec3 up = glm::vec3(0.0f, 1.0f, 0.0f);
    const glm::vec3 front = shared::math::angle_to_dir(player.viewangles);
    const glm::vec3 right = glm::normalize(glm::cross(front, up));

    constexpr aabb player_aabb = {.min = {-shared::player::HALFWIDTH + epsilon,
                                          0.0f + epsilon,
                                          -shared::player::HALFWIDTH + epsilon},
                                  .max = {shared::player::HALFWIDTH - epsilon,
                                          shared::player::HEIGHT - epsilon,
                                          shared::player::HALFWIDTH - epsilon}};

    const auto move_state = make_move_state(player_aabb, blockinfos);

    // Some of velocity we want to add or remove should be scaled by our
    // tickrate (eg, walking), while others such as jumping should not.

    const glm::vec3 scaled_acceleration = [&]() -> glm::vec3 {
        glm::vec3 acceleration = {0.0f, 0.0f, 0.0f};
        if (player.commands & shared::player::mask::forward) {
            acceleration += front;
        }
        if (player.commands & shared::player::mask::left) {
            acceleration -= right;
        }
        if (player.commands & shared::player::mask::backward) {
            acceleration -= front;
        }
        if (player.commands & shared::player::mask::right) {
            acceleration += right;
        }
        acceleration.y = 0.0f;
        if (acceleration != glm::vec3{}) {
            acceleration = glm::normalize(acceleration);
        }
        acceleration *= 1.75f;
        if (player.commands & shared::player::mask::sprint) {
            acceleration *= 1.25f;
        }

        // Increase movement when on the ground - heavily reduced with friction.
        if (!move_state.is_in_air) {
            acceleration *= 30.0f;
        }

        // Gravity.
        if (!move_state.collidable_ground.has_value()) {
            acceleration -= 25.0f * up;
        }

        return acceleration;
    }();

    const glm::vec3 constant_acceleration = [&]() -> glm::vec3 {
        glm::vec3 acceleration = {0.0f, 0.0f, 0.0f};
        // Jumping - we have to adjust our magnitude based on our environment.
        // Technically swimming is just lots of small jumps.
        if (player.commands & shared::player::mask::jump) {
            acceleration += get_jump_magnitude(player, player_aabb, blockinfos,
                                               move_state) *
                            up;
        }
        return acceleration;
    }();

    // Our deceleration is the max friction of what we're in and what we're on.
    const glm::vec3 scaled_deceleration = [&]() -> glm::vec3 {
        const auto drag = get_intersect_friction(player_aabb, blockinfos);

        float max_friction = 0.0f;
        if (move_state.collidable_ground.has_value()) {
            const float friction = shared::world::block::get_friction(
                move_state.collidable_ground.value());
            if (drag.has_value()) {
                max_friction = std::max(friction, drag.value());
            }
        } else if (drag.has_value()) {
            max_friction = drag.value();
        }

        return player.velocity * max_friction;
    }();

    player.velocity += (scaled_acceleration - scaled_deceleration) * deltatime;
    player.velocity += constant_acceleration;

    resolve_collisions(player, player_aabb, blockinfos, deltatime);

    shift_chunk(player);
}

} // namespace movement
} // namespace shared