#include <iostream>

#include "rbdlsim.h"

using namespace std;
using namespace RBDLSim;

void simplesim() {
  World world;
  SimBody sphere_body =
      CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
  sphere_body.qdot[3] = 1 * M_PI;
  world.mBodies.push_back(sphere_body);

  SimShape ground_shape;
  ground_shape.mType = SimShape::Box;
  ground_shape.pos.set(0., -5.0, 0.);
  ground_shape.orientation.set(0., 0., 0., 1.);
  ground_shape.scale.set(25.0, 10.0, 25.0);

  world.mStaticShapes.push_back(ground_shape);

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

  do {
    world.updateCollisionShapes();
    world.detectCollisions();
    world.step(1.0e-3);
  } while (world.mContactPoints.size() == 0);

  cout << "Collision at t = " << world.mSimTime << endl
       << "q = " << world.mBodies[0].q.transpose() << endl;
}

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