package electrosphere.entity.state.collidable;

import electrosphere.collision.dispatch.CollisionObject;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.state.gravity.GravityTree;
import electrosphere.entity.types.collision.CollisionObjUtils;
import electrosphere.entity.types.creature.CreatureUtils;
import electrosphere.entity.types.debug.DebugVisualizerUtils;
import electrosphere.entity.types.item.ItemUtils;
import electrosphere.game.collision.PhysicsUtils;
import electrosphere.game.collision.collidable.Collidable;
import electrosphere.game.data.creature.type.CollidableTemplate;
import electrosphere.main.Globals;

import org.joml.Matrix4f;
import org.joml.Quaterniond;
import org.joml.Quaternionf;
import org.joml.Vector3d;
import org.joml.Vector3f;
import org.joml.Vector4d;
import org.joml.Vector4f;

/**
 *
 * @author amaterasu
 */
public class CollidableTree {
    
    Entity parent;
    CollisionObject body;
    Collidable collidable;
    Quaternionf angularVelocity = new Quaternionf(0,0,0,0);
    Vector4d cumulativeTorque = new Vector4d(0,0,0,0);
    
    static final float DELTA_T = 0.01f;
    
    public CollidableTree(Entity e, Collidable collidable, CollisionObject body){
        parent = e;
        this.collidable = collidable;
        this.body = body;
    }
    
    static int incrementer = 0;
    
    public void simulate(){
        Vector3d position = EntityUtils.getPosition(parent);
        Quaternionf rotation = EntityUtils.getRotation(parent);
        Matrix4f inverseInertiaTensor = CollisionObjUtils.getInverseInertiaTensor(parent);
        Vector3d offsetVector = new Vector3d();
        Vector3d newPosition = new Vector3d(position);
        javax.vecmath.Matrix4f bodyTransformMatrix;
        //have we hit a terrain impulse?
        boolean hitTerrain = false;
        //handle impulses
        for(Impulse impulse : collidable.getImpulses()){
//            collidable.getImpulses().remove(impulse);
            Vector3d impulseForce = new Vector3d(impulse.getDirection()).mul(impulse.getForce());
            if(impulse.type.matches(Collidable.TYPE_TERRAIN)){
                hitTerrain = true;
//                System.out.println("Impulse force: " + impulseForce);
//                System.out.println("Position: " + position);
            }
            if(impulse.type.matches(Collidable.TYPE_ITEM)){
                if(parent.containsKey(EntityDataStrings.GRAVITY_TREE)){
                    ((GravityTree)parent.getData(EntityDataStrings.GRAVITY_TREE)).start();
                }
            }
            if(impulse.type.matches(Collidable.TYPE_CREATURE)){
//                System.out.println(System.currentTimeMillis() + " creature hit!");
                if(parent.containsKey(EntityDataStrings.GRAVITY_TREE)){
                    ((GravityTree)parent.getData(EntityDataStrings.GRAVITY_TREE)).start();
                }
            }
            if(
                impulse.getCollisionPoint().length() > 0 &&
                !Double.isNaN(impulse.getCollisionPoint().x) && 
                !Double.isNaN(impulse.getCollisionPoint().y) && 
                !Double.isNaN(impulse.getCollisionPoint().z) && 
                !Double.isNaN(impulse.getDirection().x) &&
                !Double.isNaN(impulse.getDirection().y) &&
                !Double.isNaN(impulse.getDirection().z) &&
                ItemUtils.isItem(parent)
            ){
                // Vector3d collisionPoint = new Vector3d(impulse.getCollisionPoint()).normalize();
                Vector3d collisionPoint = new Vector3d(impulse.getWorldPoint()).sub(position);
                Vector3d forceDir = new Vector3d(impulse.getDirection()).normalize();
                Vector3d torqueVec = new Vector3d(collisionPoint).cross(forceDir).normalize();
                // double torqueMag = Math.abs(impulse.force);
                double torqueMag = Math.sqrt(impulse.getCollisionPoint().length()) * impulse.getForce();
                if(impulse.getType().equals(Collidable.TYPE_TERRAIN)){
                    torqueMag = torqueMag * 3;
                }
                    torqueMag = torqueMag * 10;
                // } else {
                //     torqueMag = 0;
                // }
                // if(impulse.type.matches(Collidable.TYPE_ITEM) && ItemUtils.isItem(parent)){
                //     // System.out.println(rotation);
                //     if(impulse.collisionPoint.x < 0){
                //         // System.out.println("Impulse collision point: " + impulse.getCollisionPoint() + " direction: " + impulse.getDirection() + " => " + new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
                //         cumulativeTorque.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
                //     }
                // } else {
                //     // angularVelocity.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()),1.0);
                //     cumulativeTorque.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
                // }
                if(impulse.type.matches(Collidable.TYPE_CREATURE)){
                    // System.out.println("Impulse: " + impulse.getCollisionPoint() + " x " + impulse.getDirection() + " ->f " + impulse.force);
                    // incrementer++;
                    // if(incrementer > 5){
                    //     Globals.microSimulation.freeze();
                    // } else if(incrementer <= 5){
                    //     // Globals.controlHandler.showMouse();
                        // Vector3d pos = impulse.getWorldPoint();
                    //     // pos = new Vector3d(position).add(impulse.getCollisionPoint()).mul(1,0,1);
                        // DebugVisualizerUtils.spawnVectorVisualizer(impulse.getWorldPoint(), new Vector3d(torqueVec));
                    // }
                    // System.out.println("Impulse: " + torqueVec + " " + torqueMag);
                }
                // if(CreatureUtils.isCreature(parent) && forceDir.y < 0.5){
                //     System.out.println(collisionPoint + " x " + forceDir + " => " + torqueVec + " " + torqueMag);
                // }
                Quaternionf impulseRotation = new Quaternionf().rotationAxis((float)torqueMag * DELTA_T,new Vector3f((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z));
                if(angularVelocity.lengthSquared() > 0.001){
                    angularVelocity.mul(impulseRotation);
                } else {
                    angularVelocity.set(impulseRotation);
                }
                // angularVelocity.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
                // cumulativeTorque.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
            }
            offsetVector.add(impulseForce);
        }
        // if(ItemUtils.isItem(parent) && cumulativeTorque.w > 0.001){
        //     System.out.println(cumulativeTorque);
        // }
        //friction
        if(angularVelocity.lengthSquared() > 0.001){
            angularVelocity.slerp(new Quaternionf(0,0,0,1), 0.03f);
            // angularVelocity.scale((float)(Math.sqrt(angularVelocity.lengthSquared()) * 0.9));
            // System.out.println(angularVelocity);
        }
        // if(cumulativeTorque.w > 0.001){
        //     Vector4f holder = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.y,(float)cumulativeTorque.z,(float)cumulativeTorque.w));
        //     cumulativeTorque = new Vector4d(holder.x,holder.y,holder.z,holder.w);
        //     angularVelocity = angularVelocity.add(cumulativeTorque).normalize();
        //     cumulativeTorque.set(0,0,0,0);
        // //     Vector3d normalizedTorqueDir = new Vector3d(cumulativeTorque.x,cumulativeTorque.y,cumulativeTorque.z).normalize();
        // //     double newMag = cumulativeTorque.w * 0.9;
        // //     cumulativeTorque.set(normalizedTorqueDir.x,normalizedTorqueDir.y,normalizedTorqueDir.z,newMag);
        // }
        if(angularVelocity.lengthSquared() > 0.001){
            // System.out.println("-" + rotation);
            Quaternionf newRotation = new Quaternionf(rotation).mul(angularVelocity).normalize();
            // if(new Quaternionf(newRotation).add(new Quaternionf(rotation).conjugate()).lengthSquared() > 0.2){
            //     newRotation.w = Math.copySign(newRotation.w, rotation.w);
            //     newRotation.x = Math.copySign(newRotation.x, rotation.x);
            //     newRotation.y = Math.copySign(newRotation.y, rotation.y);
            //     newRotation.z = Math.copySign(newRotation.z, rotation.z);
            // }
            rotation.set(newRotation);
            // System.out.println("=" + rotation);
        }
        // if(inverseInertiaTensor != null && angularVelocity.w > 0.01){
        //     // Vector4f angularMomentum = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.w));
        //     // Quaternionf nextRotation = new Quaternionf(rotation).mul(new Quaternionf(angularMomentum.x,angularMomentum.y,angularMomentum.z,angularMomentum.w).scale(0.001f)).normalize();
        //     // rotation = nextRotation;
        //     // rotation.mul(new Quaternionf((float)angularMomentum.x,(float)angularMomentum.y,(float)angularMomentum.z,(float)angularMomentum.w / 0.01f));
        //     // if(ItemUtils.isItem(parent)){
        //     //     System.out.println("cumulative quat: " + cumulativeTorque);
        //     // }
        //     rotation.x = rotation.x + rotation.x * (float)(angularVelocity.x * angularVelocity.w) * 0.01f;
        //     rotation.y = rotation.y + rotation.y * (float)(angularVelocity.y * angularVelocity.w) * 0.01f;
        //     rotation.z = rotation.z + rotation.z * (float)(angularVelocity.z * angularVelocity.w) * 0.01f;
        //     rotation.w = 1;
        //     // rotation.w = rotation.w + rotation.w * (float)cumulativeTorque.w * 0.001f;
        //     rotation.normalize();
        // }
        
        //the reasoning here is that if we have something above something else and it's pushing it into the terrain,
        //we should instead just not push into terrain and push along terrain
        if(hitTerrain && offsetVector.y < 0){
            offsetVector.y = 0;
        }
        
        //make sure we're in a valid (World bounds) position
        newPosition.add(offsetVector);
        if(!Globals.collisionEngine.checkCanOccupyPosition(Globals.commonWorldData, parent, newPosition)){
            newPosition = Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData, parent, newPosition);
        }
        position.set(newPosition);
        
        //update collision engine of this thing's position
        CollidableTemplate template = (CollidableTemplate)parent.getData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE);
        bodyTransformMatrix = new javax.vecmath.Matrix4f(PhysicsUtils.jomlToVecmathQuaternionf(rotation),PhysicsUtils.jomlToVecmathVector3f(new Vector3f((float)position.x + template.getOffsetX(),(float)position.y + template.getOffsetY(),(float)position.z + template.getOffsetZ())),1.0f);
        body.setWorldTransform(new electrosphere.linearmath.Transform(bodyTransformMatrix));
        
//        bodyTransformMatrix = new javax.vecmath.Matrix4f(PhysicsUtils.jomlToVecmathQuaternionf(rotation),PhysicsUtils.jomlToVecmathVector3f(new Vector3f((float)newPosition.x,(float)newPosition.y,(float)newPosition.z)),1.0f);
//        body.setWorldTransform(new electrosphere.linearmath.Transform(bodyTransformMatrix));
        
    }
    
    
    public void setCollisionObject(CollisionObject body, Collidable collidable){
        this.body = body;
        this.collidable = collidable;
    }
    
    
    
    
}