static void physics_thread()
	{	
		/* Now initialize the physics engine. */
		cpInitChipmunk();
		space = cpSpaceNew();	
	
		do
		{
			/* But here we do care about the elapsed time. */
			synchronize(&oldtime /*,&server_elapsed*/);

			simple_lock(&physics_lock);

			physics(world, space);
			cpSpaceHashEach(space->activeShapes, &draw_player, NULL);
			//cpSpaceHashEach(space->staticShapes, &draw_static, NULL);

			simple_unlock(&physics_lock);

			if(to_remove_array.length > 0)
				lDestroy(&to_remove_array, remove_unused);
		}
		while(!physics_exit_time);

		lFree(&to_remove_array);
		cpSpaceFreeChildren(space);
		cpSpaceFree(space);	
	}

World_t *world_create(void)
{
    World_t *out = obj_create_autoreleased(&Class_World);
    out->cpSpace = cpSpaceNew();
    out->cpSpace->data = out;
    cpSpaceAddCollisionHandler(out->cpSpace, 0, 0,
                               collisionWillBegin,
                               NULL,
                               collisionDidBegin,
                               collisionDidEnd, NULL);
    out->entities = obj_retain(llist_create((InsertionCallback_t)&obj_retain, &obj_release));

    // Create the static entity
    out->staticEntity = obj_create(&Class_WorldEntity);
    out->staticEntity->world = out;
    out->staticEntity->owner = out;
    out->staticEntity->cpBody = out->cpSpace->staticBody;
    out->staticEntity->luaUpdateHandler = -1;
    out->staticEntity->luaPreCollisionHandler = -1;
    out->staticEntity->luaCollisionHandler = -1;
    out->staticEntity->luaPostCollisionHandler = -1;
    out->cpSpace->staticBody->data = out->staticEntity;
    out->staticEntity->shapes = obj_retain(llist_create((InsertionCallback_t)&obj_retain, &obj_release));

    return out;
}

World::World(cpVect size) {
	space=cpSpaceNew();

	static_body=cpBodyNewStatic();

	printf("new space\n"); fflush(stdout);

	cpVect s2=cpvmult(size,0.5);

	cpVect c1=s2;
	cpVect c2=cpv(s2.x,-s2.y);
	cpVect c3=cpv(-s2.x,-s2.y);
	cpVect c4=cpv(-s2.x,s2.y);

	printf("foo space\n"); fflush(stdout);

	cpBodyInitStatic(space->staticBody);

	addStaticLine(c1,c2);
	addStaticLine(c2,c3);
	addStaticLine(c3,c4);
	addStaticLine(c4,c1);

	printf("space done\n"); fflush(stdout);
}

cpSpace *Slice::Init()
{
    ChipmunkDemo::Init();

    message = "Hold right bottom corner and slice with touch.";

    space = cpSpaceNew();
    cpSpaceSetIterations(space, 30);
    cpSpaceSetGravity(space, cpv(0, -500));
    cpSpaceSetSleepTimeThreshold(space, 0.5f);
    cpSpaceSetCollisionSlop(space, 0.5f);

    cpBody *body, *staticBody = cpSpaceGetStaticBody(space);
    cpShape *shape;

    // Create segments around the edge of the screen.
    shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-1000,-240), cpv(1000,-240), 0.0f));
    cpShapeSetElasticity(shape, 1.0f);
    cpShapeSetFriction(shape, 1.0f);
    cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);

    cpFloat width = 200.0f;
    cpFloat height = 300.0f;
    cpFloat mass = width*height*DENSITY;
    cpFloat moment = cpMomentForBox(mass, width, height);

    body = cpSpaceAddBody(space, cpBodyNew(mass, moment));

    shape = cpSpaceAddShape(space, cpBoxShapeNew(body, width, height, 0.0));
    cpShapeSetFriction(shape, 0.6f);
	
	return space;
}

static cpSpace *initSpace(void) {
    int i;
    cpBody *staticBody;
    cpShape *shape;
    cpVect cannonPos;

    cpResetShapeIdCounter();

    g_Space = cpSpaceNew();
    g_Space->iterations = 30;
    g_Space->gravity = cpv(0, -300);

    staticBody = &g_Space->staticBody;
    shape = cpSpaceAddShape(g_Space, cpSegmentShapeNew(staticBody, cpv(-400,-290), cpv(-400,300), 0.0f));
    shape->e = 1.0f; shape->u = 0.0f;
    shape->collision_type = PLATFORM_TYPE;

    cannonPos = cpv(-350, -215);
    g_Cannon = createCannon(cannonPos, 30.0f, 6.0f);
    g_Cannon->ai = 0;
    for (i = 0; i < MAX_PROJECTILES; ++i) {
        g_Cannon->ammo[i] = createProjectile(6.0f, 1.0f);
    }

    platforms[0] = createPlatform(staticBody, cpv(-390, -240), cpv(1600, -240), 10.0f);

    fprintf(stderr, "Loading dominoes disabled\n");
    InitializeDominoes();

    cpSpaceAddCollisionHandler(g_Space, PROJECTILE_TYPE, DOMINO_OBJECT_TYPE,
                               NULL, NULL, postSolveProjectileDomino, NULL, NULL);

    return g_Space;
}

int l_physics_newSpace(lua_State* state)
{

    moduleData.onBegin = false;
    moduleData.onPostSolve = false;
    moduleData.onPreSolve = false;
    moduleData.onSeparate = false;

    //moduleData.letBeginCollide = true;
    //moduleData.letPreSolveCollide = true;

    float x = l_tools_toNumberOrError(state, 1);
    float y = l_tools_toNumberOrError(state, 2);

    moduleData.physics = (l_physics_PhysicsData*)lua_newuserdata(state, sizeof(l_physics_PhysicsData));
    moduleData.physics->physics = malloc(sizeof(physics_PhysicsData));

    moduleData.physics->physics->space = cpSpaceNew();
    moduleData.physics->physics->gravity = cpv(x, y);
    cpSpaceSetGravity(moduleData.physics->physics->space, moduleData.physics->physics->gravity);

    lua_rawgeti(state, LUA_REGISTRYINDEX, moduleData.physicsMT);
    lua_setmetatable(state, -2);

    return 1;
}

bool PhysicsWorld::init()
{
    do
    {
#if CC_TARGET_PLATFORM == CC_PLATFORM_WINRT || CC_TARGET_PLATFORM == CC_PLATFORM_WIN32
		_cpSpace = cpSpaceNew();
#else
        _cpSpace = cpHastySpaceNew();
        cpHastySpaceSetThreads(_cpSpace, 0);
#endif
        CC_BREAK_IF(_cpSpace == nullptr);
        
        cpSpaceSetGravity(_cpSpace, PhysicsHelper::point2cpv(_gravity));
        
        cpCollisionHandler *handler = cpSpaceAddDefaultCollisionHandler(_cpSpace);
        handler->userData = this;
        handler->beginFunc = (cpCollisionBeginFunc)PhysicsWorldCallback::collisionBeginCallbackFunc;
        handler->preSolveFunc = (cpCollisionPreSolveFunc)PhysicsWorldCallback::collisionPreSolveCallbackFunc;
        handler->postSolveFunc = (cpCollisionPostSolveFunc)PhysicsWorldCallback::collisionPostSolveCallbackFunc;
        handler->separateFunc = (cpCollisionSeparateFunc)PhysicsWorldCallback::collisionSeparateCallbackFunc;
        
        return true;
    } while (false);
    
    return false;
}

static cpSpace *
init(void)
{
	ChipmunkDemoMessageString = "Right click and drag to change the blocks's shape.";
	
	cpSpace *space = cpSpaceNew();
	cpSpaceSetIterations(space, 30);
	cpSpaceSetGravity(space, cpv(0, -500));
	cpSpaceSetSleepTimeThreshold(space, 0.5f);
	cpSpaceSetCollisionSlop(space, 0.5f);
	
	cpBody *body, *staticBody = cpSpaceGetStaticBody(space);
	
	// Create segments around the edge of the screen.
	shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
	cpShapeSetElasticity(shape, 1.0f);
	cpShapeSetFriction(shape, 1.0f);
	cpShapeSetLayers(shape, NOT_GRABABLE_MASK);

	cpFloat width = 50.0f;
	cpFloat height = 70.0f;
	cpFloat mass = width*height*DENSITY;
	cpFloat moment = cpMomentForBox(mass, width, height);
	
	body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
	
	shape = cpSpaceAddShape(space, cpBoxShapeNew(body, width, height));
	cpShapeSetFriction(shape, 0.6f);
		
	return space;
}

void eol_level_clear_layer_space(eolLevelLayer *layer)
{
  if (!layer)
  {
    return;
  }
  if (layer->space != NULL)
  {
    cpSpaceFree(layer->space);
  }
  layer->space = cpSpaceNew();
  if (layer->space == NULL)
  {
    eol_logger_message(
      EOL_LOG_ERROR,
      "Unable to create a physics space for new layer!");
    eol_level_delete_layer(layer);
    return;
  }
  layer->space->iterations = _eol_level_clip_iterations;
  layer->space->sleepTimeThreshold = 999999;
  cpSpaceSetEnableContactGraph(layer->space,eolTrue);
  cpSpaceSetCollisionSlop(layer->space, _eol_level_slop);
  cpSpaceSetCollisionBias(layer->space, _eol_level_bias);

}

MachineSystem::MachineSystem(int width, int height, int hPegs, int vPegs, cpVect position)
: parts(hPegs*vPegs),
  attachments((hPegs*vPegs), std::vector<Attachment *>(hPegs*vPegs, NULL)),
  space(cpSpaceNew()),
  nMachines(0),
  nAttachments(0)
{
    cpSpaceSetIterations(space, 20);
    gridSpacing = cpv((float)width/(hPegs + 1), (float)height/(vPegs + 1));
    size = cpv(hPegs, vPegs);
    body = cpBodyNewStatic();
    cpBodySetPos(body, position);
    
    cpShape *wallShape = cpSegmentShapeNew(body, cpv(-width/2, height/2), cpv(width/2, height/2), .5);
    cpShapeSetLayers(wallShape, WALL_LAYER);
    cpSpaceAddStaticShape(space, wallShape);
    
    wallShape = cpSegmentShapeNew(body, cpv(-width/2, -height/2), cpv(+width/2, -height/2), 0.5);
    cpShapeSetLayers(wallShape, WALL_LAYER);
    cpSpaceAddStaticShape(space, wallShape);
    
    wallShape = cpSegmentShapeNew(body, cpv(-width/2, +height/2), cpv(-width/2, -height/2), 0.5);
    cpShapeSetLayers(wallShape, WALL_LAYER);
    cpSpaceAddStaticShape(space, wallShape);
    
    wallShape = cpSegmentShapeNew(body, cpv(+width/2, +height/2), cpv(+width/2, -height/2), 0.5);
    cpShapeSetLayers(wallShape, WALL_LAYER);
    cpSpaceAddStaticShape(space, wallShape);
    
    inputMachinePosition = cpv(-1,-1);
    outputMachinePosition = cpv(-1,-1);
    
}

VALUE
make_rp_space(VALUE mod) {
    cpSpace *space = cpSpaceNew();

//    printf("rpm: integrations: %d\n", space->iterations);

    return (VALUE)space;
}

static VALUE
space_allocate(VALUE cls) {
    cpSpace *space = cpSpaceNew();
//    printf("new called: %d, %p\n", space->iterations, space);
//    printf("rpm: integrations: %d\n", space->iterations);

    return Data_Wrap_Struct(cls, space_mark, space_free, space);
}

Physics::Physics(uint32 timeStep) {
    // Create an empty space.
    mSpace = cpSpaceNew();
    cpSpaceSetGravity(mSpace, cpv(0, 0));
    cpSpaceSetDamping(mSpace, 0.5);
    mTimeStep = timeStep;
    mAccumulator = 0;
}

PhysicsWorld::PhysicsWorld()
{
    mySpace = cpSpaceNew();
    
    // Set default gravity
    cpVect gravity = cpv(0, -10);
    cpSpaceSetGravity(mySpace, gravity);
}

Space::Space(void)
	: space(cpSpaceNew()),
	  data(0),
	  body(new cp::Body(cpSpaceGetStaticBody(space)))
{
		space->data = this;
		cpBodySetUserData(cpSpaceGetStaticBody(space), body);
}

static cpSpace *
init(void)
{
	space = cpSpaceNew();
	cpSpaceSetGravity(space, cpv(0, -600));
	
	cpBody *body;
	cpShape *shape;
	
	// We create an infinite mass rogue body to attach the line segments too
	// This way we can control the rotation however we want.
	rogueBoxBody = cpBodyNew(INFINITY, INFINITY);
	cpBodySetAngVel(rogueBoxBody, 0.4f);
	
	// Set up the static box.
	cpVect a = cpv(-200, -200);
	cpVect b = cpv(-200,  200);
	cpVect c = cpv( 200,  200);
	cpVect d = cpv( 200, -200);
	
	shape = cpSpaceAddShape(space, cpSegmentShapeNew(rogueBoxBody, a, b, 0.0f));
	cpShapeSetElasticity(shape, 1.0f);
	cpShapeSetFriction(shape, 1.0f);
	cpShapeSetLayers(shape, NOT_GRABABLE_MASK);

	shape = cpSpaceAddShape(space, cpSegmentShapeNew(rogueBoxBody, b, c, 0.0f));
	cpShapeSetElasticity(shape, 1.0f);
	cpShapeSetFriction(shape, 1.0f);
	cpShapeSetLayers(shape, NOT_GRABABLE_MASK);

	shape = cpSpaceAddShape(space, cpSegmentShapeNew(rogueBoxBody, c, d, 0.0f));
	cpShapeSetElasticity(shape, 1.0f);
	cpShapeSetFriction(shape, 1.0f);
	cpShapeSetLayers(shape, NOT_GRABABLE_MASK);

	shape = cpSpaceAddShape(space, cpSegmentShapeNew(rogueBoxBody, d, a, 0.0f));
	cpShapeSetElasticity(shape, 1.0f);
	cpShapeSetFriction(shape, 1.0f);
	cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
	
	cpFloat mass = 1;
	cpFloat width = 60;
	cpFloat height = 30;
	
	// Add the bricks.
	for(int i=0; i<3; i++){
		for(int j=0; j<7; j++){
			body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForBox(mass, width, height)));
			cpBodySetPos(body, cpv(i*60 - 150, j*30 - 150));
			
			shape = cpSpaceAddShape(space, cpBoxShapeNew(body, width, height));
			cpShapeSetElasticity(shape, 0.0f);
			cpShapeSetFriction(shape, 0.7f);
		}
	}
	
	return space;
}

void GPlayState::Init()
{
    srand(time(NULL));
    stepSize = 1.0 / 60.0;
    space = cpSpaceNew();
    cpSpaceSetIterations(space, 6);

	arena = Arena(space);

    if(!player1)
        player1 = new Fighter();
    if(!player2)
        player2 = new Fighter();

    player1->Init(space, 250, 400, Character::RIGHT, sf::Color(32, 32, 200));
    player2->Init(space, 1050, 400, Character::LEFT, sf::Color(200, 32, 32));

    player1->setOponent(player2);
    player2->setOponent(player1);

	inputManager->addPlayer(player1);
	inputManager->addPlayer(player2);

    hud = HUD(player1, player2);

    if(!font.loadFromFile("rec/RevoPop.ttf")) {
        fprintf(stderr, "could not load font\n");
    }

    p1ScoreT.setFont(font);
    p1ScoreT.setString("0");
    p1ScoreT.setCharacterSize(32);
    p1ScoreT.setColor(sf::Color(32, 32, 32));
    p1ScoreT.setPosition(556, 344);

    p2ScoreT.setFont(font);
    p2ScoreT.setString("0");
    p2ScoreT.setCharacterSize(32);
    p2ScoreT.setColor(sf::Color(32, 32, 32));
    p2ScoreT.setPosition(556, 344);

    winner.setFont(font);
    winner.setCharacterSize(32);
    winner.setColor(sf::Color(32, 32, 32));
    winner.setPosition(480, 344);

    reseting = true;
    resetTime.restart();
    winner.setString("Beginning Round 1");
    round = 1;
    p1Score = 0;
    p2Score = 0;

    int ultPlayer = rand() % 2;
    ultClock.restart();

	printf("GPlayState Init\n");
}

Scene* scene_create() {
    Scene* scene = malloc(sizeof(*scene));
    scene->space = cpSpaceNew();
    scene->entities = entitypool_create();
    scene->transforms = transformpool_create();
    scene->sprites = spritepool_create();
    scene->scripts = scriptpool_create();
    return scene;
}

static cpSpace *
init(void)
{
	staticBody = cpBodyNew(INFINITY, INFINITY);
	
	cpResetShapeIdCounter();
	
	space = cpSpaceNew();
	cpSpaceResizeActiveHash(space, 30.0f, 1000);
	space->iterations = 10;
	
	cpShape *shape;
		
	// Create segments around the edge of the screen.
	shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
	shape->e = 1.0f; shape->u = 1.0f;
	shape->layers = NOT_GRABABLE_MASK;

	shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
	shape->e = 1.0f; shape->u = 1.0f;
	shape->layers = NOT_GRABABLE_MASK;

	shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
	shape->e = 1.0f; shape->u = 1.0f;
	shape->layers = NOT_GRABABLE_MASK;

	shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,240), cpv(320,240), 0.0f));
	shape->e = 1.0f; shape->u = 1.0f;
	shape->layers = NOT_GRABABLE_MASK;
	
	for(int i=0; i<50; i++){
		cpBody *body = add_box(10.0, 1.0);
		
		cpConstraint *pivot = cpSpaceAddConstraint(space, cpPivotJointNew2(staticBody, body, cpvzero, cpvzero));
		pivot->biasCoef = 0.0f; // disable joint correction
		pivot->maxForce = 1000.0f; // emulate linear friction
		
		cpConstraint *gear = cpSpaceAddConstraint(space, cpGearJointNew(staticBody, body, 0.0f, 1.0f));
		gear->biasCoef = 0.0f; // disable joint correction
		gear->maxForce = 5000.0f; // emulate angular friction
	}
	
	// We joint the tank to the control body and control the tank indirectly by modifying the control body.
	tankControlBody = cpBodyNew(INFINITY, INFINITY);
	tankBody = add_box(15.0, 10.0);
	
	cpConstraint *pivot = cpSpaceAddConstraint(space, cpPivotJointNew2(tankControlBody, tankBody, cpvzero, cpvzero));
	pivot->biasCoef = 0.0f; // disable joint correction
	pivot->maxForce = 10000.0f; // emulate linear friction
	
	cpConstraint *gear = cpSpaceAddConstraint(space, cpGearJointNew(tankControlBody, tankBody, 0.0f, 1.0f));
	gear->biasCoef = 1.0f; // limit angular correction rate
	gear->maxBias = 1.0f; // limit angular correction rate
	gear->maxForce = 500000.0f; // emulate angular friction
		
	return space;
}

// Init is called by the demo code to set up the demo.
static cpSpace *
init(void)
{
	// Create an infinite mass body to attach ground segments and other static geometry to.
	// We won't be adding this body to the space, because we don't want it to be simulated at all.
	// Adding bodies to the space simulates them. (fall under the influence of gravity, etc)
	// We want the static body to stay right where it is at all times.
	staticBody = cpBodyNew(INFINITY, INFINITY);
	
	// Create a space, a space is a simulation world. It simulates the motions of rigid bodies,
	// handles collisions between them, and simulates the joints between them.
	space = cpSpaceNew();
	
	// Lets set some parameters of the space:
	// More iterations make the simulation more accurate but slower
	space->iterations = 10;
	// These parameters tune the efficiency of the collision detection.
	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpSpace
	cpSpaceResizeStaticHash(space, 30.0f, 1000);
	cpSpaceResizeActiveHash(space, 30.0f, 1000);
	// Give it some gravity
	space->gravity = cpv(0, -100);
	
	// Create A ground segment along the bottom of the screen
	cpShape *ground = cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f);
	// Set some parameters of the shape.
	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpShape
	ground->e = 1.0f; ground->u = 1.0f;
	ground->layers = NOT_GRABABLE_MASK; // Used by the Demo mouse grabbing code
	// Add the shape to the space as a static shape
	// If a shape never changes position, add it as static so Chipmunk knows it only needs to
	// calculate collision information for it once when it is added.
	// Do not change the postion of a static shape after adding it.
	cpSpaceAddStaticShape(space, ground);
	
	// Add a moving circle object.
	cpFloat radius = 15.0f;
	cpFloat mass = 10.0f;
	// This time we need to give a mass and moment of inertia when creating the circle.
	cpBody *ballBody = cpBodyNew(mass, cpMomentForCircle(mass, 0.0f, radius, cpvzero));
	// Set some parameters of the body:
	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpBody
	ballBody->p = cpv(0, -240 + radius+5);
	// Add the body to the space so it will be simulated and move around.
	cpSpaceAddBody(space, ballBody);
	
	
	// Add a circle shape for the ball.
	// Shapes are always defined relative to the center of gravity of the body they are attached to.
	// When the body moves or rotates, the shape will move with it.
	// Additionally, all of the cpSpaceAdd*() functions return the thing they added so you can create and add in one go.
	cpShape *ballShape = cpSpaceAddShape(space, cpCircleShapeNew(ballBody, radius, cpvzero));
	ballShape->e = 0.0f; ballShape->u = 0.9f;
	
	return space;
}