#include "PhysicsObject.hpp"
#include "MathF.hpp"
#include "elements/Transformable.hpp"
#include "elements/PhysicsEngine.hpp"

TSE::PhysicsObject::PhysicsObject(BodyType t, ColliderShape s, float d, float f, Vector3 cs)
{
    type = t;
    shape = s;
    density = d;
    friction = f;
    collidersize = cs;
}

TSE::PhysicsObject::~PhysicsObject()
{
    PhysicsEngine::UnRegisterPhysicsObject(this);
    if(b2Body_IsValid(bodyId))
        b2DestroyBody(bodyId);
}

void TSE::PhysicsObject::UpdatePosition()
{
    b2Vec2 b2newPos = b2Body_GetPosition(bodyId);
    if(lastPos.x != b2newPos.x || lastPos.y != b2newPos.y)
    {
        Vector3 newPos(b2newPos.x, b2newPos.y, lastPos.z);
        lastPos = newPos;
        Vector3 delta = baseObject->GlobalToLocalPosition(newPos);
        baseObject->position = baseObject->position + delta;
    }
    float newRot = b2Rot_GetAngle(b2Body_GetRotation(bodyId));
    if(newRot != lastRot)
    {
        lastRot = newRot;
        Vector3 euler = baseObject->GetEuler();
        euler.z = Rad2Deg(newRot);
    }
}

void TSE::PhysicsObject::OnUpdate()
{
    Vector3 globalPos = baseObject->GetGlobalPosition();
    float rot = Deg2Rad(baseObject->GetEuler().z);
    if(globalPos != lastPos || rot != lastRot)
    {
        lastPos = globalPos;
        lastRot = rot;
        b2Vec2 pos;
        pos.x = globalPos.x;
        pos.y = globalPos.y;
        b2Body_SetTransform(bodyId, pos, b2MakeRot(rot));
        b2Body_SetAwake(bodyId, true);
    }
}

void TSE::PhysicsObject::Start()
{
    b2WorldId& wid = PhysicsEngine::GetWorldId();

    b2BodyDef def = b2DefaultBodyDef();
    b2Vec2 pos;
    Vector3 globalPos = baseObject->GetGlobalPosition();
    pos.x = globalPos.x;
    pos.y = globalPos.y;
    def.position = pos;
    lastPos = globalPos;
    float rot = Deg2Rad(baseObject->GetEuler().z);
    def.rotation = b2MakeRot(rot);
    lastRot = rot;
    b2BodyType b2type;
    switch(type)
    {
        case BodyType::Dynamic:
            b2type = b2_dynamicBody;
            break;
        case BodyType::Static:
            b2type = b2_staticBody;
            break;
    }
    def.type = b2type;

    bodyId = b2CreateBody(wid, &def);

    b2ShapeDef shapeDef = b2DefaultShapeDef();
    shapeDef.density = density;
    shapeDef.material.friction = friction;

    b2Polygon b2shape;
    b2Circle b2circle;
    b2Capsule b2capsule;

    switch (shape)
    {
        case ColliderShape::Box:
            b2shape = b2MakeBox(collidersize.x * 0.5f, collidersize.y * 0.5f);
            b2CreatePolygonShape(bodyId, &shapeDef, &b2shape);
            break;
        case ColliderShape::RoundedBox:
            b2shape = b2MakeRoundedBox(collidersize.x * 0.5f, collidersize.y * 0.5f, collidersize.z);
            b2CreatePolygonShape(bodyId, &shapeDef, &b2shape);
            break;
        case ColliderShape::Circle:
            b2circle.radius = collidersize.x;
            b2CreateCircleShape(bodyId, &shapeDef, &b2circle);
            break;
        case ColliderShape::Capsule:
            b2capsule.radius = collidersize.x;
            b2capsule.center1 = {0, collidersize.y * 0.5f};
            b2capsule.center1 = {0, collidersize.y * -0.5f};
            b2CreateCapsuleShape(bodyId, &shapeDef, &b2capsule);
            break;
    
    }

    PhysicsEngine::RegisterPhysicsObject(this);
}