#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;
  double restitution = 1.0;
};

struct SimBody {
  VectorNd q, qdot, qddot, tau;
  Model mModel;
  bool mIsStatic = false;

  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 {
  SimBody* mBodyA = nullptr;
  SimBody* mBodyB = nullptr;
  const SimShape* mShapeA = nullptr;
  const SimShape* mShapeB = nullptr;
  int mBodyAIndex;
  int mBodyBIndex;
  Vector3d posA = Vector3d::Zero();
  Vector3d posB = Vector3d::Zero();
  double biasVelocityA = 0.;
  double biasVelocityB = 0.;
  double accumImpulse = 0.;
  double deltaImpulse = 0.;
  Vector3d dir = Vector3d::Zero();
  VectorNd jacA = VectorNd::Zero(1);
  VectorNd jacB = VectorNd::Zero(1);
  MatrixNd MInvA = MatrixNd::Zero(1, 1);
  MatrixNd MInvB = MatrixNd::Zero(1, 1);
  double GMInvGTA = 0.;
  double GMInvGTB = 0.;
  double effectiveRestitution = 1.0;

  double depth = 0.;
};

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

  void calcUnconstrainedVelUpdate(double dt);
  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 CheckPenetrationSphereVsSphere(
    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,
    double restitution,
    const Vector3d& pos,
    const Vector3d& vel);

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

void PrepareConstraintImpulse(
    SimBody* body_a,
    SimBody* body_b,
    CollisionInfo& cinfo);
void CalcCollisions(
    SimBody& body_a,
    SimBody& body_b,
    std::vector<CollisionInfo>& collisions);
void CalcConstraintImpulse(
    SimBody* body_a,
    SimBody* body_b,
    CollisionInfo& cinfo,
    const double dt);
void ApplyConstraintImpulse(
    SimBody* body_a,
    SimBody* body_b,
    CollisionInfo& cinfo);

}  // namespace RBDLSim

#endif  //RBDLSIM_RBDLSIM_H