#include <cmath>

#include "common/world/HighMap.h"
#include "common/world/World.h"
#include "utils/Logger.h"
#include "utils/Random.h"

World::World(const BlockRegistry& blockRegistry)
    : blockRegistry(blockRegistry), blocks(7, 7), dirty(true) {
}

void World::setBlock(int x, int y, int z, const Block& block) {
    blocks.set(x, y, z, block.getId());
}

const Block& World::getBlock(int x, int y, int z) const {
    return blockRegistry.getBlock(blocks.get(x, y, z));
}

int World::getSize() const {
    return blocks.getSize();
}

int World::getHeight() const {
    return blocks.getHeight();
}

void World::addEntity(Entity* e) {
    entities.add(e);
}

void World::removeEntity(Entity* e) {
    for(int i = 0; i < entities.getLength(); i++) {
        if(entities[i] == e) {
            entities.removeBySwap(i);
            return;
        }
    }
}

List<CollisionBox> World::getBoxes(const CollisionBox& box) const {
    int minX = floorf(box.getMin()[0]);
    int minY = floorf(box.getMin()[1]);
    int minZ = floorf(box.getMin()[2]);
    int maxX = floorf(box.getMax()[0]);
    int maxY = floorf(box.getMax()[1]);
    int maxZ = floorf(box.getMax()[2]);

    List<CollisionBox> boxes;
    for(int x = minX; x <= maxX; x++) {
        for(int y = minY; y <= maxY; y++) {
            for(int z = minZ; z <= maxZ; z++) {
                getBlock(x, y, z).addBoxes(
                    boxes, Vector3(static_cast<float>(x), static_cast<float>(y),
                                   static_cast<float>(z)));
            }
        }
    }
    return boxes;
}

void World::tick() {
    for(Entity* e : entities) {
        e->tick();
        if(!e->skip) {
            Vector3 move = limitMove(*e, e->getVelocity());
            e->move(move);
        }
    }
}

Vector3 World::limitMove(const Entity& e, Vector3 move) const {
    CollisionBox box = e.getCollisionBox();
    List<CollisionBox> boxes = getBoxes(box.expand(move));
    if(boxes.getLength() == 0) {
        return move;
    }
    Vector3 realMove;

    constexpr float step = 0.05f;
    while(move[0] != 0.0f || move[1] != 0.0f || move[2] != 0.0f) {
        Vector3 old = realMove;
        if(move[0] > step) {
            realMove[0] += step;
            move[0] -= step;
        } else if(move[0] < -step) {
            realMove[0] -= step;
            move[0] += step;
        } else if(move[0] != 0.0f) {
            realMove[0] += move[0];
            move[0] = 0.0f;
        }
        CollisionBox moved = box.offset(realMove);
        for(const CollisionBox& box : boxes) {
            if(box.collidesWith(moved)) {
                move[0] = 0.0f;
                realMove = old;
                break;
            }
        }

        old = realMove;
        if(move[1] > step) {
            realMove[1] += step;
            move[1] -= step;
        } else if(move[1] < -step) {
            realMove[1] -= step;
            move[1] += step;
        } else if(move[1] != 0.0f) {
            realMove[1] += move[1];
            move[1] = 0.0f;
        }
        moved = box.offset(realMove);
        for(const CollisionBox& box : boxes) {
            if(box.collidesWith(moved)) {
                move[1] = 0.0f;
                realMove = old;
                break;
            }
        }

        old = realMove;
        if(move[2] > step) {
            realMove[2] += step;
            move[2] -= step;
        } else if(move[2] < -step) {
            realMove[2] -= step;
            move[2] += step;
        } else if(move[2] != 0.0f) {
            realMove[2] += move[2];
            move[2] = 0.0f;
        }
        moved = box.offset(realMove);
        for(const CollisionBox& box : boxes) {
            if(box.collidesWith(moved)) {
                move[2] = 0.0f;
                realMove = old;
                break;
            }
        }
    }
    return realMove;
}