#include <BoundingBox/BoundingBox.hpp>
#include <Entity/PhysicsEntity/PhysicsEntity.hpp>
#include <Vk/ShaderConst.hpp>
#include <World/World.hpp>
#include <World/LocalWorld/LocalWorld.hpp>
#include <MovementPhysics/MovementPhysics.hpp>
#include <Block/Block.hpp>

void PhysicsEntity::updateVelocity()
{
    Block *containingBlock = world->getBlockAt(glm::floor(pos));

    if (containingBlock == nullptr)
    {
        return;
    }

    MovementPhysics &movementPhysics = containingBlock->getMovementPhysicsInside();

    vel -= vel * wfvec3(movementPhysics.getFriction() * getFrictionCoefficients());
    vel.z -= wfixed(movementPhysics.getGravity());
}

void PhysicsEntity::handleWalkingOver()
{
    BoundingBox<wfixed> box = getBoundingBox(pos);
    box.z0 -= wfixed(0.1);

    checkWalkingOver(wfvec3(box.x0, box.y0, box.z0), box);
    checkWalkingOver(wfvec3(box.x1, box.y0, box.z0), box);
    checkWalkingOver(wfvec3(box.x0, box.y1, box.z0), box);
    checkWalkingOver(wfvec3(box.x1, box.y1, box.z0), box);
}

void PhysicsEntity::checkWalkingOver(wfvec3 posUnder, const BoundingBox<wfixed> &bbox)
{
    glm::ivec3 intPosUnder = glm::floor(posUnder);
    Block *blockUnder = world->getBlockAt(intPosUnder);
    if (blockUnder != nullptr && blockUnder->isIntersectedBy(*world, intPosUnder, bbox))
    {
        blockUnder->onWalkingOver(*world, intPosUnder, *this);
    }
}

std::optional<bool> PhysicsEntity::isIntersectingBlock(const BoundingBox<wfixed> &box, glm::ivec3 blockPos)
{
    Block *block = world->getBlockAt(blockPos);
    if (block == nullptr)
    {
        return std::nullopt;
    }

    return block->isIntersectedBy(*world, blockPos, box);
}

void PhysicsEntity::playFallingSound(wfvec3 overallVelocity)
{
    if (overallVelocity.z < -0.09f)
    {
        auto bbox = getBoundingBox(pos);
        double volumeCoefficient = bbox.getVolume() * 0.7161;

        glm::vec3 soundSourcePos = glm::vec3(0.5f * (bbox.x1 + bbox.x0), 0.5f * (bbox.y1 + bbox.y0), bbox.z0 + 0.1f);

        bbox.z0 -= wfixed(0.1);

        glm::ivec3 blockPos;
        blockPos.z = bbox.z0.floor();

        float feetArea = (bbox.x1 - bbox.x0) * (bbox.y1 - bbox.y0);

        for (blockPos.y = (int) bbox.y0.floor(); blockPos.y < (int) bbox.y1.ceil(); blockPos.y++)
        {
            for (blockPos.x = (int) bbox.x0.floor(); blockPos.x < (int) bbox.x1.ceil(); blockPos.x++)
            {
                Block *block = world->getBlockAt(blockPos);
                if (block != nullptr && block->isIntersectedBy(*world, blockPos, bbox))
                {
                    Sound &sound = block->getFallingSound();
                    if (!sound.isNone())
                    {
                        wfixed x0 = std::max((wfixed) blockPos.x, (wfixed) bbox.x0);
                        wfixed x1 = std::min((wfixed) (blockPos.x + 1), (wfixed) bbox.x1);

                        wfixed y0 = std::max((wfixed) blockPos.y, (wfixed) bbox.y0);
                        wfixed y1 = std::min((wfixed) (blockPos.y + 1), (wfixed) bbox.y1);

                        float collisionWidth = std::max(0.0f, (float) (x1 - x0));
                        float collisionHeight = std::max(0.0f, (float) (y1 - y0));
                        float collisionArea = collisionWidth * collisionHeight;

                        float gain = (collisionArea / feetArea) * volumeCoefficient * (glm::abs((float) overallVelocity.z) + 0.7f);
                        sound.play(soundSourcePos, glm::vec3(0.0f), 1.0f, gain);
                    }
                }
            }
        }
    }
}

void PhysicsEntity::getIntersectedBlocks(std::set<Block*> &blocks)
{
    BoundingBox<wfixed> box = getBoundingBox(pos);

    wfvec3 pos(0, 0, 0);
    for (pos.z = box.z0; pos.z < box.z1; pos.z += wfixed(1.0))
    {
        for (pos.y = box.y0; pos.y < box.y1; pos.y += wfixed(1.0))
        {
            for (pos.x = box.x0; pos.x < box.x1; pos.x += wfixed(1.0))
            {
                Block *block = world->getBlockAt(glm::floor(pos));
                if (block == nullptr)
                {
                    continue;
                }

                blocks.insert(block);
            }
        }
    }
}

PhysicsEntity::PhysicsEntity(World &world,
    std::string typeId, uint64_t id,
    wfvec3 posArg,
    wfvec3 velArg
) : Entity(world, typeId, id),
    pos(posArg),
    vel(velArg),
    restVel(0.0, 0.0, 0.0)
{}

void PhysicsEntity::update()
{
    wfvec3 prevRestVel = restVel;
    restVel = wfvec3(0, 0, 0);

    updateVelocity();
    handleWalkingOver();

    std::set<Block*> prevBlocksInside;
    getIntersectedBlocks(prevBlocksInside);

    // Check if we are still on the ground.
    if (isOnGround && canGoTo(this->pos + wfvec3(0.0f, 0.0, -0.1f)) != false)
    {
        isOnGround = false;
    }

    bool haveFallen = false;

    // Check collisions and move.
    vel += prevRestVel - restVel;
    wfvec3 overallVelocity = vel + restVel;
    wfvec3 dest = pos + overallVelocity;
    std::optional<bool> canGoToMainDest = canGoTo(dest);
    if (!canGoToMainDest.has_value())
    {
        onEnteringUnloadedChunk();
    }
    else if (canGoToMainDest == true)
    {
        pos = dest;
    }
    else if (!onCollision())
    {
        float dist = glm::length(glm::vec3(overallVelocity));
        glm::vec3 dir = glm::normalize(glm::vec3(overallVelocity));

        while (dist > 0.03f && glm::length(dir) > 0.01f)
        {
            int i;
            for (i=0; i<3; i++)
            {
                pos[i] += wfixed(0.03 * dir[i]);
                if (canGoTo(pos) != true)
                {
                    if (!isOnGround && i == 2 && dir[i] < 0.0f)
                    {
                        isOnGround = true;
                        haveFallen = true;
                    }

                    pos[i] -= wfixed(0.03f * dir[i]);
                    dir[i] = 0.0f;
                    vel[i] = wfixed(0.0f);
                }
            }

            dist -= 0.03f;
        }
    }

    if (haveFallen)
    {
        playFallingSound(overallVelocity);
    }

    std::set<Block*> newBlocksInside;
    getIntersectedBlocks(newBlocksInside);

    for (const auto &block : prevBlocksInside)
    {
        if (!newBlocksInside.contains(block))
        {
            block->onLeaving(*world, *this);
        }
    }

    for (const auto &block : newBlocksInside)
    {
        if (!prevBlocksInside.contains(block))
        {
            block->onEntering(*world, *this);
        }
    }
}

bool PhysicsEntity::onCollision()
{
    return false;
}

void PhysicsEntity::onEnteringUnloadedChunk()
{
    // NOP
}

bool PhysicsEntity::wouldCollideWith(Block &block, glm::ivec3 blockPos)
{
    BoundingBox<wfixed> box = getBoundingBox(pos);

    std::vector<BoundingBox<wfixed>> blockBoxes;
    block.getCollisionBoxes(*world, blockPos, blockBoxes);

    for (BoundingBox<wfixed> blockBox : blockBoxes)
    {
        if (box.overlaps(blockBox))
        {
            return true;
        }
    }
    
    return false;
}

std::optional<bool> PhysicsEntity::canGoTo(wfvec3 pos)
{
    BoundingBox<wfixed> box = getBoundingBox(pos);

    wvec3 blockPos(0, 0, 0);
    for (blockPos.x=(cint32)box.x0.floor().operator cint64(); blockPos.x<=(cint32)box.x1.operator cint64(); blockPos.x+=1)
    {
        for (blockPos.y=(cint32)box.y0.floor().operator cint64(); blockPos.y<=(cint32)box.y1.operator cint64(); blockPos.y+=1)
        {
            for (blockPos.z=(cint32)box.z0.floor().operator cint64(); blockPos.z<=(cint32)box.z1.operator cint64(); blockPos.z+=1)
            {
                std::optional<bool> intersecting = isIntersectingBlock(box, blockPos);
                if (!intersecting.has_value())
                {
                    return std::nullopt;
                }

                if (*intersecting)
                {
                    return false;
                }
            }
        }
    }

    return true;
}