/* ------------------------
* メイン関数
------------------------ */
int main(int argc, char *argv[])
{
    /* txtデータ読み込み */
    LoadTxt("route.txt", routeX, routeY, routeZ, &lineRoute);
    LoadTxt("obstacle.txt", obstX, obstY, obstZ, &lineObst);
    /* ODEの初期化 */
    dInitODE();
    /* 描画関数の設定 */
    setDrawStuff();
    /* ワールド, スペース, 接触点グループの生成 */
    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    contactgroup = dJointGroupCreate(0);
    /* 地面, 重力の生成 */
    ground = dCreatePlane(space,0,0,1,0);
    dWorldSetGravity(world, 0.0, 0.0, -9.8);
    /* CFM, ERPの設定 */
    dWorldSetCFM(world,1e-3);
    dWorldSetERP(world,0.8);
    /* 全方向移動ロボットの生成 */
    t1 = clock();
    MakeBox();
    MakeOmni();
    /* シミュレーションループ */
    dsSimulationLoop(argc,argv,640,480,&fn);
    /* 接触点グループ, スペース, ワールドの破壊, ODEの終了 */
    dJointGroupDestroy(contactgroup);
    dSpaceDestroy(space);
    dWorldDestroy(world);
    dCloseODE();
    return 0;
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dWorldSetAutoDisableFlag (world,1);
  dWorldSetContactMaxCorrectingVel (world,0.1);
  dWorldSetContactSurfaceLayer (world,0.001);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}

int main( int argc, char **argv )
{
	// setup pointers to drawstuff callback functions
	dsFunctions fn;
	fn.version = DS_VERSION;
	fn.start = &start;
	fn.step = &simLoop;
	fn.command = &command;
	fn.stop = 0;
	fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

	// create world
	dInitODE2( 0 );
	world = dWorldCreate();

	space = dSimpleSpaceCreate( 0 );
	contactgroup = dJointGroupCreate( 0 );
	dWorldSetGravity( world,0,0,-0.5 );
	dWorldSetCFM( world,1e-5 );
	dCreatePlane( space,0,0,1,0 );
	memset( obj,0,sizeof( obj ) );

	// run simulation
	dsSimulationLoop( argc,argv,352,288,&fn );

	dJointGroupDestroy( contactgroup );
	dSpaceDestroy( space );
	dWorldDestroy( world );
	dCloseODE();
	return 0;
}

void constructWorldForTest (dReal gravity, int bodycount,
 /* body 1 pos */           dReal pos1x, dReal pos1y, dReal pos1z,
 /* body 2 pos */           dReal pos2x, dReal pos2y, dReal pos2z,
 /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
 /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
 /* rotation angles */      dReal a1, dReal a2)
{
  // create world
  world = dWorldCreate();
  dWorldSetERP (world,0.2);
  dWorldSetCFM (world,1e-6);
  dWorldSetGravity (world,0,0,gravity);

  dMass m;
  dMassSetBox (&m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&m,MASS);

  body[0] = dBodyCreate (world);
  dBodySetMass (body[0],&m);
  dBodySetPosition (body[0], pos1x, pos1y, pos1z);
  dQuaternion q;
  dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
  dBodySetQuaternion (body[0],q);

  if (bodycount==2) {
    body[1] = dBodyCreate (world);
    dBodySetMass (body[1],&m);
    dBodySetPosition (body[1], pos2x, pos2y, pos2z);
    dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
    dBodySetQuaternion (body[1],q);
  }
  else body[1] = 0;
}

CPhysicManager::CPhysicManager(IEngine* _Engine):CBaseObject(_Engine){

    SetParam("BaseClass","PhysicManager");
    SetParam("Type","CPhysicManager");
    SetParam("Name","-");

	dSetErrorHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
    dSetDebugHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
    dSetMessageHandler (SilenceMessage); //затыкаем рот идиотским ошибкам


    calc_time= 0.0f;

	world = dWorldCreate();
	dWorldSetGravity(world, 0, 0, -0.01 );
	space=dHashSpaceCreate(0);
	contactgroup = dJointGroupCreate(0);

    LOGGER->LogMsg("+CPhysicManager");

    ptr_world = world;
    ptr_contactgroup = contactgroup;

    dWorldSetCFM(world, 1e-5);
    dWorldSetERP(world, 0.2);
    dWorldSetContactMaxCorrectingVel(world, 0.9);
    dWorldSetContactSurfaceLayer(world, 0.001);
    dWorldSetAutoDisableFlag(world, 1);

//    ground =

#define SHIRINA 1000

    //dGeomID tmp = dCreateBox( space, SHIRINA, SHIRINA, 1);
    //dGeomSetPosition(tmp, SHIRINA/2, SHIRINA/2, 0); // двигаем центр координат

    //tmp = dCreateBox( space, 1, SHIRINA, SHIRINA);
    //dGeomSetPosition(tmp, 0, SHIRINA/2, SHIRINA/2); // двигаем центр координат

    //tmp = dCreateBox( space, SHIRINA, 1, SHIRINA);
    //dGeomSetPosition(tmp, SHIRINA/2, 0, SHIRINA/2); // двигаем центр координат

    dCreatePlane (space,0,0,1,0);

    //dCreatePlane (space,1,1,0,0);

    //dCreatePlane (space,0,1,0,0);
    //dCreatePlane (space,1,0,0,0);

    //dCreatePlane (space,0,0,20,0);
    //dCreatePlane (space,0,10,0,0);
    //dCreatePlane (space,10,0,0,0);

    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);

};

void Scene::step( void )
{
	//Our internal units are pixels, and at 160dpi there are 6300 pixels per meter
	float gravity[3];
	gravity[0] = (m_motion_lopass[0]*1 + m_motion_hipass[0]*10) * 6300;
	gravity[1] = (m_motion_lopass[1]*1 + m_motion_hipass[1]*10) * 6300;
	gravity[2] = (m_motion_lopass[2]*1 + m_motion_hipass[2]*10) * 6300;
	
	float hisq = m_motion_hipass[0]*m_motion_hipass[0] + m_motion_hipass[1]*m_motion_hipass[1] + m_motion_hipass[2]*m_motion_hipass[2];
	if( hisq > 1.0f )
	{
		int num_woken = 0;
		for( SceneObj *obj_p=m_obj_p; obj_p; obj_p=obj_p->m_next_p )
		{
			if( !dBodyIsEnabled(obj_p->m_dbody) )
			{
				dBodyEnable( obj_p->m_dbody );
				num_woken++;
				if( num_woken == 3 )
					break;
			}
		}
	}

	dWorldSetGravity( m_dworld, gravity[0], gravity[1], gravity[2] );

	do_collision();
	dWorldQuickStep( m_dworld, 1.0f/60.0f );
	dJointGroupEmpty( m_colljoints );
}

int main(int argc, char** argv)
{
    init_gfx();
    init_2d();
 
    // create world and set gravity   
    world = dWorldCreate();
    dWorldSetGravity(world, 0, -5, 0);
    
    // create a body for our ball
    dBodyID body = dBodyCreate(world);
    dBodySetPosition(body, 0, 0, 0);

    for (;;) {  // infinite loop

        // get the position and move there in OpenGL
        const dReal* pos = dBodyGetPosition(body);
        glTranslatef(pos[0], pos[1], pos[2]);
        
        draw_circle();

        // integrate all bodies
        dWorldQuickStep(world, 0.01);

        // redraw screen
        step();
    }
}

// initialize server (supply world dimensions)
bool PhysicsServer::init(int groundSizeX, int groundSizeY)
{
	if (groundSizeX <= 20 || groundSizeY <= 20)
		return false;

	// create global ODE world
	_world = dWorldCreate();

	// create global space
	_space = dHashSpaceCreate(0);

	// create group for handling collisions
	_contactgroup = dJointGroupCreate (0);

	// make ground a bit slippery :)
	_envData.slip = 0.15; 
	// TODO: let user specify ground surface properties

	// set world properties
	dWorldSetGravity (_world,0,0,-(PHYS_GRAVITY*PHYS_MASS_SCALE));
	dWorldSetERP (_world, 0.6);

	// create ground and walls (0,0,0 is the ground's center)
	// and put them in their own space
	_envSpace = dSimpleSpaceCreate(_space);
	_ground = dCreatePlane (_envSpace,0, 0, 1, 0);
	dGeomSetData(_ground, (void*)&_envData);

	_wall[0] = dCreatePlane(_envSpace,  1, 0,  0, -(groundSizeX/2.0) ); // a,b,c,d
	_wall[1] = dCreatePlane(_envSpace, -1, 0,  0, -(groundSizeX/2.0) ); // a,b,c,d
	_wall[2] = dCreatePlane(_envSpace,  0, 1,  0, -(groundSizeY/2.0) ); // a,b,c,d
	_wall[3] = dCreatePlane(_envSpace,  0, -1, 0, -(groundSizeY/2.0) ); // a,b,c,d

	return true;
}

// Called by Physics::Server when the Level is attached to the server.
void CLevel::Activate()
{
	TimeToSim = 0.0;

	// Initialize ODE //???per-Level?
	dInitODE();
	ODEWorldID = dWorldCreate();
	dWorldSetQuickStepNumIterations(ODEWorldID, 20);

	// FIXME(enno): is a quadtree significantly faster? -- can't count geoms with quadtree
	ODECommonSpaceID  = dSimpleSpaceCreate(NULL);
	ODEDynamicSpaceID = dSimpleSpaceCreate(ODECommonSpaceID);
	ODEStaticSpaceID  = dSimpleSpaceCreate(ODECommonSpaceID);

	dWorldSetGravity(ODEWorldID, Gravity.x, Gravity.y, Gravity.z);
	dWorldSetContactSurfaceLayer(ODEWorldID, 0.001f);
	dWorldSetContactMaxCorrectingVel(ODEWorldID, 100.0f);
	dWorldSetERP(ODEWorldID, 0.2f);     // ODE's default value
	dWorldSetCFM(ODEWorldID, 0.001f);    // the default is 10^-5

	// setup autodisabling
	dWorldSetAutoDisableFlag(ODEWorldID, 1);
	dWorldSetAutoDisableSteps(ODEWorldID, 5);
	//dWorldSetAutoDisableTime(ODEWorldID, 1.f);
	dWorldSetAutoDisableLinearThreshold(ODEWorldID, 0.05f);   // default is 0.01
	dWorldSetAutoDisableAngularThreshold(ODEWorldID, 0.1f);  // default is 0.01

	// create a Contact group for joints
	ContactJointGroup = dJointGroupCreate(0);
}

void PhysicsServer::SetGravity( Vec3 g )
{
    m_Gravity = g;
    Vec3 gr( g );
    gr *= m_WorldScale;
    if (m_WorldID) dWorldSetGravity( m_WorldID, gr.x, gr.y, gr.z );
} // PhysicsServer::SetGravity

void RigidBodyEnvironment::createWorld(void)
{
    // BEGIN SETTING UP AN OPENDE ENVIRONMENT
    // ***********************************

    bodyWorld = dWorldCreate();
    space = dHashSpaceCreate(0);

    dWorldSetGravity(bodyWorld, 0, 0, -0.981);

    double lx = 0.2;
    double ly = 0.2;
    double lz = 0.1;

    dMassSetBox(&m, 1, lx, ly, lz);

    boxGeom = dCreateBox(space, lx, ly, lz);
    boxBody = dBodyCreate(bodyWorld);
    dBodySetMass(boxBody, &m);
    dGeomSetBody(boxGeom, boxBody);

    // *********************************
    // END SETTING UP AN OPENDE ENVIRONMENT

    setPlanningParameters();
}

void CPHWorld::SetGravity(float g)
{
	m_gravity				=g;
	dWorldID phWorld		=0;
	dWorldSetGravity		(phWorld, 0,-m_gravity, 0);//-2.f*9.81f

}

void HarrierSim::start2()
{
  //        setDrawStuff();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  ground = dCreatePlane(space, 0, 0, 1, 0);
  dWorldSetGravity(world,0,0,-0.5);

  dWorldSetCFM (world,1e-6);
  dWorldSetERP (world,1);
  //dWorldSetAutoDisableFlag (world,1);
  dWorldSetContactMaxCorrectingVel (world,0.1);
  //set the contact penetration
  dWorldSetContactSurfaceLayer(world, 0.001);


  makeHarrier();

  itsHarrierObject = vp->load3DSObject("./etc/spaceship.3ds", "./etc/textures/spaceshiptexture.ppm");
  itsHarrierObject.scale = 0.1;


  //set the viewpoint
  double xyz[3] = {0 , 25.0, 20};
  double hpr[3] = {-90.0,-35,0.0};
  vp->dsSetViewpoint (xyz,hpr);
  vp->setZoom(1.5);

}

    /**
     *  \brief This function initializes the ode world.
     *
     * pre:
     *     - world_init = false
     *
     * post:
     *     - world, space and contactgroup should be created
     *     - the ODE world parameters should be set
     *     - at the end world_init have to become true
     */
    void WorldPhysics::initTheWorld(void) {
      MutexLocker locker(&iMutex);
  
      // if world_init = true debug something
      if (!world_init) {
        //LOG_DEBUG("init physics world");
        world = dWorldCreate();
        space = dHashSpaceCreate(0);
        contactgroup = dJointGroupCreate(0);

        old_gravity = world_gravity;
        old_cfm = world_cfm;
        old_erp = world_erp;

        dWorldSetGravity(world, world_gravity.x(), world_gravity.y(), world_gravity.z()); 
        dWorldSetCFM(world, (dReal)world_cfm);
        dWorldSetERP (world, (dReal)world_erp);

        dWorldSetAutoDisableFlag (world,0);
        // if usefull for some tests a ground can be created here
        plane = 0; //dCreatePlane (space,0,0,1,0);
        world_init = 1;
        drawStruct draw;
        draw.ptr_draw = (DrawInterface*)this;
        if(control->graphics)
          control->graphics->addDrawItems(&draw);
      }
      //printf("initTheWorld..\n");
    }

void Simulator_Initialize(void) {

	dInitODE2(0);
	world = dWorldCreate();
	space = dHashSpaceCreate (0);
	contactgroup = dJointGroupCreate (0);
	dWorldSetGravity (world,0,0,-0.5);
	ground = dCreatePlane (space,0,0,1,0);
}

int main (int argc, char **argv)
{
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

    // create world
    dInitODE();
    world = dWorldCreate();

#if 1
    space = dHashSpaceCreate (0);
#elif 0
    dVector3 center = {0,0,0}, extents = { 100, 100, 100};
    space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
    space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
    space = dSimpleSpaceCreate(0);
#endif

    contactgroup = dJointGroupCreate (0);
    dWorldSetGravity (world,0,0,-0.5);
    dWorldSetCFM (world,1e-5);
    
    dWorldSetLinearDamping(world, 0.00001);
    dWorldSetAngularDamping(world, 0.005);
    dWorldSetMaxAngularSpeed(world, 200);

    dWorldSetContactSurfaceLayer (world,0.001);
    ground = dCreatePlane (space,0,0,1,0);
    
    memset (obj,0,sizeof(obj));

    // create lift platform
    platform = dCreateBox(space, 4, 4, 1);

    dGeomSetCategoryBits(ground, 1ul);
    dGeomSetCategoryBits(platform, 2ul);
    dGeomSetCollideBits(ground, ~2ul);
    dGeomSetCollideBits(platform, ~1ul);

    // run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);

    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

void ODEDomain::setGravity(double x, double y, double z)
{
    /*gravity_vector[0]=x;
    gravity_vector[1]=y;
    gravity_vector[2]=z;*/
    
    BaseDomain::setGravity(x,y,z);
 
    dWorldSetGravity(world,x,y,z);
}

WorldManagerServer::WorldManagerServer()
{
	dInitODE();
	mWorld = dWorldCreate();
	mGlobalSpace = dHashSpaceCreate(0);
	mContactGroup = dJointGroupCreate(0);
	dWorldSetGravity(mWorld, 0, -100, 0);

	mStaticSpace = dSimpleSpaceCreate(mGlobalSpace);
}

int main(int argc, char *argv[]) {
    dsFunctions fn;
    double x[4] = {0.00}, y[4] = {0.00};  // Center of gravity
    double z[4] = { 0.05, 0.50, 1.50, 2.55};

    double m[4] = {10.00, 2.00, 2.00, 2.00};       // mass

    double anchor_x[4]  = {0.00}, anchor_y[4] = {0.00};// anchors of joints
    double anchor_z[4] = { 0.00, 0.10, 1.00, 2.00};

    double axis_x[4]  = { 0.00, 0.00, 0.00, 0.00};  // axises of joints
    double axis_y[4]  = { 0.00, 0.00, 1.00, 1.00};
    double axis_z[4]  = { 1.00, 1.00, 0.00, 0.00};

    fn.version = DS_VERSION;
    fn.start   = &init;
    fn.step    = &simLoop;
    fn.command = &command;
    fn.path_to_textures = "drawstuff_texture";

    dInitODE();  // Initialize ODE
    world = dWorldCreate();  // Create a world
    dWorldSetGravity(world, 0, 0, -9.8);

    bool isDrawingEnabled = true;

    Leg leg(world);
    LegDraw legDraw(&leg);
    
    /*
    for (int i = 0; i < NUM; i++) {
    dMass mass;
    link[i] = dBodyCreate(world);
    dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position
    dMassSetZero(&mass);      // Set mass parameter to zero
    dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]);  // Calculate mass parameter
    dBodySetMass(link[i], &mass);  // Set mass
    }

    joint[0] = dJointCreateFixed(world, 0); // A fixed joint
    dJointAttach(joint[0], link[0], 0);     // Attach the joint between the ground and the base
    dJointSetFixed(joint[0]);               // Set the fixed joint

    for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint
    dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint
    dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]);
    }*/

    dsSimulationLoop(argc, argv, 640, 480, &fn); //　Simulation loop

    dCloseODE();
    return 0;
}