package electrosphere.collision;

import org.joml.Matrix4d;
import org.joml.Quaterniond;
import org.joml.Vector3d;
import org.ode4j.ode.DBody;
import org.ode4j.ode.DGeom;
import org.ode4j.ode.DTriMesh;

import electrosphere.collision.collidable.Collidable;
import electrosphere.engine.Globals;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityTags;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.state.collidable.ClientCollidableTree;
import electrosphere.entity.state.collidable.ServerCollidableTree;
import electrosphere.entity.state.physicssync.ClientPhysicsSyncTree;
import electrosphere.entity.state.physicssync.ServerPhysicsSyncTree;
import electrosphere.entity.types.terrain.TerrainChunkData;
import electrosphere.game.data.collidable.CollidableTemplate;
import electrosphere.server.datacell.Realm;
import electrosphere.server.datacell.utils.ServerEntityTagUtils;

/**
 * Utilities for performing physics-related operations on entities (particularly attaching physics to them in the first place)
 */
public class PhysicsEntityUtils {

    /**
     * [CLIENT ONLY] Attaches a collidable template to a given entity
     * @param rVal The entity
     * @param physicsTemplate The collidable template
     */
    public static void clientAttachCollidableTemplate(Entity rVal, CollidableTemplate physicsTemplate){
        DBody rigidBody = null;
        Collidable collidable;
        float mass = 1.0f;
        switch(physicsTemplate.getType()){
            case "CYLINDER": {
                rigidBody = CollisionBodyCreation.createCylinderBody(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    physicsTemplate.getDimension1(),
                    physicsTemplate.getDimension2(),
                    Collidable.TYPE_CREATURE_BIT
                );
                CollisionBodyCreation.setOffsetPosition(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE);
                ClientCollidableTree tree = new ClientCollidableTree(rVal,collidable,rigidBody);
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, rigidBody);
                Matrix4d offsetTransform = new Matrix4d().translationRotate(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW() //rotate
                );
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                Globals.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CUBE": {
                rigidBody = CollisionBodyCreation.createCubeBody(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
                    Collidable.TYPE_CREATURE_BIT
                );
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE);
                ClientCollidableTree tree = new ClientCollidableTree(rVal,collidable,rigidBody);
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, rigidBody);
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                Globals.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
            } break;
        }
        //if we successfully attached the body, add a sync tree
        if(rigidBody != null){
            ClientPhysicsSyncTree.attachTree(rVal);
        }
    }


    /**
     * [SERVER ONLY] Attaches a collidable template to a given entity
     * @param realm The realm the entity is inside of
     * @param rVal The entity
     * @param physicsTemplate The collidable template
     */
    public static void serverAttachCollidableTemplate(Realm realm, Entity rVal, CollidableTemplate physicsTemplate){
        DBody rigidBody = null;
        Collidable collidable;
        float mass = 1.0f;
        switch(physicsTemplate.getType()){
            case "CYLINDER": {
                rigidBody = CollisionBodyCreation.createCylinderBody(
                    realm.getCollisionEngine(),
                    physicsTemplate.getDimension1(),
                    physicsTemplate.getDimension2(),
                    Collidable.TYPE_CREATURE_BIT
                );
                CollisionBodyCreation.setOffsetPosition(
                    realm.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE);
                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,rigidBody);
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, rigidBody);
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CUBE": {
                rigidBody = CollisionBodyCreation.createCubeBody(realm.getCollisionEngine(),new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),Collidable.TYPE_CREATURE_BIT);
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE);
                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,rigidBody);
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, rigidBody);
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
            } break;
        }
        //if we successfully attached the body, add a sync tree
        if(rigidBody != null){
            ServerPhysicsSyncTree.attachTree(rVal);
        }
    }




    /**
     * [CLIENT ONLY] Attaches a heightmap dbody to an entity
     * @param terrain The terrain entity
     * @param heightfield The heightfield values
     * 
     * @return The DBody created
     */
    public static DBody clientAttachTerrainRigidBody(Entity terrain, CollisionEngine collisionEngine, float[][] heightfield){
        Vector3d position = EntityUtils.getPosition(terrain);
        int arrayLength = heightfield.length;
        int arrayWidth = heightfield[0].length;
        float collisionMargin = 0.08f;
        
        /*
        Traditional buffer code not working for some reason
        the approach of
        https://stackoverflow.com/questions/40855945/lwjgl-mesh-to-jbullet-collider
        works much better
        IDK why
        */
        
        int numberTriangles = (arrayLength - 1) * (arrayWidth - 1) * 2;
        int triangleStride = 0;
        
        int numberVertices = arrayLength * arrayWidth;
        int vertexStride = 0;
        
        float[] vertices = new float[numberVertices * 3];
        int vertexInserterPos = 0;
        int[] indices = new int[numberTriangles * 3];
        int indexInserterPos = 0;
        
        for(int x = 0; x < arrayLength; x++){
            for(int y = 0; y < arrayWidth; y++){
                vertices[vertexInserterPos] = x;
                vertexInserterPos++;
                vertices[vertexInserterPos] = heightfield[x][y] - collisionMargin;
                vertexInserterPos++;
                vertices[vertexInserterPos] = y;
                vertexInserterPos++;
                if(x < arrayLength - 1 && y < arrayWidth - 1){
                    //if we should also add a triangle index
                    /*
                    as copied from ModelUtil's terrain mesh generation function
                    faces.put((x / stride + 0) * actualHeight + (y / stride + 0));
                    faces.put((x / stride + 0) * actualHeight + (y / stride + 1));
                    faces.put((x / stride + 1) * actualHeight + (y / stride + 0));
                    faces.put((x / stride + 1) * actualHeight + (y / stride + 0));
                    faces.put((x / stride + 0) * actualHeight + (y / stride + 1));
                    faces.put((x / stride + 1) * actualHeight + (y / stride + 1));
                    */
                    indices[indexInserterPos] = (x + 0) * arrayWidth + (y + 0);
                    indexInserterPos++;
                    indices[indexInserterPos] = (x + 0) * arrayWidth + (y + 1);
                    indexInserterPos++;
                    indices[indexInserterPos] = (x + 1) * arrayWidth + (y + 0);
                    indexInserterPos++;
                    indices[indexInserterPos] = (x + 1) * arrayWidth + (y + 0);
                    indexInserterPos++;
                    indices[indexInserterPos] = (x + 0) * arrayWidth + (y + 1);
                    indexInserterPos++;
                    indices[indexInserterPos] = (x + 1) * arrayWidth + (y + 1);
                    indexInserterPos++;
                }
            }
        }
        
        DTriMesh triMesh = collisionEngine.createTrimeshGeom(vertices,indices,Collidable.TYPE_STATIC_BIT);
        DBody body = collisionEngine.createDBody(triMesh);

        Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(body, new Collidable(terrain,Collidable.TYPE_TERRAIN));
        terrain.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, body);

        return body;
    }


    /**
     * [CLIENT ONLY] Given an entity and a terrain chunk description, create physics for the chunk and attach it to the entity
     * @param terrain The entity
     * @param data The terrain description
     * @return The rigid body created (note, attachment has already been performed)
     */
    public static DBody clientAttachTerrainChunkRigidBody(Entity terrain, TerrainChunkData data){
        Vector3d position = EntityUtils.getPosition(terrain);
        DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(Globals.clientSceneWrapper.getCollisionEngine(), data,Collidable.TYPE_STATIC_BIT);
        CollisionBodyCreation.setKinematic(Globals.clientSceneWrapper.getCollisionEngine(), terrainBody);
        CollisionBodyCreation.setGravityMode(Globals.clientSceneWrapper.getCollisionEngine(), terrainBody, false);
        
        
        Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_TERRAIN));
        terrain.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, terrainBody);
        
        return terrainBody;
    }


    /**
     * [SERVER ONLY] Given an entity and a terrain chunk description, create physics for the chunk and attach it to the entity
     * @param terrain The entity
     * @param data The terrain description
     * @return The rigid body created (note, attachment has already been performed)
     */
    public static DBody serverAttachTerrainChunkRigidBody(Entity terrain, TerrainChunkData data){
        Vector3d position = EntityUtils.getPosition(terrain);
        Realm terrainRealm = Globals.realmManager.getEntityRealm(terrain);
        DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(terrainRealm.getCollisionEngine(),data,Collidable.TYPE_STATIC_BIT);
        CollisionBodyCreation.setKinematic(terrainRealm.getCollisionEngine(), terrainBody);
        CollisionBodyCreation.setGravityMode(terrainRealm.getCollisionEngine(), terrainBody, false);
        
        terrainRealm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_TERRAIN));
        terrain.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, terrainBody);
        
        return terrainBody;
    }

    /**
     * Gets the transform from parent entity position to rigid body
     * @param entity The entity
     * @return The transform
     */
    public static Matrix4d getEntityCollidableTransform(Entity entity){
        return (Matrix4d) entity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM);
    }

    /**
     * Gets the DBody attached to an entity
     * @param entity the entity
     * @return The dbody if it exists, null otherwise
     */
    public static DBody getDBody(Entity entity){
        return (DBody)entity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY);
    }

    /**
     * Sets the position of a DGeom
     * @param collisionEngine the collision engine
     * @param geom the geometry
     */
    public static void setGeometryPosition(CollisionEngine collisionEngine, DGeom geom, Vector3d position, Quaterniond rotation){
        collisionEngine.setGeomTransform(geom, position, rotation);
    }
    
}
