#include <cmath>

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

World::World() : blocks(7, 7), dirty(true) {
}

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<Box> World::getBoxes(const Box& 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<Box> boxes;
    for(int x = minX; x <= maxX; x++) {
        for(int y = minY; y <= maxY; y++) {
            for(int z = minZ; z <= maxZ; z++) {
                Block::addBoxes(blocks.get(x, y, z), 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 {
    Box box = e.getBox();
    List<Box> 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) {
        for(int i = 0; i < 3; i++) {
            Vector3 old = realMove;
            if(move[i] > step) {
                realMove[i] += step;
                move[i] -= step;
            } else if(move[i] < -step) {
                realMove[i] -= step;
                move[i] += step;
            } else if(move[i] != 0.0f) {
                realMove[i] += move[i];
                move[i] = 0.0f;
            }
            Box moved = box.offset(realMove);
            for(const Box& box : boxes) {
                if(box.collidesWith(moved)) {
                    move[i] = 0.0f;
                    realMove = old;
                    break;
                }
            }
        }
    }
    return realMove;
}