#include <iostream>

#include "rbdlsim.h"

using namespace std;
using namespace RBDLSim;

void simplesim() {
  World world;
  SimBody sphere_body =
      CreateSphereBody(10., 1., 0., Vector3d(0., 5.405, 0.), Vector3d::Zero());
  world.mBodies.push_back(sphere_body);

  SimShape ground_shape;
  ground_shape.mType = SimShape::Plane;
  ground_shape.pos.set(0., 0., 0.);
  ground_shape.orientation.set(0., 0., 0., 1.);
  ground_shape.scale.set(1.0, 1.0, 1.0);

  world.mStaticShapes.push_back(ground_shape);

  world.mSimTime = 0.;
  cout << world.mBodies[0].q.transpose() << endl;

  double dt = 1.0e-3;
  do {
    world.updateCollisionShapes();
    world.detectCollisions();
    world.resolveCollisions(dt);
    world.integrateWorld(dt);
  } while (world.mSimTime < 10.01);
}

int main(int argc, char* argv[]) {
  simplesim();
  return 0;
}
