package electrosphere.game.collision;

import electrosphere.collision.dispatch.CollisionObject;
import electrosphere.collision.shapes.BoxShape;
import electrosphere.collision.shapes.BvhTriangleMeshShape;
import electrosphere.collision.shapes.CylinderShape;
import electrosphere.collision.shapes.IndexedMesh;
import electrosphere.collision.shapes.TriangleIndexVertexArray;
import electrosphere.dynamics.RigidBody;
import electrosphere.dynamics.RigidBodyConstructionInfo;
import electrosphere.linearmath.DefaultMotionState;
import electrosphere.linearmath.Transform;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.game.collision.collidable.Collidable;
import electrosphere.main.Globals;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;
import java.nio.IntBuffer;
import org.joml.Quaternionf;
import org.joml.Vector3d;
import org.joml.Vector3f;

/**
 *
 * @author amaterasu
 */
public class PhysicsUtils {
    
    /**
     * Constructor for rigid body:
     * http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/CylinderShape.html
     * Creates a sphere where the dimensions are twice the size of the vector passed in
     * ie diameter = x * 2 or z * 2
     * @param entity
     * @param halfDimensions 
     */
    public static void attachCylinderRigidBody(Entity entity, Vector3f halfDimensions){
        new electrosphere.collision.shapes.CylinderShape(new javax.vecmath.Vector3f(halfDimensions.x,halfDimensions.y,halfDimensions.z));
    }
    
    
    public static RigidBody attachTerrainRigidBody(Entity terrain, float[][] heightfield){
        
        Vector3d position = EntityUtils.getPosition(terrain);
        
        int arrayLength = heightfield.length;
        int arrayWidth = heightfield[0].length;
        
        /*
        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];
                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++;
                }
            }
        }
        
        
        javax.vecmath.Vector3f aabbMin = new javax.vecmath.Vector3f();
        javax.vecmath.Vector3f aabbMax = new javax.vecmath.Vector3f();
        
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/IndexedMesh.html
        electrosphere.collision.shapes.IndexedMesh indexedMesh = new electrosphere.collision.shapes.IndexedMesh();
        
        indexedMesh.numTriangles = indices.length / 3;
        indexedMesh.triangleIndexBase = ByteBuffer.allocateDirect(indices.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.triangleIndexBase.asIntBuffer().put(indices);
        indexedMesh.triangleIndexStride = 3 * Float.BYTES;
        
        indexedMesh.numVertices = vertices.length / 3;
        indexedMesh.vertexBase = ByteBuffer.allocateDirect(vertices.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.vertexBase.asFloatBuffer().put(vertices);
        indexedMesh.vertexStride = 3 * Float.BYTES;
        
        
        
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shpaes/TriangleIndexVertexArray.html
        electrosphere.collision.shapes.TriangleIndexVertexArray triangleIndexArray = new electrosphere.collision.shapes.TriangleIndexVertexArray();
        triangleIndexArray.addIndexedMesh(indexedMesh); //this assumes the scalar type is integer (assumes bytebuffer is actually integer
//        triangleIndexArray.calculateAabbBruteForce(aabbMin, aabbMax);
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html
        electrosphere.collision.shapes.BvhTriangleMeshShape terrainShape = new electrosphere.collision.shapes.BvhTriangleMeshShape(
                triangleIndexArray,
                true // "useQuantizedAabbCompression" -- apparently means better memory usage ( http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html )
        );
        
        //uncomment if we start falling through things again
//        terrainShape.setMargin(0.08f);
        
//        terrainShape.localGetSupportingVertex(new javax.vecmath.Vector3f(1,0,0), aabbMin);
//        terrainShape.recalcLocalAabb();
//        terrainShape.getLocalAabbMin(aabbMin);
//        terrainShape.getLocalAabbMax(aabbMax);
        
        
        DefaultMotionState defaultMotionState = new DefaultMotionState(new Transform(new javax.vecmath.Matrix4f(new javax.vecmath.Quat4f(0,0,0,1),new javax.vecmath.Vector3f((float)position.x,(float)position.y,(float)position.z),1.0f)));
        RigidBodyConstructionInfo terrainRigidBodyCI = new RigidBodyConstructionInfo(0, defaultMotionState, terrainShape);
        RigidBody terrainRigidBody = new RigidBody(terrainRigidBodyCI);
        
//        terrainRigidBody.setFriction(1f);
        
        
        Globals.collisionEngine.registerCollisionObject(terrainRigidBody, new Collidable(terrain,Collidable.TYPE_TERRAIN));
        
//        terrainRigidBody.getAabb(aabbMin, aabbMax);
//        
//        System.out.println("aabbMin: " + aabbMin + "    aabbMax: " + aabbMax);
        
        
        terrain.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, terrainRigidBody);
        
        return terrainRigidBody;
        
    }
    
    
    public static void addTestPlaneRigidBody(){
        
        /*
        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[] indices = {
          0, 1, 3,
          1, 2, 3
        };
        
        float[] coords = {
            0, 0, 0,
            100, 0, 0,
            0, 0, 100,
            100, 0, 100
        };
        
        
        IndexedMesh indexedMesh = new IndexedMesh();
        indexedMesh.numTriangles = indices.length / 3;
        indexedMesh.triangleIndexBase = ByteBuffer.allocateDirect(indices.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.triangleIndexBase.asIntBuffer().put(indices);
        indexedMesh.triangleIndexStride = 3 * Float.BYTES;
        indexedMesh.numVertices = coords.length / 3;
        indexedMesh.vertexBase = ByteBuffer.allocateDirect(coords.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.vertexBase.asFloatBuffer().put(coords);
        indexedMesh.vertexStride = 3 * Float.BYTES;
        
        
        
        javax.vecmath.Vector3f aabbMin = new javax.vecmath.Vector3f();
        javax.vecmath.Vector3f aabbMax = new javax.vecmath.Vector3f();
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shpaes/TriangleIndexVertexArray.html
        electrosphere.collision.shapes.TriangleIndexVertexArray triangleIndexArray = new electrosphere.collision.shapes.TriangleIndexVertexArray(
//                numberTriangles,
//                triangleData,
//                triangleStride,
//                numberVertices,
//                vertexData,
//                vertexStride
        );
        triangleIndexArray.addIndexedMesh(indexedMesh);
//        triangleIndexArray.setScaling(new javax.vecmath.Vector3f(1.0f,1.0f,1.0f));
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html
        electrosphere.collision.shapes.BvhTriangleMeshShape terrainShape = new electrosphere.collision.shapes.BvhTriangleMeshShape(
                triangleIndexArray,
                true // "useQuantizedAabbCompression" -- apparently means better memory usage ( http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html )
        );
        
        
        DefaultMotionState defaultMotionState = new DefaultMotionState(new Transform(new javax.vecmath.Matrix4f(new javax.vecmath.Quat4f(0,0,0,1),new javax.vecmath.Vector3f(0,0,0),1.0f)));
        RigidBodyConstructionInfo terrainRigidBodyCI = new RigidBodyConstructionInfo(0, defaultMotionState, terrainShape);
        RigidBody terrainRigidBody = new RigidBody(terrainRigidBodyCI);
        
        
        
//        Globals.collisionEngine.registerCollisionObject(terrainRigidBody, new Collidable(terrain,Collidable.TYPE_TERRAIN));
        
        terrainRigidBody.getAabb(aabbMin, aabbMax);
        
    }
    
    public static Vector3f getRigidBodyPosition(CollisionObject body){
        javax.vecmath.Vector3f transform = new javax.vecmath.Vector3f(0,0,0);
        body.getWorldTransform(new electrosphere.linearmath.Transform()).transform(transform);
//        body.getMotionState().getWorldTransform(new com.bulletphysics.linearmath.Transform()).transform(transform);
        return vecmathToJomlVector3f(transform);
    }
    
    public static Quaternionf getRigidBodyRotation(RigidBody body){
        return vecmathtoJomlQuaternionf(body.getMotionState().getWorldTransform(new electrosphere.linearmath.Transform()).getRotation(new javax.vecmath.Quat4f()));
    }
    
    public static Vector3f vecmathToJomlVector3f(javax.vecmath.Vector3f vector){
        return new Vector3f(vector.x,vector.y,vector.z);
    }
    
    public static Quaternionf vecmathtoJomlQuaternionf(javax.vecmath.Quat4f quaternion){
        return new Quaternionf(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }
    
    public static javax.vecmath.Vector3f jomlToVecmathVector3f(Vector3f vector){
        return new javax.vecmath.Vector3f(vector.x, vector.y, vector.z);
    }
    
    public static javax.vecmath.Quat4f jomlToVecmathQuaternionf(Quaternionf quaternion){
        return new javax.vecmath.Quat4f(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }
    
    public static electrosphere.linearmath.Transform jomlVecToTransform(Vector3f vector){
        electrosphere.linearmath.Transform transform = new electrosphere.linearmath.Transform();
        
        javax.vecmath.Matrix4f transformMatrix = new javax.vecmath.Matrix4f();
        transformMatrix.setIdentity();
        
        transformMatrix.setTranslation(new javax.vecmath.Vector3f(vector.x, vector.y, vector.z));
        transform.set(transformMatrix);
        
        return transform;
    }
    
    public static void setRigidBodyTransform(Vector3f position, Quaternionf rotation, CollisionObject body){
        electrosphere.linearmath.Transform transform = new electrosphere.linearmath.Transform();
        
        javax.vecmath.Matrix4f transformMatrix = new javax.vecmath.Matrix4f();
        transformMatrix.setIdentity();
        
        transformMatrix.setTranslation(new javax.vecmath.Vector3f(position.x, position.y, position.z));
        transformMatrix.setRotation(new javax.vecmath.Quat4f(rotation.x,rotation.y,rotation.z,rotation.w));
        transform.set(transformMatrix);
        
        
        //https://docs.oracle.com/cd/E17802_01/j2se/javase/technologies/desktop/java3d/forDevelopers/j3dapi/javax/vecmath/Quat4f.html
        //constructor is x,y,z,w
//        transform.setRotation(new javax.vecmath.Quat4f(rotation.x,rotation.y,rotation.z,rotation.w));
        
        body.setWorldTransform(transform);
    }
    
//    public static RigidBody getUnitCylinderRigidBody(float mass){
//        CylinderShape cylinderShape = new CylinderShape(jomlToVecmathVector3f(new Vector3f(1.0f,1.0f,1.0f)));
//        DefaultMotionState defaultMotionState = new DefaultMotionState(new Transform(new javax.vecmath.Matrix4f(new javax.vecmath.Quat4f(0,1,0,1),new javax.vecmath.Vector3f(0,0,0),1.0f)));
//        RigidBodyConstructionInfo cylinderRigidBodyCI = new RigidBodyConstructionInfo(mass, defaultMotionState, cylinderShape);
//        RigidBody cylinderRigidBody = new RigidBody(cylinderRigidBodyCI);
////        cylinderRigidBody.setMassProps(mass, PhysicsUtils.jomlToVecmathVector3f(new Vector3f(1.0f,1.0f,1.0f)));
//        cylinderRigidBody.clearForces();
//        return cylinderRigidBody;
//    }
    
    public static CollisionObject getCylinderObject(Vector3f dimensions){
        CylinderShape cylinderShape = new CylinderShape(jomlToVecmathVector3f(dimensions));
        CollisionObject cylinderCollisionObject = new CollisionObject();
        cylinderCollisionObject.setCollisionShape(cylinderShape);
        return cylinderCollisionObject;
    }
    
    public static CollisionObject getPlaneObject(Vector3f dimensions){
        int[] indices = {
          0, 1, 2,
          1, 2, 3
        };
        
        float[] coords = {
            -1 * dimensions.x,    0,    -1 * dimensions.z,
             1 * dimensions.x,    0,    -1 * dimensions.z,
            -1 * dimensions.x,    0,     1 * dimensions.z,
             1 * dimensions.x,    0,     1 * dimensions.z
        };
        
        IndexedMesh indexedMesh = new IndexedMesh();
        indexedMesh.numTriangles = indices.length / 3;
        indexedMesh.triangleIndexBase = ByteBuffer.allocateDirect(indices.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.triangleIndexBase.asIntBuffer().put(indices);
        indexedMesh.triangleIndexStride = 3 * Float.BYTES;
        indexedMesh.numVertices = coords.length / 3;
        indexedMesh.vertexBase = ByteBuffer.allocateDirect(coords.length*Float.BYTES).order(ByteOrder.nativeOrder());
        indexedMesh.vertexBase.asFloatBuffer().put(coords);
        indexedMesh.vertexStride = 3 * Float.BYTES;
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shpaes/TriangleIndexVertexArray.html
        TriangleIndexVertexArray triangleIndexArray = new TriangleIndexVertexArray();
        triangleIndexArray.addIndexedMesh(indexedMesh);
//        triangleIndexArray.setScaling(new javax.vecmath.Vector3f(1.0f,1.0f,1.0f));
        
        
        
        //http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html
        BvhTriangleMeshShape planeShape = new BvhTriangleMeshShape(
                triangleIndexArray,
                true // "useQuantizedAabbCompression" -- apparently means better memory usage ( http://jbullet.advel.cz/javadoc/com/bulletphysics/collision/shapes/BvhTriangleMeshShape.html )
        );
        
        
        CollisionObject planeCollisionObject = new CollisionObject();
        planeCollisionObject.setCollisionShape(planeShape);
        
        return planeCollisionObject;
    }
    
    
    public static CollisionObject getCubeObject(Vector3f dimensions){
        BoxShape boxShape = new BoxShape(jomlToVecmathVector3f(dimensions));
        CollisionObject boxCollisionObject = new CollisionObject();
        boxCollisionObject.setCollisionShape(boxShape);
        return boxCollisionObject;
    }
}
