#include "shared/movement/movement.hh"

namespace shared {
namespace movement {

constexpr float MAX_SPEED_XZ = 5.0f;         // max speed before sprinting
constexpr float MAX_SPEED_XZ_LIQUID = 2.8f;  // max water speed before sprinting
constexpr float MAX_SPEED_XZ_FLYING = 11.0f; // max water speed before sprinting
constexpr float MAX_SPEED_Y = 80.0f;
constexpr float MAX_SPEED_Y_LIQUID = 3.0f;
constexpr float MAX_SPEED_Y_FLYING = 8.0f;
constexpr float MOVE_ACCEL = 75.0f;  // base acceleration in m*s^-2
constexpr float AIR_MULT = 0.125f;   // multiplier to acceleration if in air
constexpr float SPRINT_MULT = 1.20f; // multiplier to acceleration if sprinting
constexpr float SWIM_ACCEL = 50.0f;  // y-axis accel when swimming in m*s^-2
constexpr float JUMP_ACCEL = 7.85f;  // y-axis accel when jumping in m*s^-2
constexpr float FLY_ACCEL = 75.0f;   // y-axis accel when swimming in m*s^-2

constexpr float GRAVITY = 28.0f;
constexpr float DRAG = 0.1f;
constexpr float FRICTION = 15.0f;
constexpr float FLY_DRAG = FRICTION;
constexpr float VISCOSITY = 25.0f;

glm::vec3 make_relative(const shared::math::coords& base_chunk_pos,
                        const glm::vec3& other_local_pos,
                        const shared::math::coords& other_chunk_pos) noexcept {
    const auto diff_x = static_cast<std::int64_t>(other_chunk_pos.x) -
                        static_cast<std::int64_t>(base_chunk_pos.x);
    const auto diff_z = static_cast<std::int64_t>(other_chunk_pos.z) -
                        static_cast<std::int64_t>(base_chunk_pos.z);
    return {
        other_local_pos.x + static_cast<float>(world::chunk::WIDTH * diff_x),
        other_local_pos.y,
        other_local_pos.z + static_cast<float>(world::chunk::WIDTH * diff_z)};
}

void normalise_position(glm::vec3& local_pos,
                        shared::math::coords& chunk_pos) noexcept {
    const bool g_x = (local_pos.x >= world::chunk::WIDTH);
    const bool l_x = (local_pos.x < 0.0f);
    const bool g_z = (local_pos.z >= world::chunk::WIDTH);
    const bool l_z = (local_pos.z < 0.0f);

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

glm::ivec2 get_move_xy(const std::uint32_t& tickrate,
                       const shared::moveable& moveable) noexcept {
    const float max_x = std::max(
        MAX_SPEED_XZ, std::max(MAX_SPEED_XZ_LIQUID, MAX_SPEED_XZ_FLYING));
    const float max_y =
        std::max(MAX_SPEED_Y, std::max(MAX_SPEED_Y_LIQUID, MAX_SPEED_Y_FLYING));
    const float mult = 1.0f / static_cast<float>(tickrate);
    const auto& aabb = moveable.get_aabb();
    return {max_x * mult + aabb.max.x + 1.0f, max_y * mult + aabb.max.y + 1.0f};
}

static float get_block_friction(const enum world::block::type block) noexcept {
    if (world::block::is_liquid(block)) {
        return VISCOSITY;
    }
    if (world::block::is_collidable(block)) {
        return FRICTION;
    }
    return DRAG;
}

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) noexcept {
    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;
        }
    }

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

// Gets the closest collision of an entity to a block.
struct collide_ret {
    moving_aabb_ret collision;
    shared::world::block block;
};
static std::optional<collide_ret> collide(const aabb& aabb,
                                          const blocks& blocks,
                                          const glm::vec3& move) noexcept {

    const moving_aabb moving_aabb{.aabb = aabb, .velocity = move};

    // TODO: It's possible we collide at the same time, but on a different block
    // If this occurs, we will phase through it as we ignore the other
    // collision. So fix it by doing some maths.
    std::optional<collide_ret> collision;
    for (const auto& block_aabb : blocks) {
        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(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->collision.time) {

            collision =
                collide_ret{.collision = *intersect, .block = block_aabb.block};
        }
    }

    return collision;
}

// Returns the closest ground object if such an object exists.
static std::optional<shared::world::block>
maybe_get_ground(const blocks& blocks, const aabb& aabb,
                 bool (*filter)(const enum shared::world::block::type) =
                     world::block::is_tangible) noexcept {
    const moving_aabb moving_aabb{.aabb = aabb,
                                  .velocity = {0.0f, -1.0f, 0.0f}};

    // blockinfo, cur max distance
    std::optional<std::pair<block, float>> ground;
    for (const auto& block : blocks) {

        if (!filter(block.block)) {
            continue;
        }

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

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

            if (intersect->time > EPSILON2) {
                continue;
            }

            const float distance =
                glm::distance(aabb.min, block.aabb.min); // cool hack

            if (!ground.has_value()) {
                ground.emplace(std::make_pair(block, distance));
                continue;
            }

            const auto& [cur_max_block, cur_max_distance] = *ground;
            if (distance >= cur_max_distance) {
                continue;
            }

            ground.emplace(std::make_pair(block, distance));
        }
    }
    if (ground.has_value()) {
        return ground->first.block;
    }
    return std::nullopt;
}

static bool is_intersecting(
    const blocks& blocks, const aabb& aabb,
    bool (*filter)(const enum shared::world::block::type)) noexcept {
    for (const auto& block_aabb : blocks) {
        if (!filter(block_aabb.block)) {
            continue;
        }
        if (!intersect_aabbs(block_aabb.aabb, aabb)) {
            continue;
        }
        return true;
    }
    return false;
}

struct vectors {
    glm::vec3 up;
    glm::vec3 front;
    glm::vec3 right;
};

static glm::vec3 get_accel(const blocks& blocks, const aabb& aabb,
                           const vectors& vectors,
                           const entity::index_t& commands) noexcept {
    glm::vec3 acceleration{};

    const auto add_input = [&](const auto& mask, const glm::vec3& dir) {
        if (commands & mask) {
            acceleration += dir;
        }
    };
    add_input(animate::mask::forward, vectors.front);
    add_input(animate::mask::left, -vectors.right);
    add_input(animate::mask::backward, -vectors.front);
    add_input(animate::mask::right, vectors.right);

    acceleration.y = 0.0f; // so we don't move faster when facing up/down
    if (acceleration != glm::vec3{}) {
        acceleration = glm::normalize(acceleration);
    }
    acceleration *= MOVE_ACCEL; // 7.25 blocks/sec^2 by default
    if (commands & animate::mask::sprint) {
        acceleration *= SPRINT_MULT;
    }

    const bool in_water =
        is_intersecting(blocks, aabb, world::block::is_liquid);

    if ((commands & animate::mask::jump) && in_water) {
        acceleration += SWIM_ACCEL * vectors.up;
    }

    // flying up/down
    if (commands & animate::mask::flying) {
        if (commands & animate::mask::jump) {
            acceleration += FLY_ACCEL * vectors.up;
        }
        if (commands & animate::mask::crouch) {
            acceleration -= FLY_ACCEL * vectors.up;
        }
    }

    const bool on_ground =
        maybe_get_ground(blocks, aabb, world::block::is_collidable).has_value();
    // Move slower in the air, also our hitbox must be outside non-tangible.
    if (!on_ground && !in_water && !(commands & animate::mask::flying)) {
        acceleration *= AIR_MULT;
    }

    // gravity - only applied if there isn't a collidable block beneath us.
    // or if we're not flying
    if (!on_ground && !(commands & animate::mask::flying)) {
        acceleration -= GRAVITY * vectors.up;
    }

    return acceleration;
}

static float get_decel_factor(const blocks& blocks, const aabb& aabb,
                              const entity::index_t& commands) noexcept {

    const float drag = [&]() -> float { // max drag of all intersecting blocks
        float max = 0.0f;
        for (const auto& block : blocks) {
            if (!intersect_aabbs(aabb, block.aabb)) {
                continue;
            }
            max = std::max(max, get_block_friction(block.block));
        }

        if (commands & animate::flying) {
            return std::max(max, FLY_DRAG);
        }

        return max;
    }();

    const float friction = [&]() -> float {
        const auto ground =
            maybe_get_ground(blocks, aabb, world::block::is_collidable);
        return ground.has_value() ? get_block_friction(*ground) : 0.0f;
    }();

    // Our deceleration is simply the max of what we're in and what we're on
    return std::max(drag, friction);
}

static void decelerate(glm::vec3& velocity, const blocks& blocks,
                       const aabb& aabb, const entity::index_t& commands,
                       const float max_time) noexcept {
    const float decel =
        get_decel_factor(blocks, aabb, commands) * max_time * 2.0f;

    if (const float xy_speed = glm::length(glm::vec2{velocity.x, velocity.z});
        xy_speed > 0.0f) {

        const float new_speed = std::max(0.0f, xy_speed - decel);
        velocity.x *= new_speed / xy_speed;
        velocity.z *= new_speed / xy_speed;
    }

    // we decelerate on y, but only if we're flying
    if (const float y_speed = std::abs(velocity.y);
        y_speed > 0.0f && (commands & shared::animate::flying)) {

        velocity.y *= std::max(0.0f, y_speed - decel) / y_speed;
    }
}

static void clamp_move_xy(float& x, float& z, const entity::index_t& commands,
                          const bool in_liquid,
                          const float mult = 1.0f) noexcept {

    const float max_speed_xy = [&]() {
        const float base = [&]() { // lol
            if (commands & animate::mask::flying) {
                return MAX_SPEED_XZ_FLYING;
            }
            if (in_liquid) {
                return MAX_SPEED_XZ_LIQUID;
            }
            return MAX_SPEED_XZ;
        }();
        if (commands & animate::mask::sprint) {
            return base * SPRINT_MULT;
        }
        return base;
    }() * mult;

    if (const float speed_xz = std::hypot(x, z); speed_xz > max_speed_xy) {
        const float ratio = max_speed_xy / speed_xz;
        x *= ratio;
        z *= ratio;
    }
}

static void clamp_move_y(float& y, const entity::index_t& commands,
                         const bool in_liquid,
                         const float mult = 1.0f) noexcept {
    const float max_speed_y = [&]() {
        if (commands & animate::mask::flying) {
            return MAX_SPEED_Y_FLYING;
        }
        return in_liquid ? MAX_SPEED_Y_LIQUID : MAX_SPEED_Y;
    }() * mult;

    if (const float speed_y = std::abs(y); speed_y > max_speed_y) {
        const float ratio = max_speed_y / speed_y;
        y *= ratio;
    }
}

static void clamp_move(glm::vec3& move, const entity::index_t& commands,
                       const bool in_liquid, const float mult = 1.0f) noexcept {

    clamp_move_xy(move.x, move.z, commands, in_liquid, mult);
    clamp_move_y(move.y, commands, in_liquid, mult);
}

static void handle_jumps(glm::vec3& velocity, const blocks& blocks,
                         const aabb& aabb, const entity::index_t& commands,
                         const bool in_liquid) noexcept {
    if (!(commands & animate::mask::jump)) {
        return;
    }
    if (!(velocity.y >= 0 && velocity.y <= EPSILON)) {
        return;
    }
    if (in_liquid) {
        return;
    }
    if (!maybe_get_ground(blocks, aabb, world::block::is_collidable)) {
        return;
    }
    // TODO: the jump will only occur in the next movement tick
    // fix this by modifying both our velocity and our move vector
    velocity.y += JUMP_ACCEL;
}

static void handle_collisions(glm::vec3& velocity, glm::vec3& local_pos,
                              glm::vec3 move, const blocks& blocks, aabb aabb,
                              const entity::index_t& commands,
                              const float max_time) noexcept {
    constexpr int MAX_COLLISIONS = 100;
    for (int collisions = 0; collisions < MAX_COLLISIONS; ++collisions) {

        {
            const bool in_liquid =
                is_intersecting(blocks, aabb, world::block::is_liquid);

            handle_jumps(velocity, blocks, aabb, commands, in_liquid);
            clamp_move(velocity, commands, in_liquid);
            clamp_move(move, commands, in_liquid, max_time);
        }

        const auto collision = collide(aabb, blocks, move);

        if (!collision.has_value()) {
            local_pos += move;
            return;
        }

        const glm::vec3 pos = move * collision->collision.time;
        const glm::vec3 off = collision->collision.normal * EPSILON;

        for (int i = 0; i < 3; ++i) {

            float& move_axis = move[i];
            float& vel_axis = velocity[i];

            const float diff = pos[i] - off[i];
            move_axis -= diff;

            vel_axis -= collision->collision.normal[i] *
                        glm::dot(velocity, collision->collision.normal);
            move_axis -= collision->collision.normal[i] *
                         glm::dot(move, collision->collision.normal);

            vel_axis = std::abs(vel_axis) <= EPSILON ? 0.0f : vel_axis;
            move_axis = std::abs(move_axis) <= EPSILON ? 0.0f : move_axis;

            if (abs(pos[i]) <= EPSILON2) {
                continue;
            }

            local_pos[i] += diff; // add movement to current position
            aabb.min[i] += diff;
            aabb.max[i] += diff;
        }
    }
}

static void handle_movement(shared::animate& animate, const blocks& blocks,
                            const aabb& aabb, const vectors& vectors,
                            const entity::index_t& commands,
                            const float max_time) noexcept {

    auto& velocity = animate.get_mutable_velocity();
    auto& position = animate.get_mutable_local_pos();

    decelerate(velocity, blocks, aabb, commands, max_time);

    const glm::vec3 accel = get_accel(blocks, aabb, vectors, commands);
    const glm::vec3 move = animate.get_velocity() * max_time +
                           0.5f * accel * std::pow(max_time, 2.0f);

    animate.get_mutable_velocity() += accel * max_time;

    handle_collisions(velocity, position, move, blocks, aabb, commands,
                      max_time);
}

shared::animate move(const shared::moveable& moveable, const blocks& blocks,
                     const std::uint32_t& tickrate) noexcept {

    const movement::aabb& aabb = moveable.get_aabb();

    const auto vectors = [&]() -> struct vectors {
        const auto up = glm::vec3{0.0f, 1.0f, 0.0f};
        const auto front = moveable.get_angles().to_dir();
        const auto right = glm::normalize(glm::cross(front, up));
        return {.up = up, .front = front, .right = right};
    }();
    const float max_time = 1.0f / static_cast<float>(tickrate);

    shared::animate ret = moveable;
    handle_movement(ret, blocks, aabb, vectors, moveable.get_commands(),
                    max_time);
    normalise_position(ret.get_mutable_local_pos(),
                       ret.get_mutable_chunk_pos());
    return ret;
}

} // namespace movement
} // namespace shared