#include "engine/physics.hpp"
#include <cstdio>

PhysicsWorld::PhysicsWorld() {
    b2WorldDef def = b2DefaultWorldDef();
    def.gravity = {0.0f, -10.0f};
    world_id_ = b2CreateWorld(&def);
    printf("Physics: Box2D world created\n");
}

PhysicsWorld::~PhysicsWorld() {
    b2DestroyWorld(world_id_);
}

void PhysicsWorld::step(float dt) {
    // High sub-step count for stable distance joint networks
    b2World_Step(world_id_, dt, 8);
}

PhysicsBody* PhysicsWorld::add_static_box(b2Vec2 center,
                                           float half_w, float half_h,
                                           float r, float g, float b) {
    b2BodyDef body_def = b2DefaultBodyDef();
    body_def.type     = b2_staticBody;
    body_def.position = center;

    b2BodyId body_id = b2CreateBody(world_id_, &body_def);

    b2ShapeDef shape_def = b2DefaultShapeDef();
    b2Polygon  box       = b2MakeBox(half_w, half_h);
    b2CreatePolygonShape(body_id, &shape_def, &box);

    PhysicsBody pb;
    pb.id        = body_id;
    pb.size      = {half_w, half_h};
    pb.red       = r;
    pb.green     = g;
    pb.blue      = b;
    pb.alpha     = 1.0f;
    pb.is_static = true;
    bodies_.push_back(pb);
    return &bodies_.back();
}

PhysicsBody* PhysicsWorld::add_dynamic_box(b2Vec2 center,
                                            float half_w, float half_h,
                                            float r, float g, float b) {
    b2BodyDef body_def = b2DefaultBodyDef();
    body_def.type     = b2_dynamicBody;
    body_def.position = center;

    b2BodyId body_id = b2CreateBody(world_id_, &body_def);

    b2ShapeDef shape_def = b2DefaultShapeDef();
    shape_def.density = 1.0f;
    shape_def.material.friction = 0.3f;
    b2Polygon box = b2MakeBox(half_w, half_h);
    b2CreatePolygonShape(body_id, &shape_def, &box);

    PhysicsBody pb;
    pb.id        = body_id;
    pb.size      = {half_w, half_h};
    pb.red       = r;
    pb.green     = g;
    pb.blue      = b;
    pb.alpha     = 1.0f;
    pb.is_static = false;
    bodies_.push_back(pb);
    return &bodies_.back();
}