#include "physics.hpp"

struct BoxTest Box2d_setup(b2World &world) {
  BoxTest box;
  float wall_x[4] = {0.0f, 0.0f, -0.99f, 0.99f};
  float wall_y[4] = {-0.99, 0.99f, 0.0f, 0.0f};
  float bound_w[4] = {1.0f, 1.0f, 0.1f, 0.1f};
  float bound_h[4] = {0.1f, 0.1f, 1.0f, 1.0f};

  for(size_t i = 0; i < 4; i++) {
    b2BodyDef groundBodyDef;
    groundBodyDef.position.Set(wall_x[i], wall_y[i]);
    b2Body *groundBody = world.CreateBody(&groundBodyDef);

    b2PolygonShape groundBox;
    groundBox.SetAsBox(bound_w[i], bound_h[i]);
    groundBody->CreateFixture(&groundBox, 1.0f);

    box.walls[i] = groundBody;
  }

  for(size_t i = 0; i < BODY_COUNT; i++) {
    b2BodyDef bodyDef;
    bodyDef.type = b2_dynamicBody;
    bodyDef.position.Set(0.0f, 0.5f);
    box.bodies[i] = world.CreateBody(&bodyDef);

    b2PolygonShape dynamicBox;
    dynamicBox.SetAsBox(0.1f, 0.1f);
    b2FixtureDef fixtureDef;
    fixtureDef.shape = &dynamicBox;

    fixtureDef.density = 1.0f;
    fixtureDef.friction = 0.3f;

    box.bodies[i]->CreateFixture(&fixtureDef);
  }

  return box;
}

void Box2d_step_world(b2World& world) {
  float timeStep = 1.0f / 60.0f;
  int velocityIterations = 6;
  int positionIterations = 2;

  // step the world
  world.Step(timeStep, velocityIterations, positionIterations);
}