#include <ode/ode.h>
 
#include <drawstuff/drawstuff.h>
 
 
 
// select correct drawing functions
 
#ifdef dDOUBLE
 
#define dsDrawBox dsDrawBoxD
 
#define dsDrawSphere dsDrawSphereD
 
#define dsDrawCylinder dsDrawCylinderD
 
#define dsDrawCapsule dsDrawCapsuleD
 
#endif
 
 
 
static dWorldID world;
 
static dSpaceID space;
 
static dJointGroupID contactgroup;
 
static dGeomID ground;
 
static dBodyID body_up,body_low;
 
static dGeomID box_up,box_low;
 
static dJointID joint_up2low;
 
static dJointID joint_up2env;
 
static dSpaceID geom_group;
 
 
 
// box location, size, mass
 
double box_up_loc[3]   = {0  , 0  , 1  };
 
double box_low_loc[3]  = {0.4, 0  , 1  };
 
double box_up_size[3]  = {0.4, 0.1, 0.1};
 
double box_low_size[3] = {0.4, 0.1, 0.1};
 
double box_up_mass=1.0, box_low_mass=1.0;
 
 
 
// joint location
 
double joint_up2low_x, joint_up2low_y, joint_up2low_z, 
 
  joint_up2env_x, joint_up2env_y, joint_up2env_z;
 
 
 
#define DT 0.001
 
#define PI 3.14159
 
 
 
// start simulation - set viewpoint
 
static void start()
 
{
 
  static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
 
  static float hpr[3] = {136.5000f,0.5000f,0.0000f};
 
  dsSetViewpoint (xyz,hpr);
 
  printf ("start simulation\n");
 
}
 
 
 
// control
 
double theta1, theta2;
 
//double omega1, omega2;
 
int controler_cnt=0;
 
void controler(){
 
  controler_cnt++;
 
  //--- get angular
 
  theta1 = dJointGetHingeAngle (joint_up2env);
 
  theta2 = dJointGetHingeAngle (joint_up2low);
 
  //omega1 = dJointGetHingeAngleRate (joint_up2env);
 
  //omega2 = dJointGetHingeAngleRate (joint_up2low);
 
 
 
  //--- control
 
  // velocity contorl
 
  double v;
 
  v = ( (2*PI * sin( 2*PI*(controler_cnt * DT) ))
 
	- PI/2 - theta1 );
 
  dJointSetHingeParam (joint_up2env,dParamVel,v);
 
  dJointSetHingeParam (joint_up2env,dParamFMax,20000);
 
  // torque control
 
  //dJointAddHingeTorque(joint_up2env,1);
 
}
 
 
 
// simulation loop
 
double t=0;
 
static void simLoop (int pause)
 
{
 
  int i,j;
 
  if (!pause) {
 
    dWorldStep (world,DT);
 
    // remove all contact joints
 
    dJointGroupEmpty (contactgroup);
 
    // control
 
    controler();
 
    // print real time
 
    //if( t > 1 && fmod(t,1) < DT ){ printf("%f sec\n",t); }
 
    t += DT;
 
  }
 
 
 
  dsSetTexture (DS_WOOD);
 
  dReal sides[3];
 
 
 
  dsSetColor (1,0,0);
 
  for(i=0; i<3; i++){ sides[i]=box_up_size[i]; }
 
  dsDrawBox( dBodyGetPosition(body_up),
 
	     dBodyGetRotation(body_up),
 
	     sides);
 
  dsSetColor (0,1,0);
 
  for(i=0; i<3; i++){ sides[i]=box_low_size[i]; }
 
  dsDrawBox( dBodyGetPosition(body_low),
 
	     dBodyGetRotation(body_low),
 
	     sides);
 
}
 
 
 
int main (int argc, char **argv)
 
{
 
  dMass m;
 
 
 
  //--- setup pointers to drawstuff callback functions
 
  dsFunctions fn;
 
  fn.version = DS_VERSION;
 
  fn.start = &start;
 
  fn.step = &simLoop;
 
  fn.stop = 0;
 
  fn.path_to_textures = "../simulator/ode-0.9/drawstuff/textures";
 
  if(argc==2){ fn.path_to_textures = argv[1]; }
 
 
 
  //--- create world
 
  world = dWorldCreate();
 
  space = dHashSpaceCreate (0);
 
  contactgroup = dJointGroupCreate (0);
 
  dWorldSetGravity (world,0,0,-9.8);
 
  ground = dCreatePlane (space,0,0,1,0);
 
 
 
  //--- create pendulum arm
 
  body_up  = dBodyCreate(world);
 
  body_low = dBodyCreate(world);
 
  dBodySetPosition( body_up,
 
		    box_up_loc[0], box_up_loc[1], box_up_loc[2]);
 
  dBodySetPosition( body_low,
 
		    box_low_loc[0], box_low_loc[1], box_low_loc[2]);
 
  dMassSetBox(&m,1,box_up_size[0], box_up_size[1], box_up_size[2] );
 
  dMassSetBox(&m,1,box_low_size[0], box_low_size[1], box_low_size[2] );
 
  dMassAdjust(&m,box_up_mass);
 
  dMassAdjust(&m,box_low_mass);
 
  dBodySetMass(body_up,&m);
 
  dBodySetMass(body_low,&m);
 
  box_up  = dCreateBox(0,box_up_size[0], box_up_size[1], box_up_size[2]);
 
  box_low = dCreateBox(0,box_low_size[0], box_low_size[1], box_low_size[2]);
 
  dGeomSetBody ( box_up, body_up );
 
  dGeomSetBody ( box_low, body_low );
 
 
 
  //--- create joint
 
  dVector3 a;
 
  dBodyGetRelPointPos(body_up,box_up_size[0]/2,0,0,a);
 
  joint_up2low_x=a[0];
 
  joint_up2low_y=a[1];
 
  joint_up2low_z=a[2];
 
  dBodyGetRelPointPos(body_up,-box_up_size[0]/2,0,0,a);
 
  joint_up2env_x=a[0];
 
  joint_up2env_y=a[1];
 
  joint_up2env_z=a[2];
 
 
 
  //--- joint up to low
 
  joint_up2low = dJointCreateHinge (world,0);
 
  dJointAttach (joint_up2low,body_up,body_low);
 
  dJointSetHingeAnchor (joint_up2low,
 
			joint_up2low_x,joint_up2low_y,joint_up2low_z);
 
  dJointSetHingeAxis ( joint_up2low,0,1,0);
 
  dJointSetHingeParam (joint_up2low,dParamLoStop,-dInfinity);
 
  dJointSetHingeParam (joint_up2low,dParamHiStop,dInfinity);
 
  dJointSetHingeParam (joint_up2low,dParamFudgeFactor,0.1);
 
  //--- joint up to evnvironment
 
  joint_up2env = dJointCreateHinge (world,0);
 
  dJointAttach (joint_up2env, body_up,0);
 
  dJointSetHingeAnchor (joint_up2env,
 
			joint_up2env_x,joint_up2env_y,joint_up2env_z);
 
  dJointSetHingeAxis ( joint_up2env,0,1,0);
 
  dJointSetHingeParam (joint_up2env,dParamLoStop,-dInfinity);
 
  dJointSetHingeParam (joint_up2env,dParamHiStop,dInfinity);
 
  dJointSetHingeParam (joint_up2env,dParamFudgeFactor,0.1);
 
 
 
  //--- add pendulum to world
 
  geom_group = dSimpleSpaceCreate (space);
 
  dSpaceSetCleanup (geom_group,0);
 
  dSpaceAdd(geom_group,box_up);
 
  dSpaceAdd(geom_group,box_low);
 
 
 
  //--- run simulation
 
  dsSimulationLoop (argc,argv,640,480,&fn);
 
 
 
  //--- destroy
 
  dGeomDestroy(box_up);
 
  dGeomDestroy(box_low);
 
  dJointGroupDestroy (contactgroup);
 
  dSpaceDestroy (space);
 
  dWorldDestroy (world);
 
 
 
  return 0;
 
}