#include "PhysicsEngine.hpp"
#include "utils/Time.hpp"

void TSE::PhysicsEngine::InitPhysics(Vector2 gravity)
{
    b2WorldDef def = b2DefaultWorldDef();
    def.enableSleep = true;
    b2Vec2 grav;
    grav.x = gravity.x;
    grav.y = gravity.y;
    def.gravity = grav;
    worldId = b2CreateWorld(&def);
}

void TSE::PhysicsEngine::UpdatePhysics()
{
    if(elapsedTime >= timestep)
    {
        elapsedTime = 0;
        b2World_Step(worldId, timestep, iterations);
        //update physics objects
    }
    elapsedTime += Time::deltaTime();

    for(auto obj : registeredObjects)
    {
        obj->UpdatePosition();
    }
}

void TSE::PhysicsEngine::DeletePhysics()
{
    b2DestroyWorld(worldId);
}

void TSE::PhysicsEngine::RegisterPhysicsObject(PhysicsObject *obj)
{
    registeredObjects.push_back(obj);
}

void TSE::PhysicsEngine::UnRegisterPhysicsObject(PhysicsObject *obj)
{
    auto it = registeredObjects.begin();
    for (; it != registeredObjects.end(); it++)
    {
        if(*it == obj)
        {
            registeredObjects.erase(it);
            return;
        }
    }
}

b2WorldId &TSE::PhysicsEngine::GetWorldId()
{
    return worldId;
}