#ifndef RBDLSIM_RBDLSIM_H
#define RBDLSIM_RBDLSIM_H

#include <rbdl/Model.h>
#include <rbdl/rbdl_math.h>

#include <vector>

namespace RBDLSim {

using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

struct SimShape;
struct SimBody;
struct CollisionInfo;

const double cCollisionEps = 1.0e-4;

struct SimShape {
  enum ShapeType {
    Box = 0,
    Sphere = 1,
    Plane = 2
  };
  ShapeType mType;
  Vector3d pos;
  Quaternion orientation;
  Vector3d scale;
};

struct SimBody {
  VectorNd q, qdot, qddot, tau;
  Model mModel;
  
  typedef std::pair<unsigned int, SimShape> BodyCollisionInfo;
  std::vector<BodyCollisionInfo> mCollisionShapes;

  void step(double ts);
  void resolveCollisions (double dt, std::vector<CollisionInfo>& collisions);
  void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q);
  void updateCollisionShapes();
};

struct CollisionInfo {
  const SimBody* mBodyA;
  const SimBody* mBodyB;
  const SimShape* mShapeA;
  const SimShape* mShapeB;
  int mBodyAIndex;
  int mBodyBIndex;
  Vector3d pos;
  Vector3d dir;
  double depth;
};

struct World {
  double mSimTime = 0.;
  std::vector<SimBody> mBodies;
  std::vector<SimShape> mStaticShapes;
  std::vector<CollisionInfo> mContactPoints;

  void updateCollisionShapes();
  void detectCollisions();
  void resolveCollisions(double dt);
  bool integrateWorld(double dt);
};

bool CheckPenetration(
    const SimShape& shape_a,
    const SimShape& shape_b,
    CollisionInfo& cinfo);

bool CheckPenetrationSphereVsPlane(
    const SimShape& shape_a,
    const SimShape& shape_b,
    CollisionInfo& cinfo);

SimBody CreateSphereBody(
    double mass,
    double radius,
    const Vector3d& pos,
    const Vector3d& vel);

SimBody CreateBoxBody(
    double mass,
    const Vector3d& size,
    const Vector3d& pos,
    const Vector3d& vel);

}  // namespace RBDLSim

#endif  //RBDLSIM_RBDLSIM_H