package electrosphere.collision;

// import static org.ode4j.ode.OdeConstants.dContactBounce;
// import static org.ode4j.ode.OdeConstants.dContactSoftCFM;
// import static org.ode4j.ode.OdeConstants.dInfinity;
import static org.ode4j.ode.OdeHelper.areConnectedExcluding;
// import static org.ode4j.ode.OdeMath.dCalcVectorLengthSquare3;
// import static org.ode4j.ode.OdeMath.dSubtractVectors3;
// import static org.ode4j.ode.internal.Common.dRecip;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.concurrent.Semaphore;

import org.joml.Matrix4d;
import org.joml.Quaterniond;
import org.joml.Vector3d;
import org.joml.Vector4d;
import org.ode4j.math.DMatrix3;
import org.ode4j.math.DVector3;
import org.ode4j.ode.DBody;
import org.ode4j.ode.DBox;
import org.ode4j.ode.DCapsule;
import org.ode4j.ode.DContact;
import org.ode4j.ode.DContactBuffer;
import org.ode4j.ode.DContactGeom;
import org.ode4j.ode.DContactJoint;
import org.ode4j.ode.DCylinder;
import org.ode4j.ode.DGeom;
import org.ode4j.ode.DGeom.DNearCallback;
import org.ode4j.ode.DJoint;
import org.ode4j.ode.DJointGroup;
import org.ode4j.ode.DMass;
import org.ode4j.ode.DRay;
import org.ode4j.ode.DSpace;
import org.ode4j.ode.DSphere;
import org.ode4j.ode.DTriMesh;
import org.ode4j.ode.DTriMeshData;
import org.ode4j.ode.DWorld;
import org.ode4j.ode.OdeConstants;
import org.ode4j.ode.OdeHelper;

import electrosphere.collision.RayCastCallback.RayCastCallbackData;
import electrosphere.collision.collidable.Collidable;
import electrosphere.engine.Globals;
import electrosphere.engine.time.Timekeeper;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.state.collidable.Impulse;
import electrosphere.entity.state.physicssync.ServerPhysicsSyncTree;
import electrosphere.game.data.collidable.CollidableTemplate;
import electrosphere.game.data.collidable.HitboxData;
import electrosphere.logger.LoggerInterface;

/**
 * The main collision engine class. Tracks all entities that collide in its system and fires callbacks when they do.
 */
public class CollisionEngine {

    //gravity constant
    public static final float GRAVITY_MAGNITUDE = 9.8f * 2;
    
    //world data that the collision engine leverages for position correction and the like
    CollisionWorldData collisionWorldData;

    //Ode-specific stuff
    private DWorld world;
    private DSpace space;
    private static Semaphore spaceLock = new Semaphore(1);
    private DJointGroup contactgroup;

    private static final int MAX_CONTACTS = 10;		// maximum number of contact points per body

    //The list of dbodies ode should be tracking
    List<DBody> bodies = new ArrayList<DBody>();

    //This is used to relate DBody's back to their collidables so that when the library detects a collision, the callback can know which collidables are involved.
    Map<DBody,Collidable> bodyPointerMap = new HashMap<DBody,Collidable>();

    //The list of all collidables the engine is currently tracking
    List<Collidable> collidableList = new ArrayList<Collidable>();

    //callbacks for collision check
    RayCastCallback rayCastCallback = new RayCastCallback();

    //the collision resolution callback
    CollisionResolutionCallback collisionResolutionCallback;

    //buffer for collisions
    DContactBuffer contacts = null;

    // Callback for any near collisions in the broadphase of the collision check
    private DNearCallback nearCallback;
    
    /**
     * Constructor
     */
    public CollisionEngine(){
        world = OdeHelper.createWorld();
        space = OdeHelper.createBHVSpace(Collidable.TYPE_STATIC_BIT);
        world.setGravity(0,-GRAVITY_MAGNITUDE,0);
        contactgroup = OdeHelper.createJointGroup();
        this.nearCallback = new DNearCallback() {
            @Override
            public void call(Object data, DGeom o1, DGeom o2) {
                nearCallback( data, o1, o2);
            }
        };
    }


    /**
     * Resolves collisions in the engine
     * @param contactGeom the ode4j contact geometry
     * @param impactor the instigator of the collision
     * @param receiver the receiver of the collision
     * @param normal the normal to the collision surface
     * @param localPosition the local position of the collision
     * @param worldPos the world position of the collision
     * @param magnitude the magnitude of the collision
     */
    public static void resolveCollision(DContactGeom contactGeom, Collidable impactor, Collidable receiver, Vector3d normal, Vector3d localPosition, Vector3d worldPos, float magnitude){
        switch(receiver.getType()){
            case Collidable.TYPE_CREATURE:
                switch(impactor.getType()){
                    case Collidable.TYPE_TERRAIN:
                    //    System.out.println(EntityUtils.getPosition(impactor.getParent()) + " " + EntityUtils.getPosition(receiver.getParent()));
//                        System.out.println();
                    //    System.out.println("Terrain-creature collision: " + normal + " mag:" + magnitude);
                        // if(normal.y > normal.x + normal.z){
                        //     normal.x = 0;
                        //     normal.z = 0;
                        // }
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude * 2, Collidable.TYPE_TERRAIN));
                        break;
                    case Collidable.TYPE_CREATURE:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_CREATURE));
                        break;
                    case Collidable.TYPE_STRUCTURE:
//                        float realMag = 1f/(float)Math.pow(0.1, magnitude);
//                        System.out.println(normal + " - " + realMag);
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_STRUCTURE));
//                        System.out.println("Structure-creature collision");
                        break;
                    case Collidable.TYPE_ITEM:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_ITEM));
                        break;
                    case Collidable.TYPE_OBJECT:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_OBJECT));
                        break;
                    case Collidable.TYPE_FOLIAGE_STATIC:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_OBJECT));
                        break;
                }
                break;
            case Collidable.TYPE_ITEM:
                switch(impactor.getType()){
                    case Collidable.TYPE_TERRAIN:
//                        System.out.println(EntityUtils.getPosition(impactor.getParent()) + " " + EntityUtils.getPosition(receiver.getParent()));
//                        System.out.println();
//                        System.out.println("Terrain-item collision: " + normal + " mag:" + magnitude);
                        // if(normal.y > normal.x + normal.z){
                        //     normal.x = 0;
                        //     normal.z = 0;
                        // }
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude * 2, Collidable.TYPE_TERRAIN));
                        break;
                    case Collidable.TYPE_CREATURE:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_CREATURE));
                        break;
                    case Collidable.TYPE_STRUCTURE:
//                        float realMag = 1f/(float)Math.pow(0.1, magnitude);
//                        System.out.println(normal + " - " + realMag);
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_STRUCTURE));
//                        System.out.println("Structure-creature collision");
                        break;
                    case Collidable.TYPE_ITEM:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_ITEM));
                        break;
                    case Collidable.TYPE_OBJECT:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_OBJECT));
                        break;
                }
                break;
        }
    }
    
    public void clearCollidableImpulseLists(){
        spaceLock.acquireUninterruptibly();
        for(Collidable collidable : collidableList){
            collidable.clear();
        }
        spaceLock.release();
    }

    public List<Collidable> getCollidables(){
        return collidableList;
    }
    
    
    /**
     * 
     * @param e the entity that wants to move
     * @param positionToCheck the position that it wants to move to
     * @return true if it can occupy that position, false otherwise
     */
    public boolean checkCanOccupyPosition(CollisionWorldData w, Entity e, Vector3d positionToCheck){
        boolean rVal = true;
        //
        // check world bounds
        //
        if(
                positionToCheck.x < collisionWorldData.getWorldBoundMin().x ||
                positionToCheck.y < collisionWorldData.getWorldBoundMin().y ||
                positionToCheck.z < collisionWorldData.getWorldBoundMin().z ||
                positionToCheck.x > collisionWorldData.getWorldBoundMax().x ||
                positionToCheck.y > collisionWorldData.getWorldBoundMax().y ||
                positionToCheck.z > collisionWorldData.getWorldBoundMax().z
                ){
            return false;
        }
//        //
//        // are we below the terrain?
//        //
//        if(w.getElevationAtPoint(positionToCheck) > positionToCheck.y){
//            return false;
//        }
        return rVal;
    }
    
    /**
     * Performs the collision and simulation phases for this collision engine
     * @param time The time to increment the physics simulation by
     */
    public void simulatePhysics(float time){
        Globals.profiler.beginCpuSample("physics");
        spaceLock.acquireUninterruptibly();
        Globals.profiler.beginCpuSample("collide");
        OdeHelper.spaceCollide(space, 0, nearCallback);
        Globals.profiler.endCpuSample();
        // space.collide2(space, collisionWorldData, nearCallback);

        //simulate physics
        Globals.profiler.beginCpuSample("step physics");
		world.quickStep(Timekeeper.ENGINE_STEP_SIZE);
        Globals.profiler.endCpuSample();

		// remove all contact joints
		contactgroup.empty();
        spaceLock.release();
        Globals.profiler.endCpuSample();
    }

    /**
     * Runs a collision cycle on all bodies in the collision engine
     */
    public void collide(){
        Globals.profiler.beginCpuSample("physics");
        spaceLock.acquireUninterruptibly();
        Globals.profiler.beginCpuSample("collide");
        OdeHelper.spaceCollide(space, 0, nearCallback);
        Globals.profiler.endCpuSample();

		// remove all contact joints
		contactgroup.empty();
        spaceLock.release();
        Globals.profiler.endCpuSample();
    }

    /**
     * Sets the near callback function for all collision calls.
     * !!YOU LIKELY WANT TO INSTEAD OVERWRITE THE CollisionResolutionCallback!!
     * @param callback the callback
     */
    public void setNearCallback(DNearCallback callback){
        this.nearCallback = callback;
    }

    /**
     * This is called by dSpaceCollide when two objects in space are potentially colliding.
     * @param data The data
     * @param o1 the first collision body
     * @param o2 the second collision body
     */
	private void nearCallback (Object data, DGeom o1, DGeom o2)	{
		// if (o1->body && o2->body) return;

		// exit without doing anything if the two bodies are connected by a joint
		DBody b1 = o1.getBody();
		DBody b2 = o2.getBody();
		if (b1!=null && b2!=null && areConnectedExcluding (b1,b2,DContactJoint.class)){
            return;
        }

        //creates a buffer to store potential collisions
		DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);   // up to MAX_CONTACTS contacts per box-box
		for (int i=0; i<MAX_CONTACTS; i++) {
			DContact contact = contacts.get(i);
			contact.surface.mode = OdeConstants.dContactApprox1 & OdeConstants.dContactRolling;//dContactBounce | dContactSoftCFM;
			contact.surface.mu = 10;
            // contact.surface.mu = dInfinity;
			// contact.surface.mu2 = 0;
			// contact.surface.bounce = 0.1;
			// contact.surface.bounce_vel = 0.1;
			// contact.surface.soft_cfm = 0.01;
		}
        if(
            bodyPointerMap.get(b1) != null &&
            bodyPointerMap.get(b2) != null &&
            !(bodyPointerMap.get(b1).getType() == Collidable.TYPE_TERRAIN && bodyPointerMap.get(b2).getType() == Collidable.TYPE_TERRAIN) &&
            !(bodyPointerMap.get(b1).getType() == Collidable.TYPE_FOLIAGE_STATIC && bodyPointerMap.get(b2).getType() == Collidable.TYPE_FOLIAGE_STATIC)
            ){
            try {
                //calculate collisions
                int numc = OdeHelper.collide(o1,o2,MAX_CONTACTS,contacts.getGeomBuffer());
                //create DContacts based on each collision that occurs
                if (numc != 0) {
                    DMatrix3 RI = new DMatrix3();
                    RI.setIdentity ();
                    for (int i=0; i<numc; i++) {
                        DContact contact = contacts.get(i);
                        //special code for ray casting
                        if (o1 instanceof DRay || o2 instanceof DRay){
                            DVector3 end = new DVector3();
                            end.eqSum( contact.geom.pos, contact.geom.normal, contact.geom.depth );
                            continue;
                        }
                        // Use the default collision resolution
                        if(collisionResolutionCallback == null) {
                            resolveCollision(
                                contact.geom,
                                bodyPointerMap.get(o1.getBody()),
                                bodyPointerMap.get(o2.getBody()),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.normal).mul(-1.0),
                                PhysicsUtils.odeVecToJomlVec(contact.fdir1).mul(-1.0),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
                                (float)contact.geom.depth
                                );
                            resolveCollision(
                                contact.geom,
                                bodyPointerMap.get(o2.getBody()),
                                bodyPointerMap.get(o1.getBody()),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.normal),
                                PhysicsUtils.odeVecToJomlVec(contact.fdir1),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
                                (float)contact.geom.depth
                                );
                        } else {
                            //use custom collision resolution
                            collisionResolutionCallback.resolve(
                                contact.geom,
                                o1,
                                o2,
                                bodyPointerMap.get(o1.getBody()),
                                bodyPointerMap.get(o2.getBody()),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.normal).mul(-1.0),
                                PhysicsUtils.odeVecToJomlVec(contact.fdir1).mul(-1.0),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
                                (float)contact.geom.depth
                            );
                            collisionResolutionCallback.resolve(
                                contact.geom,
                                o2,
                                o1,
                                bodyPointerMap.get(o2.getBody()),
                                bodyPointerMap.get(o1.getBody()),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.normal),
                                PhysicsUtils.odeVecToJomlVec(contact.fdir1),
                                PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
                                (float)contact.geom.depth
                            );
                        }

                        //add contact to contact group
                        DJoint c = OdeHelper.createContactJoint (world,contactgroup,contact );
                        c.attach (b1,b2);
                    }
                }
            } catch(ArrayIndexOutOfBoundsException ex){
                //I've found that ode4j occasionally throws an exception on the OdeHelper.collide function.
                //I don't know why it has out of bounds elements, but it's happening.
                //Catching the exception here allows the engine to keep running at least.
                LoggerInterface.loggerEngine.ERROR("ode4j error", ex);
            }
        }
	}

    /**
     * Sets the function that is called once a collision has happened
     * @param collisionResolutionCallback the function
     */
    public void setCollisionResolutionCallback(CollisionResolutionCallback collisionResolutionCallback){
        this.collisionResolutionCallback = collisionResolutionCallback;
    }
    
    /**
     * 
     * @param w The collision world data
     * @param positionToCheck the position the entity wants to be at
     * @return the position the engine recommends it move to instead (this is
     * guaranteed to be a valid position)
     */
    public Vector3d suggestMovementPosition(CollisionWorldData w, Vector3d positionToCheck){
        Vector3d suggestedPosition = new Vector3d(positionToCheck);
        //
        // adjust for world bounds
        //
        if(suggestedPosition.x < collisionWorldData.getWorldBoundMin().x){
            suggestedPosition.x = collisionWorldData.getWorldBoundMin().x;
        }
        if(suggestedPosition.y < collisionWorldData.getWorldBoundMin().y){
            suggestedPosition.y = collisionWorldData.getWorldBoundMin().y;
        }
        if(suggestedPosition.z < collisionWorldData.getWorldBoundMin().z){
            suggestedPosition.z = collisionWorldData.getWorldBoundMin().z;
        }
        if(suggestedPosition.x > collisionWorldData.getWorldBoundMax().x){
            suggestedPosition.x = collisionWorldData.getWorldBoundMax().x;
        }
        if(suggestedPosition.y > collisionWorldData.getWorldBoundMax().y){
            suggestedPosition.y = collisionWorldData.getWorldBoundMax().y;
        }
        if(suggestedPosition.z > collisionWorldData.getWorldBoundMax().z){
            suggestedPosition.z = collisionWorldData.getWorldBoundMax().z;
        }
        return suggestedPosition;
    }
    
    
    /**
     * Sets the collision world data
     * @param collisionWorldData The collision world data
     */
    public void setCollisionWorldData(CollisionWorldData collisionWorldData){
        this.collisionWorldData = collisionWorldData;
    }
    
    
    public boolean collisionSphereCheck(Entity hitbox1, HitboxData hitbox1data, Entity hitbox2, HitboxData hitbox2data){
        Vector3d position1 = EntityUtils.getPosition(hitbox1);
        Vector3d position2 = EntityUtils.getPosition(hitbox2);
        float radius1 = hitbox1data.getRadius();
        float radius2 = hitbox2data.getRadius();
        double distance = position1.distance(position2);
        if(distance < radius1 + radius2){
            return true;
        } else {
            return false;
        }
    }
    
    /**
     * Main function to resynchronize entity positions with physics object positions after impulses are applied.
     */
    public void updateDynamicObjectTransforms(){
        Globals.profiler.beginCpuSample("updateDynamicObjectTransforms");
        spaceLock.acquireUninterruptibly();
        if(this.collisionWorldData != null){
            for(Collidable collidable : collidableList){
                if(collidable.getParentTracksCollidable()){
                    Entity physicsEntity = collidable.getParent();
                    DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                    Matrix4d inverseTransform = new Matrix4d();
                    Vector4d rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.getRigidBodyPosition(rigidBody),1));
                    Vector3d calculatedPosition = new Vector3d(rawPos.x,rawPos.y,rawPos.z);
                    Vector3d suggestedPosition = this.suggestMovementPosition(collisionWorldData, calculatedPosition);
                    if(calculatedPosition.distance(suggestedPosition) > 0){
                        collidable.addImpulse(new Impulse(new Vector3d(), new Vector3d(), new Vector3d(), 0, Collidable.TYPE_WORLD_BOUND));
                    }
                    Quaterniond newRotation = PhysicsUtils.getRigidBodyRotation(rigidBody);
                    EntityUtils.getPosition(physicsEntity).set(suggestedPosition);
                    EntityUtils.getRotation(physicsEntity).set(newRotation);
                }
            }
        }
        spaceLock.release();
        Globals.profiler.endCpuSample();
    }
    
    public void registerCollisionObject(DBody body, Collidable collidable){
        spaceLock.acquireUninterruptibly();
        registerPhysicsObject(body);
        bodyPointerMap.put(body,collidable);
        collidableList.add(collidable);
        spaceLock.release();
    }

    /**
     * Deregisters a collidable from the physics engine
     * @param collidable The collidable
     */
    public void deregisterCollisionObject(DBody body, Collidable collidable){
        spaceLock.acquireUninterruptibly();
        bodyPointerMap.remove(body);
        collidableList.remove(collidable);
        spaceLock.release();
    }
    
    public void listBodyPositions(){
        for(DBody body : bodies){
            LoggerInterface.loggerEngine.INFO("" + body);
            LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.getRigidBodyPosition(body));
        }
    }
    

    /**
     * Casts a ray into the scene and returns the first entity that the ray collides with.
     * This will collide with any type of collidable object.
     * @param start THe start position of the ray
     * @param direction The direction the ray will travel in
     * @param length The length of the ray to cast
     * @return The entity that the ray collides with if successful, null otherwise
     */
    public Entity rayCast(Vector3d start, Vector3d direction, double length){
        return rayCast(start,direction,length,null);
    }

    /**
     * Casts a ray into the collision space and returns the first entity that the ray collides with.
     * The type mask is a list of collidable types that are valid collisions.
     * For instance, if the typeMask only contains Collidable.TYPE_TERRAIN, only entities with the type terrain will
     * be returned from the raycast.
     * @param start The start position of the way
     * @param length The length to cast the ray out to
     * @param typeMask The mask of types to collide the ray with
     * @return The entity that the ray cast collided with. Will be null if no entity was collided with.
     */
    public Entity rayCast(Vector3d start, Vector3d direction, double length, List<String> typeMask){
        spaceLock.acquireUninterruptibly();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x, start.y, start.z, direction.x, direction.y, direction.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, typeMask);
        rayCastCallback.setLength(length);
        space.collide2(space, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.release();
        return data.collidedEntity;
    }

    /**
     * Ray casts into the scene and gets the position of the closest collision's position in world space.
     * Will collide with any collidable types including characters and items.
     * @param start The start position of the ray to cast
     * @param direction The direction of the ray to cast
     * @param length The length of the ray to cast
     * @return The position, in world coordinates, of the closest collision of the way, or null if it did not collide with anything.
     */
    public Vector3d rayCastPosition(Vector3d start, Vector3d direction, double length){
        spaceLock.acquireUninterruptibly();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x, start.y, start.z, direction.x, direction.y, direction.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, null);
        rayCastCallback.setLength(length);
        space.collide2(ray, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.release();
        return data.collisionPosition;
    }

    /**
     * Ray casts into the scene and gets the position of the closest collision's position in world space.
     * @param start The start position of the ray to cast
     * @param direction The direction of the ray to cast
     * @param length The length of the ray to cast
     * @param typeMask The mask of types to collide the ray with
     * @return The position, in world coordinates, of the closest collision of the way, or null if it did not collide with anything.
     */
    public Vector3d rayCastPositionMasked(Vector3d start, Vector3d direction, double length, List<String> typeMask){
        spaceLock.acquireUninterruptibly();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x, start.y, start.z, direction.x, direction.y, direction.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, typeMask);
        rayCastCallback.setLength(length);
        space.collide2(ray, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.release();
        return data.collisionPosition;
    }
    
    public void registerPhysicsObject(DBody body){
        if(!bodies.contains(body)){
            bodies.add(body);
            // OdeHelper.createBody(world);
        }
    }
    
    /**
     * Destroys a body and all geometry under the body
     * @param body The DBody to destroy
     */
    private void deregisterPhysicsObject(DBody body){
        spaceLock.acquireUninterruptibly();
        if(bodies.contains(body)){
            bodies.remove(body);
        }
        Iterator<DGeom> geomIterator = body.getGeomIterator();
        while(geomIterator.hasNext()){
            DGeom geom = geomIterator.next();
            space.remove(geom);
            geom.destroy();
        }
        body.destroy();
        spaceLock.release();
    }
    
    /**
     * Destroys the physics on an entity
     * @param e The entity
     */
    public void destroyPhysics(Entity e){
        //make uncollidable
        if(e.containsKey(EntityDataStrings.PHYSICS_COLLISION_BODY)){
            DBody rigidBody = (DBody)e.getData(EntityDataStrings.PHYSICS_COLLISION_BODY);
            deregisterCollisionObject(rigidBody,PhysicsEntityUtils.getCollidable(e));
            e.removeData(EntityDataStrings.PHYSICS_COLLISION_BODY);
            deregisterPhysicsObject(rigidBody);
        }
        if(ServerPhysicsSyncTree.hasTree(e)){
            ServerPhysicsSyncTree.detachTree(e, ServerPhysicsSyncTree.getTree(e));
        }
    }

    /**
     * Returns the Ode DSpace
     * @return The DSpace
     */
    public DSpace getSpace(){
        return space;
    }

    /**
     * Gets the DWorld for this collision engine
     * @return The DWorld
     */
    public DWorld getDWorld(){
        return world;
    }
    
    /**
     * Creates a trimesh from a given set of vertices and indices
     * @param verts The vertices
     * @param indices The indices
     * @return The DTriMesh
     */
    protected DTriMesh createTrimeshGeom(float[] verts, int[] indices, long categoryBits){
        spaceLock.acquireUninterruptibly();
        DTriMeshData data = OdeHelper.createTriMeshData();
        data.build(verts, indices);
        DTriMesh rVal = OdeHelper.createTriMesh(getSpace(), data);
        rVal.setCategoryBits(categoryBits);
        spaceLock.release();
        return rVal;
    }

    /**
     * Creates a cube geometry. Dimensions vector control the total length of the x, y, and z dimensions respectively.
     * @param dimensions The dimensions of the box
     * @return The DBox
     */
    protected DBox createCubeGeom(Vector3d dimensions, long categoryBits){
        spaceLock.acquireUninterruptibly();
        DBox boxGeom = OdeHelper.createBox(space, dimensions.x, dimensions.y, dimensions.z);
        boxGeom.setCategoryBits(categoryBits);
        spaceLock.release();
        return boxGeom;
    }

    /**
     * Creates a cylinder geometry in the physics space
     * @param dimensions The dimensions of the cylinder. X is the radius, y is the total height.
     * @return The cylinder geometry
     */
    protected DCylinder createCylinderGeom(double radius, double length, long categoryBits){
        spaceLock.acquireUninterruptibly();
        DCylinder cylinderGeom = OdeHelper.createCylinder(space, radius, length);
        cylinderGeom.setCategoryBits(categoryBits);
        spaceLock.release();
        return cylinderGeom;
    }

    /**
     * Creates a sphere geometry in the physics space
     * @param radius The radius of the sphere
     * @return The sphere geometry
     */
    protected DSphere createSphereGeom(double radius, long categoryBits){
        spaceLock.acquireUninterruptibly();
        DSphere sphereGeom = OdeHelper.createSphere(space, radius);
        sphereGeom.setCategoryBits(categoryBits);
        spaceLock.release();
        return sphereGeom;
    }

    /**
     * Creates a capsule geometry in the physics space
     * @param radius The radius of the capsule
     * @param length The length of the capsule
     * @return The capsule geometry
     */
    protected DCapsule createCapsuleGeom(double radius, double length, long categoryBits){
        spaceLock.acquireUninterruptibly();
        DCapsule capsuleGeom = OdeHelper.createCapsule(space, radius, length);
        capsuleGeom.setCategoryBits(categoryBits);
        spaceLock.release();
        return capsuleGeom;
    }

    /**
     * Creates a DBody. Can optionally be passed DGeom objects to be attached to the body when it is created.
     * @param geom The geometry objects to attach to the body on creation
     * @return The DBody
     */
    protected DBody createDBody(DGeom ...geom){
        spaceLock.acquireUninterruptibly();
        DBody body = OdeHelper.createBody(world);
        if(geom != null){
            for(int i = 0; i < geom.length; i++){
                if(geom != null){
                    geom[i].setBody(body);
                }
            }
        }
        spaceLock.release();
        return body;
    }

    /**
     * Creates a DMass and attaches a body to it
     * @param massValue The amount of mass for the object
     * @param body The DBody to attach the mass to
     * @return The DMass
     */
    protected DMass createDMass(double massValue, DBody body){
        spaceLock.acquireUninterruptibly();
        DMass mass = OdeHelper.createMass();
        mass.setMass(massValue);
        spaceLock.release();
        return mass;
    }

    /**
     * Sets the transform on a body
     * @param body The body
     * @param position The position
     * @param rotation The rotation
     */
    protected void setBodyTransform(DBody body, Vector3d position, Quaterniond rotation){
        spaceLock.acquireUninterruptibly();
        body.setPosition(position.x, position.y, position.z);
        body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        spaceLock.release();
    }

    /**
     * Synchronizes the data on a body
     * @param body The body
     * @param position The position
     * @param rotation The rotation
     * @param linearVel The linear velocity
     * @param angularVel The angular velocity
     * @param linearForce The linear force
     * @param angularForce The angular force
     */
    protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
        spaceLock.acquireUninterruptibly();
        body.setPosition(position.x, position.y, position.z);
        body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        body.setLinearVel(PhysicsUtils.jomlVecToOdeVec(linearVel));
        body.setAngularVel(PhysicsUtils.jomlVecToOdeVec(angularVel));
        body.setForce(PhysicsUtils.jomlVecToOdeVec(linearForce));
        body.setTorque(PhysicsUtils.jomlVecToOdeVec(angularForce));
        spaceLock.release();
    }

    /**
     * Sets the transform on a body
     * @param body The body
     * @param position The position
     * @param rotation The rotation
     * @param scale The scale
     */
    protected void setBodyTransform(DBody body, CollidableTemplate template, Vector3d position, Quaterniond rotation, Vector3d scale){
        spaceLock.acquireUninterruptibly();
        body.setPosition(position.x, position.y, position.z);
        body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        DGeom firstGeom = body.getFirstGeom();
        if(firstGeom instanceof DCylinder){
            ((DCylinder)firstGeom).setParams(template.getDimension1() * scale.x,template.getDimension2() * scale.y);
        } else if(firstGeom instanceof DBox){
            ((DBox)firstGeom).setLengths(template.getDimension1() * scale.x,template.getDimension2() * scale.y,template.getDimension3() * scale.z);
        } else if(firstGeom instanceof DCapsule){
            ((DCapsule)firstGeom).setParams(template.getDimension1() * scale.x,template.getDimension2() * scale.y);
        }
        spaceLock.release();
    }

    /**
     * Sets the transform of a geometry
     * @param geom The geometry
     * @param position The position
     * @param rotation The rotation
     */
    protected void setGeomTransform(DGeom geom, Vector3d position, Quaterniond rotation){
        spaceLock.acquireUninterruptibly();
        geom.setOffsetWorldPosition(position.x, position.y, position.z);
        geom.setOffsetQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        spaceLock.release();
    }

    /**
     * Corrects the initial axis of eg cylinders or capsules
     * @param geom the geometry to correct
     */
    protected void setOffsetRotation(DGeom geom){
        spaceLock.acquireUninterruptibly();
        geom.setOffsetRotation(CollisionBodyCreation.AXIS_CORRECTION_MATRIX);
        spaceLock.release();
    }

    /**
     * Gets the position of the body in a thread-safe way
     * @param body The body to get the position of
     * @return The position
     */
    protected Vector3d getBodyPosition(DBody body){
        Vector3d rVal = null;
        spaceLock.acquireUninterruptibly();
        rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition());
        spaceLock.release();
        return rVal;
    }

    /**
     * Gets the rotation of the body in a thread-safe way
     * @param body The body to get the rotation of
     * @return The rotation
     */
    protected Quaterniond getBodyRotation(DBody body){
        Quaterniond rVal = null;
        spaceLock.acquireUninterruptibly();
        rVal = PhysicsUtils.odeQuatToJomlQuat(body.getQuaternion());
        spaceLock.release();
        return rVal;
    }

    /**
     * Sets a body to be kinematic (infinite mass, not affected by gravity)
     * @param body The body to set
     */
    protected void setKinematic(DBody body){
        spaceLock.acquireUninterruptibly();
        body.setKinematic();
        spaceLock.release();
    }

    /**
     * Sets the gravity mode of the body
     * @param body the body
     * @param gravityMode the gravity mode
     */
    protected void setGravityMode(DBody body, boolean gravityMode){
        spaceLock.acquireUninterruptibly();
        body.setGravityMode(gravityMode);
        spaceLock.release();
    }

    /**
     * Sets the offset position of the first geometry in the body
     * @param body The body
     * @param offsetVector The offset position
     */
    protected void setOffsetPosition(DBody body, Vector3d offsetVector){
        spaceLock.acquireUninterruptibly();
        body.getGeomIterator().next().setOffsetPosition(offsetVector.x,offsetVector.y,offsetVector.z);
        spaceLock.release();
    }

    /**
     * Sets whether the body is angularly static or not
     * @param body The body
     * @param angularlyStatic true if angularly static, false otherwise
     */
    protected void setAngularlyStatic(DBody body, boolean angularlyStatic){
        spaceLock.acquireUninterruptibly();
        if(angularlyStatic){
            body.setMaxAngularSpeed(0);
        } else {
            body.setMaxAngularSpeed(1000);
        }
        spaceLock.release();
    }

    /**
     * Removes the geometry from the body
     * @param body the body
     * @param geom the geometry
     */
    protected void removeGeometryFromBody(DBody body, DGeom geom){
        geom.setBody(null);
    }

    /**
     * Destroys a geometry
     * @param geom The geometry
     */
    protected void destroyGeom(DGeom geom){
        geom.destroy();
    }

    /**
     * Attaches a geom to a body
     * @param body the body
     * @param geom the geom
     */
    protected void attachGeomToBody(DBody body, DGeom geom){
        geom.setBody(body);
    }
    

    /**
     * A callback for resolving collisions between two entities
     */
    public interface CollisionResolutionCallback {
        /**
         * Resolves a collision between two collidables in the engine
         * @param contactGeom the ode4j contact geom
         * @param geom1 The first geometry
         * @param geom2 The second geometry
         * @param impactor The collidable initiating the contact
         * @param receiver The collidable recieving the contact
         * @param normal The normal of the collision
         * @param localPosition The local position of the collision
         * @param worldPos The world position of the collision
         * @param magnitude The magnitude of the collision
         */
        public void resolve(DContactGeom contactGeom, DGeom geom1, DGeom geom2, Collidable impactor, Collidable receiver, Vector3d normal, Vector3d localPosition, Vector3d worldPos, float magnitude);
    }

}