package electrosphere.collision;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.concurrent.locks.ReentrantLock;

import org.joml.Matrix4d;
import org.joml.Quaterniond;
import org.joml.Vector3d;
import org.joml.Vector4d;
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.OdeHelper;

import electrosphere.collision.RayCastCallback.RayCastCallbackData;
import electrosphere.collision.collidable.Collidable;
import electrosphere.collision.collidable.SurfaceParams;
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 = 0.15f;

    /**
     * The damping applied to angular velocity
     */
    public static final double DEFAULT_ANGULAR_DAMPING = 0.01;

    /**
     * The damping applied to linear velocity
     */
    public static final double DEFAULT_LINEAR_DAMPING = 0.01;

    /**
     * Default max angular speed
     */
    public static final double DEFAULT_MAX_ANGULAR_SPEED = 100;

    /**
     * The number of times the physics engine should be simulated per frame of main game.
     * IE, if this value is 3, every main game engine frame, the physics simulation will run 3 frames.
     * This keeps the physics simulation much more stable than it would be otherwise.
     */
    public static final int PHYSICS_SIMULATION_RESOLUTION = 3;

    /**
     * Threshold after which the engine warns about collidable count
     */
    public static final int COLLIDABLE_COUNT_WARNING_THRESHOLD = 5000;

    /**
     * Default distance to interact with collidables at (ie picking where to place things)
     */
    public static final double DEFAULT_INTERACT_DISTANCE = 5.0;
    
    //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 ReentrantLock spaceLock = new ReentrantLock();
    private DJointGroup contactgroup;

    // maximum number of contact points per body
    /**
     * <p> Maximum number of contact points per body </p>
     * <p><b> Note: </b></p>
     * <p>
     * This value must be sufficiently high (I'd recommend 64), in order for large bodies to not sink into other large bodies.
     * I used a value of 10 for a long time and found that cubes were sinking into TriMeshes.
     * </p>
     */
    private static final int MAX_CONTACTS = 64;

    //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>();

    /**
     * Dynamic spatial offset applied to all operations on the space.
     * This is used for world origin rebasing.
     * This makes physics behave more consistently by moving far out objects to center around 0,0,0.
     */
    Vector3d floatingOrigin = new Vector3d(0,0,0);

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

    //the collision resolution callback
    CollisionResolutionCallback collisionResolutionCallback;

    //buffer for collisions
    DContactBuffer contacts = null;

    /**
     * The body that contains all static shapes
     */
    DBody staticBody;

    // Callback for any near collisions in the broadphase of the collision check
    private DNearCallback nearCallback;
    
    /**
     * Constructor
     */
    public CollisionEngine(){
        world = OdeHelper.createWorld();
        world.setGravity(0,-GRAVITY_MAGNITUDE,0);
        space = OdeHelper.createBHVSpace(Collidable.TYPE_STATIC_BIT);
        // world.setContactMaxCorrectingVel(0.1);
        // world.setContactSurfaceLayer(0.001);
        // world.setCFM(1e-10);

        //base plane
        OdeHelper.createPlane(space, 0, 1, 0, 0);

        //static body
        staticBody = this.createDBody();
        staticBody.setKinematic();


        contactgroup = OdeHelper.createJointGroup();
        this.nearCallback = new DNearCallback() {
            @Override
            public void call(Object data, DGeom o1, DGeom o2) {
                nearCallback( data, o1, o2);
            }
        };
    }

    /**
     * Creates a collision engine with a specified callback
     */
    public static CollisionEngine create(CollisionResolutionCallback callback){
        CollisionEngine rVal = new CollisionEngine();
        rVal.setCollisionResolutionCallback(callback);
        return rVal;
    }


    /**
     * 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_STATIC:
                    //    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_STATIC));
                        break;
                    case Collidable.TYPE_CREATURE:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_CREATURE));
                        break;
                    case Collidable.TYPE_OBJECT:
                        receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_OBJECT));
                        break;
                }
                break;
        }
    }
    
    public void clearCollidableImpulseLists(){
        spaceLock.lock();
        for(Collidable collidable : collidableList){
            collidable.clear();
        }
        spaceLock.unlock();
    }

    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.lock();
        for(int i = 0; i < PHYSICS_SIMULATION_RESOLUTION; i++){
            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.unlock();
        Globals.profiler.endCpuSample();
    }

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

		// remove all contact joints
		contactgroup.empty();
        spaceLock.unlock();
        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;

        //if the collision is static-on-static, skip
        if(o1.getCategoryBits() == Collidable.TYPE_STATIC_BIT && o2.getCategoryBits() == Collidable.TYPE_STATIC_BIT){
            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 && OdeHelper.areConnectedExcluding(b1,b2,DContactJoint.class)){
            return;
        }

        //if collision is between static and disabled, skip
        if(o1.getCategoryBits() == Collidable.TYPE_STATIC_BIT && !b2.isEnabled()){
            return;
        }
        if(o2.getCategoryBits() == Collidable.TYPE_STATIC_BIT && !b1.isEnabled()){
            return;
        }

        Collidable c1 = bodyPointerMap.get(b1);
        Collidable c2 = bodyPointerMap.get(b2);
        if(c1 == null || c2 == null){
            return;
        }

        Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - try collisions");
        try {
            //creates a buffer to store potential collisions
            DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);   // up to MAX_CONTACTS contacts per box-box
            SurfaceParams surfaceParams = c1.getSurfaceParams();
            for (int i=0; i<MAX_CONTACTS; i++) {
                DContact contact = contacts.get(i);
                contact.surface.mode = surfaceParams.getMode();
                contact.surface.mu = surfaceParams.getMu();
                if(surfaceParams.getRho() != null){
                    contact.surface.rho = surfaceParams.getRho();
                }
                if(surfaceParams.getRho2() != null){
                    contact.surface.rho2 = surfaceParams.getRho2();
                }
                if(surfaceParams.getRhoN() != null){
                    contact.surface.rhoN = surfaceParams.getRhoN();
                }
                if(surfaceParams.getBounce() != null){
                    contact.surface.bounce = surfaceParams.getBounce();
                }
                if(surfaceParams.getBounceVel() != null){
                    contact.surface.bounce_vel = surfaceParams.getBounceVel();
                }
            }
            //calculate collisions
            int numc = OdeHelper.collide(o1,o2,MAX_CONTACTS,contacts.getGeomBuffer());
            //create DContacts based on each collision that occurs
            if (numc != 0) {
                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;
                    }

                    // //
                    // //apply shallow slope correction
                    // if(
                    //     //is terrain
                    //     (c1.getType() == Collidable.TYPE_TERRAIN || c2.getType() == Collidable.TYPE_TERRAIN) &&
                    //     (!c1.getType().equals(c2.getType())) &&
                    //     contact.geom.normal.length() > 0 
                    //     &&
                    //     //force is pointing basically upwards
                    //     PhysicsUtils.odeVecToJomlVec(contact.geom.normal).dot(MathUtils.getUpVector()) > 0.7
                    // ){
                    //     System.out.println(PhysicsUtils.odeVecToJomlVec(contact.geom.normal).dot(MathUtils.getUpVector()));
                    //     if(//force is pointing basically upwards
                    //     PhysicsUtils.odeVecToJomlVec(contact.geom.normal).dot(MathUtils.getUpVector()) > 0.7){
                    //         contact.geom.normal.set(PhysicsUtils.jomlVecToOdeVec(MathUtils.getUpVector()));
                    //     }
                    // }

                    //
                    //add contact to contact group
                    DJoint c = OdeHelper.createContactJoint(world,contactgroup,contact);
                    c.attach(b1,b2);

                    // Use the default collision resolution
                    if(collisionResolutionCallback == null) {
                        CollisionEngine.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
                            );
                        CollisionEngine.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
                        );
                    }
                }
            }
        } 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);
        }
        Globals.profiler.endCpuSample();
	}

    /**
     * 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.lock();
        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.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin),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.unlock();
        Globals.profiler.endCpuSample();
    }

    /**
     * Rebases the world origin
     */
    public void rebaseWorldOrigin(){
        Globals.profiler.beginCpuSample("rebaseWorldOrigin");
        spaceLock.lock();
        if(this.collisionWorldData != null && this.floatingOrigin.x == 0 && this.floatingOrigin.z == 0){
            int collected = 0;
            Vector3d newOrigin = new Vector3d();
            //calculate new origin
            for(Collidable collidable : collidableList){
                Entity physicsEntity = collidable.getParent();
                DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                if(rigidBody != null){
                    Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
                    if(collected == 0){
                        newOrigin.set(currentBodyOffset);
                    } else {
                        float percentExisting = collected / (float)(collected + 1);
                        float percentNew = 1.0f - percentExisting;
                        newOrigin = newOrigin.mul(percentExisting).add(currentBodyOffset.mul(percentNew));
                    }
                    collected++;
                }
            }
            Vector3d delta = new Vector3d(this.floatingOrigin).sub(newOrigin);
            this.floatingOrigin = this.floatingOrigin.set(newOrigin);
            //apply new origin to all geoms
            //calculate new origin
            for(Collidable collidable : collidableList){
                Entity physicsEntity = collidable.getParent();
                DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                if(rigidBody != null){
                    Vector3d existingPosition = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
                    rigidBody.setPosition(PhysicsUtils.jomlVecToOdeVec(existingPosition.add(delta)));
                }
            }
        }
        spaceLock.unlock();
        Globals.profiler.endCpuSample();
    }
    
    /**
     * Registers a collision object with the server
     * @param body The body
     * @param collidable The corresponding collidable
     */
    public void registerCollisionObject(DBody body, Collidable collidable){
        spaceLock.lock();
        this.registerPhysicsObject(body);
        bodyPointerMap.put(body,collidable);
        collidableList.add(collidable);
        spaceLock.unlock();
    }

    /**
     * Deregisters a collidable from the physics engine
     * @param collidable The collidable
     */
    public void deregisterCollisionObject(DBody body, Collidable collidable){
        spaceLock.lock();
        bodyPointerMap.remove(body);
        collidableList.remove(collidable);
        spaceLock.unlock();
    }
    
    public void listBodyPositions(){
        for(DBody body : bodies){
            LoggerInterface.loggerEngine.INFO("" + body);
            LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin));
        }
    }
    

    /**
     * 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.lock();
        Vector3d unitDir = new Vector3d(direction).normalize();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x - this.floatingOrigin.x, start.y - this.floatingOrigin.y, start.z - this.floatingOrigin.z, unitDir.x, unitDir.y, unitDir.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, typeMask);
        rayCastCallback.setLength(length);
        space.collide2(space, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.unlock();
        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 (this will automatically be converted into a unit vector if it isn't already)
     * @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.lock();
        Vector3d unitDir = new Vector3d(direction).normalize();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x - this.floatingOrigin.x, start.y - this.floatingOrigin.y, start.z - this.floatingOrigin.z, unitDir.x, unitDir.y, unitDir.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, null);
        rayCastCallback.setLength(length);
        space.collide2(ray, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.unlock();
        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.lock();
        Vector3d unitDir = new Vector3d(direction).normalize();
        //create the ray
        DRay ray = OdeHelper.createRay(space, length);
        ray.set(start.x - this.floatingOrigin.x, start.y - this.floatingOrigin.y, start.z - this.floatingOrigin.z, unitDir.x, unitDir.y, unitDir.z);
        //collide
        RayCastCallbackData data = new RayCastCallbackData(bodyPointerMap, typeMask);
        rayCastCallback.setLength(length);
        space.collide2(ray, data, rayCastCallback);
        //destroy ray
        ray.destroy();
        spaceLock.unlock();
        return data.collisionPosition;
    }
    
    /**
     * Registers a body
     * @param body The body
     */
    public void registerPhysicsObject(DBody body){
        if(!bodies.contains(body)){
            bodies.add(body);
            if(bodies.size() > COLLIDABLE_COUNT_WARNING_THRESHOLD){
                LoggerInterface.loggerEngine.WARNING("Body count has superceded the warning threshold! " + bodies.size());
            }
        }
    }
    
    /**
     * Destroys a body and all geometry under the body
     * @param body The DBody to destroy
     */
    protected void destroyDBody(DBody body){
        spaceLock.lock();
        try {
            if(bodies.contains(body)){
                bodies.remove(body);
            }
            //destroy all geometries
            Iterator<DGeom> geomIterator = body.getGeomIterator();
            while(geomIterator.hasNext()){
                DGeom geom = geomIterator.next();
                space.remove(geom);
                geom.DESTRUCTOR();
                geom.destroy();
            }
            //destroy all joints
            for(int i = 0; i < body.getNumJoints(); i++){
                DJoint joint = body.getJoint(i);
                joint.DESTRUCTOR();
                joint.destroy();
            }
            //destroy actual body
            body.destroy();
        } catch (NullPointerException ex){
            LoggerInterface.loggerEngine.ERROR(ex);
            spaceLock.unlock();
        }
        spaceLock.unlock();
    }
    
    /**
     * Destroys the physics on an entity
     * @param e The entity
     */
    public void destroyPhysics(Entity e){
        //make uncollidable
        if(PhysicsEntityUtils.containsDBody(e)){
            DBody rigidBody = PhysicsEntityUtils.getDBody(e);
            if(rigidBody == null){
                throw new Error("DBody key set to null rigid body! " + rigidBody);
            }
            this.deregisterCollisionObject(rigidBody,PhysicsEntityUtils.getCollidable(e));
            e.removeData(EntityDataStrings.PHYSICS_COLLISION_BODY);
            this.destroyDBody(rigidBody);
        }
        if(ServerPhysicsSyncTree.hasTree(e)){
            ServerPhysicsSyncTree.detachTree(e, ServerPhysicsSyncTree.getTree(e));
        }
    }

    /**
     * Destroys a body + collidable pair
     * @param body The body
     * @param collidable The collidable
     */
    protected void destroyPhysicsPair(DBody body, Collidable collidable){
        spaceLock.lock();
        this.deregisterCollisionObject(body, collidable);
        this.destroyDBody(body);
        spaceLock.unlock();
    }

    /**
     * Disables a body
     * @param body The body
     */
    protected void disable(DBody body){
        spaceLock.lock();
        body.disable();
        spaceLock.unlock();
    }

    /**
     * Enables a body
     * @param body The body
     */
    protected void enable(DBody body){
        spaceLock.lock();
        body.enable();
        spaceLock.unlock();
    }

    /**
     * Checks if a body is enabled
     * @param body The body
     * @return true if it is enabled, false otherwise
     */
    protected boolean isEnabled(DBody body){
        spaceLock.lock();
        boolean rVal = body.isEnabled();
        spaceLock.unlock();
        return rVal;
    }
    
    /**
     * 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.lock();
        DTriMeshData data = OdeHelper.createTriMeshData();
        data.build(verts, indices);
        final int preprocessFlags = 
        (1 << DTriMeshData.dTRIDATAPREPROCESS_BUILD.CONCAVE_EDGES) | 
        (1 << DTriMeshData.dTRIDATAPREPROCESS_BUILD.FACE_ANGLES) | 
        (1 << DTriMeshData.dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX)
        ;
        data.preprocess2(preprocessFlags,null);
        DTriMesh rVal = OdeHelper.createTriMesh(space, data);
        rVal.setTrimeshData(data);
        rVal.setCategoryBits(categoryBits);
        spaceLock.unlock();
        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.lock();
        DBox boxGeom = OdeHelper.createBox(space, dimensions.x, dimensions.y, dimensions.z);
        boxGeom.setCategoryBits(categoryBits);
        spaceLock.unlock();
        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.lock();
        DCylinder cylinderGeom = OdeHelper.createCylinder(space, radius, length);
        cylinderGeom.setCategoryBits(categoryBits);
        spaceLock.unlock();
        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.lock();
        DSphere sphereGeom = OdeHelper.createSphere(space, radius);
        sphereGeom.setCategoryBits(categoryBits);
        spaceLock.unlock();
        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.lock();
        DCapsule capsuleGeom = OdeHelper.createCapsule(space, radius, length);
        capsuleGeom.setCategoryBits(categoryBits);
        spaceLock.unlock();
        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.lock();
        DBody body = OdeHelper.createBody(world);
        body.setDamping(DEFAULT_LINEAR_DAMPING, DEFAULT_ANGULAR_DAMPING);
        body.setMaxAngularSpeed(DEFAULT_MAX_ANGULAR_SPEED);
        if(geom != null){
            for(int i = 0; i < geom.length; i++){
                if(geom != null){
                    geom[i].setBody(body);
                }
            }
        }
        spaceLock.unlock();
        return body;
    }

    /**
     * Creates a DMass and attaches a body to it
     * @param massValue The amount of mass for the object
     * @param radius The radius of the cylinder
     * @param length The length of the cylinder
     * @param body The DBody to attach the mass to
     * @return The DMass
     */
    protected DMass createCylinderMass(double massValue, double radius, double length, DBody body, Vector3d offset, Quaterniond rotation){
        spaceLock.lock();
        DMass mass = OdeHelper.createMass();
        mass.setCylinder(massValue, 2, radius, length);
        body.setMass(mass);
        spaceLock.unlock();
        return mass;
    }

    /**
     * Creates a DMass and attaches a body to it
     * @param massValue The amount of mass for the object
     * @param dims The dimensions of the box
     * @param body The DBody to attach the mass to
     * @return The DMass
     */
    protected DMass createBoxMass(double massValue, Vector3d dims, DBody body, Vector3d offset, Quaterniond rotation){
        spaceLock.lock();
        DMass mass = OdeHelper.createMass();
        mass.setBox(massValue, dims.x, dims.y, dims.z);
        body.setMass(mass);
        spaceLock.unlock();
        return mass;
    }

    /**
     * Creates a DMass and attaches a body to it
     * @param massValue The amount of mass for the object
     * @param radius The radius of the capsule
     * @param length The length of the capsule
     * @param body The DBody to attach the mass to
     * @return The DMass
     */
    protected DMass createCapsuleMass(double massValue, double radius, double length, DBody body, Vector3d offset, Quaterniond rotation){
        spaceLock.lock();
        DMass mass = OdeHelper.createMass();
        mass.setCapsule(massValue, 2, radius, length);
        body.setMass(mass);
        spaceLock.unlock();
        return mass;
    }

    /**
     * Sets the auto disable flags for the body
     * @param body The body
     * @param autoDisable whether auto disable should be used or not
     * @param linearThreshold The linear velocity threshold to disable under
     * @param angularThreshold The angular velocity threshold to disable under
     * @param steps The number of steps the body must be beneath the threshold before disabling
     */
    protected void setAutoDisableFlags(DBody body, boolean autoDisable, double linearThreshold, double angularThreshold, int steps){
        spaceLock.lock();
        body.setAutoDisableFlag(autoDisable);
        body.setAutoDisableLinearThreshold(0.1);
        body.setAutoDisableAngularThreshold(angularThreshold);
        body.setAutoDisableSteps(steps);
        spaceLock.unlock();
    }

    /**
     * Sets the damping of the body
     * @param body The body
     * @param linearDamping The linear damping
     * @param angularDamping The angular damping
     */
    protected void setDamping(DBody body, double linearDamping, double angularDamping){
        spaceLock.lock();
        body.setDamping(linearDamping, angularDamping);
        spaceLock.unlock();
    }

    /**
     * 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.lock();
        body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
        body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        spaceLock.unlock();
    }

    /**
     * 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){
        if(body != null){
            spaceLock.lock();
            body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.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.unlock();
        }
    }

    /**
     * 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.lock();
        body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.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.unlock();
    }

    /**
     * Sets the transform of a geometry (local to the parent)
     * @param geom The geometry
     * @param position The position
     * @param rotation The rotation
     */
    protected void setGeomTransform(DGeom geom, Vector3d position, Quaterniond rotation){
        spaceLock.lock();
        geom.setOffsetPosition(position.x, position.y, position.z);
        geom.setOffsetQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
        spaceLock.unlock();
    }

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

    /**
     * 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.lock();
        rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin);
        spaceLock.unlock();
        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.lock();
        rVal = PhysicsUtils.odeQuatToJomlQuat(body.getQuaternion());
        spaceLock.unlock();
        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.lock();
        body.setKinematic();
        spaceLock.unlock();
    }

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

    /**
     * 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.lock();
        body.getGeomIterator().next().setOffsetPosition(offsetVector.x,offsetVector.y,offsetVector.z);
        spaceLock.unlock();
    }

    /**
     * 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.lock();
        if(angularlyStatic){
            body.setMaxAngularSpeed(0);
        } else {
            body.setMaxAngularSpeed(DEFAULT_MAX_ANGULAR_SPEED);
        }
        spaceLock.unlock();
    }

    /**
     * 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){
        spaceLock.lock();
        this.space.remove(geom);
        geom.DESTRUCTOR();
        geom.destroy();
        spaceLock.unlock();
    }

    /**
     * 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);
    }

}
