#include <iostream>
#include <btBulletDynamicsCommon.h>

int main(int argc, char** argv)
{
  btGeneric6DofConstraint* slider = NULL;
  btCollisionConfiguration* colConf = new btDefaultCollisionConfiguration();
  btCollisionDispatcher* colDis = new btCollisionDispatcher(colConf);
  btBroadphaseInterface* pCache = new btDbvtBroadphase();
  btConstraintSolver* cSolver = new btSequentialImpulseConstraintSolver();
  btDynamicsWorld* world = new btDiscreteDynamicsWorld(colDis, pCache, cSolver, colConf);

  world->setGravity(btVector3(0, 0, -9.81));

  // Static sphere
  btCollisionShape* staticSh = new btSphereShape(1);
  btScalar staticmass = 0;
  btVector3 staticinertia(0,0,0);
  staticSh->calculateLocalInertia(staticmass, staticinertia);
  btDefaultMotionState* staticMs = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,3)));
  btRigidBody::btRigidBodyConstructionInfo staticBodyCI(staticmass, staticMs, staticSh, staticinertia);
  btRigidBody* staticBody = new btRigidBody(staticBodyCI);
  world->addRigidBody(staticBody);

  // Hanging sphere
  btCollisionShape* hangSh = new btSphereShape(1);
  btScalar mass = 1;
  btVector3 inertia(0,0,0);
  hangSh->calculateLocalInertia(mass, inertia);
  btDefaultMotionState* hangMs = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,1)));
  btRigidBody::btRigidBodyConstructionInfo hangBodyCI(mass, hangMs, hangSh, inertia);
  btRigidBody* hangBody = new btRigidBody(hangBodyCI);
  world->addRigidBody(hangBody);

  // The slider constraint (made from a 6DOF as on the examples)
  btVector3 sliderWorldPos(0,0,3);
  btVector3 sliderAxis(0,0,1);
  btScalar angle = 0.f;
  btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle));
  btTransform trans;
  trans.setIdentity();
  trans.setOrigin(sliderWorldPos);
  //trans.setBasis(sliderOrientation); // Already commented in example
  btTransform sliderTransform = trans;

  btTransform frameInA, frameInB;
  frameInA = btTransform::getIdentity();
  frameInB = btTransform::getIdentity();
  frameInA.setOrigin(btVector3(0., 0., 5.));
  frameInB.setOrigin(btVector3(0., 0., 5.));

  // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
  bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
  btGeneric6DofConstraint* spSlider6Dof = new btGeneric6DofConstraint(*staticBody, *hangBody, frameInA, frameInB, useLinearReferenceFrameA);
  spSlider6Dof->setLinearLowerLimit(btVector3(0,0,0.1));
  spSlider6Dof->setLinearUpperLimit(btVector3(0,0,1.5));

  //range should be small, otherwise singularities will 'explode' the constraint
  //        spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0));
  //        spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
  //        spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0));
  //        spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0));
  spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
  spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));

  spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
  spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
  spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;


  world->addConstraint(spSlider6Dof);
  spSlider6Dof->setDbgDrawSize(btScalar(5.f));

  for(int i = 0; i < 300; i++)
  {
    world->stepSimulation(1/60.f, 10);
    btTransform tStatic;
    btTransform tHang;
    staticBody->getMotionState()->getWorldTransform(tStatic);
    hangBody->getMotionState()->getWorldTransform(tHang);

    std::cout << "static: " << tStatic.getOrigin().getZ()
          << " hang: " << tHang.getOrigin().getZ() << std::endl;
  }

  delete staticSh, hangSh;
  delete staticMs, hangMs;
  delete staticBody, hangBody;
  delete world;
  delete cSolver;
  delete pCache;
  delete colDis;
  delete colConf;
  return 0;
}