void Mirror::SetRotationAngle(float a_Angle)
{
	//rotate physbodies
	cpBodySetAngle(m_pBody, cpFloat(a_Angle));
	cpBodySetAngle(m_pReflectBody, cpFloat(a_Angle));

	//rotate sprite
	m_Sprite.sprite->setRotation( (a_Angle * 180) / 3.14159265f );
}

 bool CDynamics2DBoxEntity::MoveTo(const CVector3& c_position,
                                       const CQuaternion& c_orientation,
                                       bool b_check_only) {
    SInt32 nCollision;
    /* Check whether the box is movable or not */
    if(m_cBoxEntity.GetEmbodiedEntity().IsMovable()) {
       /* The box is movable */
       /* Save body position and orientation */
       cpVect tOldPos = m_ptBody->p;
       cpFloat fOldA = m_ptBody->a;
       /* Move the body to the desired position */
       m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
       CRadians cXAngle, cYAngle, cZAngle;
       c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
       cpBodySetAngle(m_ptBody, cZAngle.GetValue());
       /* Create a shape sensor to test the movement */
       /* First construct the vertices */
       CVector3 cHalfSize = m_cBoxEntity.GetSize() * 0.5f;
       cpVect tVertices[] = {
          cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
          cpv(-cHalfSize.GetX(),  cHalfSize.GetY()),
          cpv( cHalfSize.GetX(),  cHalfSize.GetY()),
          cpv( cHalfSize.GetX(), -cHalfSize.GetY())
       };
       /* Then create the shape itself */
       cpShape* ptTestShape = cpPolyShapeNew(m_ptBody,
                                             4,
                                             tVertices,
                                             cpvzero);
       /* Check if there is a collision */
       nCollision = cpSpaceShapeQuery(m_cEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
       /* Dispose of the sensor shape */
       cpShapeFree(ptTestShape);
       if(b_check_only || nCollision) {
          /* Restore old body state if there was a collision or
             it was only a check for movement */
          m_ptBody->p = tOldPos;
          cpBodySetAngle(m_ptBody, fOldA);
       }
       else {
          /* Update the active space hash if the movement is actual */
          cpSpaceReindexShape(m_cEngine.GetPhysicsSpace(), m_ptShape);
       }
    }
    else {
       /* The box is not movable, so you can't move it :-) */
       nCollision = 1;
    }
    /* The movement is allowed if there is no collision */
    return !nCollision;
 }

void RCPBody::setAngle(float a)
{
    mAngle = a;
    if (mBody) {
        cpBodySetAngle(mBody, degToRad(mAngle));
    }
}

void PhysicsBody::setRotation(float rotation)
{
    if (!_rotationResetTag)
    {
        cpBodySetAngle(_info->getBody(), -PhysicsHelper::float2cpfloat((rotation + _rotationOffset) * (M_PI / 180.0f)));
    }
}

cpBody*
cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
{
	body->velocity_func = cpBodyUpdateVelocityDefault;
	body->position_func = cpBodyUpdatePositionDefault;
	
	cpBodySetMass(body, m);
	cpBodySetMoment(body, i);

	body->p = cpvzero;
	body->v = cpvzero;
	body->f = cpvzero;
	
	cpBodySetAngle(body, 0.0f);
	body->w = 0.0f;
	body->t = 0.0f;
	
	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
	
	body->data = NULL;
	body->v_limit = (cpFloat)INFINITY;
	body->w_limit = (cpFloat)INFINITY;
	
	body->space = NULL;
	body->shapesList = NULL;
	
	cpComponentNode node = {NULL, NULL, 0, 0.0f};
	body->node = node;
	
	return body;
}

ETERM *body_copy(ETERM *fromp, ETERM *argp) {

    // get the args
    ETERM *space_refp = erl_element(1, argp);
    ETERM *idp = erl_element(2, argp);
    ETERM *from_idp = erl_element(3, argp);

    erlmunk_space *s;
    int space_id = ERL_REF_NUMBER(space_refp);
    HASH_FIND_INT(erlmunk_spaces, &space_id, s);

    int body_id = ERL_INT_VALUE(idp);
    erlmunk_body *b;
    HASH_FIND_INT(s->bodies, &body_id, b);
    if (b == NULL)
        return NULL;

    int from_body_id = ERL_INT_VALUE(from_idp);
    erlmunk_body *from_b;
    HASH_FIND_INT(s->bodies, &from_body_id, from_b);

    // DEBUGF(("copying location from body #%d(%p) to #%d(%p)",
    //     from_body_id, from_b, body_id, b));

    // copy position and angle from the from body
    cpBodySetPosition(b->body, cpBodyGetPosition(from_b->body));
    cpBodySetAngle(b->body, cpBodyGetAngle(from_b->body));
    cpBodySetVelocity(b->body, cpBodyGetVelocity(from_b->body));

    return NULL;
}

cpBody*
cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
{
	body->velocity_func = cpBodyUpdateVelocity;
	body->position_func = cpBodyUpdatePosition;
	
	cpBodySetMass(body, m);
	cpBodySetMoment(body, i);

	body->p = cpvzero;
	body->v = cpvzero;
	body->f = cpvzero;
	
	cpBodySetAngle(body, 0.0f);
	body->w = 0.0f;
	body->t = 0.0f;
	
	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
	
	body->data = NULL;
//	body->active = 1;

	return body;
}

	Shell(cpVect pos, cpVect vel, float angle)
	{
		cpVect vl[4] = {cpv(0, 0), cpv(0.1, 0), cpv(0.07, 0.3), cpv(0.03, 0.3)};
		int vn = sizeof(vl)/sizeof(cpVect);
		
		float mass = cpAreaForPoly(vn, vl, 0) * shell_density;
		float moi = cpMomentForPoly(mass, vn, vl, cpv(0, 0), 0);
		body = cpBodyNew(mass, moi);
		
		cpshape = cpPolyShapeNew(body, vn, vl, cpTransformIdentity, 0);
		cpShapeSetFriction(cpshape, 0.9);

		cpVect centroid = cpCentroidForPoly(vn, vl);		

		shape.setPointCount(vn);
		for (int i = 0; i < vn; i++)
		{
			shape.setPoint(i, sf::Vector2f(vl[i].x, vl[i].y));
		}
		
		cpBodySetCenterOfGravity(body, centroid);
		cpBodySetPosition(body, pos-centroid);
		cpBodySetVelocity(body, vel);
		cpBodySetAngle(body, angle);
		
		cpShapeSetCollisionType(cpshape, 2);
		cpShapeSetUserData(cpshape, this);
	}

static void
add_box(cpSpace *space)
{
	const cpFloat size = 10.0f;
	const cpFloat mass = 1.0f;
	
	cpVect verts[] = {
		cpv(-size,-size),
		cpv(-size, size),
		cpv( size, size),
		cpv( size,-size),
	};
	
	cpFloat radius = cpvlength(cpv(size, size));
	cpVect pos = rand_pos(radius);
	
	cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, 4, verts, cpvzero, 0.0f)));
	body->velocity_func = planetGravityVelocityFunc;
	cpBodySetPosition(body, pos);

	// Set the box's velocity to put it into a circular orbit from its
	// starting position.
	cpFloat r = cpvlength(pos);
	cpFloat v = cpfsqrt(gravityStrength / r) / r;
	cpBodySetVelocity(body, cpvmult(cpvperp(pos), v));

	// Set the box's angular velocity to match its orbital period and
	// align its initial angle with its position.
	cpBodySetAngularVelocity(body, v);
	cpBodySetAngle(body, cpfatan2(pos.y, pos.x));

	cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, 4, verts, cpTransformIdentity, 0.0));
	cpShapeSetElasticity(shape, 0.0f);
	cpShapeSetFriction(shape, 0.7f);
}

 void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
                                              const CQuaternion& c_orientation) {
    /* Set target position and orientation */
    cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
    CRadians cXAngle, cYAngle, cZAngle;
    c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
    cpFloat tBodyOrient = cZAngle.GetValue();
    /* For each body: */
    for(size_t i = 0; i < m_vecBodies.size(); ++i) {
       /* Set body orientation at anchor */
       cpBodySetAngle(m_vecBodies[i].Body,
                      tBodyOrient + m_vecBodies[i].OffsetOrient);
       /* Set body position at anchor */
       cpBodySetPos(m_vecBodies[i].Body,
                    cpvadd(tBodyPos,
                           cpvrotate(m_vecBodies[i].OffsetPos,
                                     m_vecBodies[i].Body->rot)));
       
       /* Update shape index */
       cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
                                   m_vecBodies[i].Body);
    }
    /* Update ARGoS entity state */
    UpdateEntityStatus();
 }

void physics_add(Entity ent)
{
    PhysicsInfo *info;

    if (entitypool_get(pool, ent))
        return; /* already has physics */

    transform_add(ent);

    info = entitypool_add(pool, ent);

    info->mass = 1.0;
    info->type = PB_DYNAMIC;

    /* create, init cpBody */
    info->body = cpSpaceAddBody(space, cpBodyNew(info->mass, 1.0));
    cpBodySetUserData(info->body, ent); /* for cpBody -> Entity mapping */
    cpBodySetPos(info->body, cpv_of_vec2(transform_get_position(ent)));
    cpBodySetAngle(info->body, transform_get_rotation(ent));
    info->last_dirty_count = transform_get_dirty_count(ent);

    /* initially no shapes */
    info->shapes = array_new(ShapeInfo);

    /* initialize last_pos/last_ang info for kinematic bodies */
    info->last_pos = cpBodyGetPos(info->body);
    info->last_ang = cpBodyGetAngle(info->body);

    info->collisions = NULL;
}

static void _update_kinematics()
{
    PhysicsInfo *info;
    cpVect pos;
    cpFloat ang;
    Scalar invdt;
    Entity ent;

    if (timing_dt <= FLT_EPSILON)
        return;
    invdt = 1 / timing_dt;

    entitypool_foreach(info, pool)
        if (info->type == PB_KINEMATIC)
        {
            ent = info->pool_elem.ent;

            /* move to transform */
            pos = cpv_of_vec2(transform_get_position(ent));
            ang = transform_get_rotation(ent);
            cpBodySetPos(info->body, pos);
            cpBodySetAngle(info->body, ang);
            info->last_dirty_count = transform_get_dirty_count(ent);

            /* update linear, angular velocities based on delta */
            cpBodySetVel(info->body,
                         cpvmult(cpvsub(pos, info->last_pos), invdt));
            cpBodySetAngVel(info->body, (ang - info->last_ang) * invdt);
            cpSpaceReindexShapesForBody(space, info->body);

            /* save current state for next computation */
            info->last_pos = pos;
            info->last_ang = ang;
        }
}

static void grabObject(cpBody *body) {
    cpVect lp;

    lp = cpBodyWorld2Local(body, atlMouseClickPos());
    atlCreateMouseJoint(g_Space, body, lp);
    cpBodySetAngle(body, M_PI/180);
}

void NaiveRotationAlgorithm::stepSystem(SystemInfo *individual)
{
    
    MachineSystem *oldSystem = individual->system;
    individual->system = new MachineSystem(*oldSystem); // copy it to stop all the damn bouncing about
    delete oldSystem;
    
    cpBody *inputBody = individual->system->partAtPosition(individual->system->inputMachinePosition)->body;
    cpBody *outputBody = individual->system->partAtPosition(individual->system->outputMachinePosition)->body;
    cpSpace *systemSpace = individual->system->getSpace();

    
    cpConstraint *motor = cpSimpleMotorNew(cpSpaceGetStaticBody(systemSpace), inputBody, M_PI);
    cpSpaceAddConstraint(systemSpace, motor);
    cpConstraintSetMaxForce(motor, 50000);

    
    for (int i=0; i<simSteps; i++) {
        individual->inputValues[i] = (cpBodyGetAngle(inputBody));
        
        cpSpaceStep(systemSpace, 0.1);
        individual->outputValues[i] = (cpBodyGetAngle(outputBody));
    }
    cpSpaceRemoveConstraint(systemSpace, motor);
    cpBodySetAngVel(outputBody, 0);
    cpBodySetAngle(inputBody, 0);
}

__declspec( dllexport ) void push( const void * _in, int in_size, void * _out, int out_sz )
{
	int index;
	Variable *var;
	cpBody *body;
	cpShape *shape;

	index = PEEKINT(INPUT_MEMBLOCK,0);
	var = vhGetVariable(&mVariableHandler,index);
	switch (var->mType)
	{
	case VarTypeBody:
		body = (cpBody*)var->mPtr;
		
		cpBodySetAngle(body,degToRad(PEEKFLOAT(INPUT_MEMBLOCK,4)));
		cpBodySetPos(body,PEEKVECT(INPUT_MEMBLOCK,8));
		cpBodySetAngVel(body,degToRad(PEEKFLOAT(INPUT_MEMBLOCK,16)));
		cpBodySetVel(body,PEEKVECT(INPUT_MEMBLOCK,20));
		break;

	case VarTypeShape:
		shape = (cpShape*)var->mPtr;
		cpShapeSetFriction(shape,PEEKFLOAT(INPUT_MEMBLOCK,4));
		cpShapeSetElasticity(shape,PEEKFLOAT(INPUT_MEMBLOCK,8));
		cpShapeSetLayers(shape,PEEKUINT(INPUT_MEMBLOCK,12));
		cpShapeSetGroup(shape,PEEKUINT(INPUT_MEMBLOCK,16));
		break;
	}
}

int lc_body_newindex(lua_State *vm){
    //userdata, key, data
    const char *key = lua_tostring(vm, 2);
    cpBody *body = (lc_GetBody(1, vm))->body;
    if (strcmp("pos", key) == 0 && lua_istable(vm, 3)){
        cpBodySetPos(body, lc_TableTocpVect(3, vm));
    }
    else if (strcmp("vel", key) == 0 && lua_istable(vm, 3)){
        cpBodySetVel(body, lc_TableTocpVect(3, vm));
    }
    else if (strcmp("angle", key) == 0 || strcmp("radangle", key) == 0){
        cpBodySetAngle(body, (cpFloat)lua_tonumber(vm, 3));
    }
    else if (strcmp("degangle", key) == 0){
        cpBodySetAngle(body, ( (cpFloat)lua_tonumber(vm, 3)*(cpFloat)M_PI/(cpFloat)180.f ) );
    }
    return 0;
}

void landerRotate(int direction, Lander l) //-1 counterclockwise, 1 clockwise
{
    if(abs(l->orientation + direction) >= MAX_ORIENTATION)
        return;
    
    l->orientation += direction;
    
    cpBodySetAngle(l->body, radiansFromOrientation(l->orientation));
}

void
cpBodyUpdatePosition(cpBody *body, cpFloat dt)
{
	body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
	cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt);
	
	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
}

void LinkerAgent::UpdateFromEngine()
{
	DJVector2 djPosition = m_pSprite->GetPosition();
	float djAngle = m_pSprite->GetRotation();
	cpVect cpPosition;
	cpPosition.x = (float) djPosition.x();
	cpPosition.y = (float) -djPosition.y();
	cpBodySetAngle(m_pBody, djAngle);
	cpBodySetPos(m_pBody, cpPosition);
}

	void RigidBody2D::CopyBodyData(cpBody* from, cpBody* to)
	{
		cpBodySetAngle(to, cpBodyGetAngle(from));
		cpBodySetAngularVelocity(to, cpBodyGetAngularVelocity(from));
		cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
		cpBodySetForce(to, cpBodyGetForce(from));
		cpBodySetPosition(to, cpBodyGetPosition(from));
		cpBodySetTorque(to, cpBodyGetTorque(from));
		cpBodySetVelocity(to, cpBodyGetVelocity(from));
	}