package electrosphere.entity.state.movement;

import electrosphere.entity.state.collidable.Impulse;
import electrosphere.collision.dispatch.CollisionObject;
import electrosphere.dynamics.RigidBody;
import electrosphere.entity.CameraEntityUtils;
import electrosphere.entity.types.creature.CreatureUtils;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.state.GravityTree;
import electrosphere.entity.state.GravityTree;
import electrosphere.game.collision.CollisionEngine;
import electrosphere.game.collision.PhysicsUtils;
import electrosphere.game.collision.collidable.Collidable;
import electrosphere.main.Globals;
import electrosphere.main.Main;
import electrosphere.net.NetUtils;
import electrosphere.net.parser.net.message.EntityMessage;
import electrosphere.renderer.Actor;
import electrosphere.renderer.anim.Animation;
import electrosphere.renderer.Model;
import java.util.LinkedList;
import java.util.List;
import java.util.concurrent.CopyOnWriteArrayList;
import org.joml.Quaternionf;
import org.joml.Vector3d;
import org.joml.Vector3f;

/*
Behavior tree for movement in an entity
*/
public class GroundMovementTree {
    
    public static enum MovementTreeState {
        STARTUP,
        MOVE,
        SLOWDOWN,
        IDLE,
    }
    
    
    
    MovementTreeState state;
    
    Entity parent;
    
    Collidable collidable;
    
    CopyOnWriteArrayList<EntityMessage> networkMessageQueue = new CopyOnWriteArrayList();
    
    long lastUpdateTime = 0;
    
    
    public GroundMovementTree(Entity e, Collidable collidable){
        state = MovementTreeState.IDLE;
        parent = e;
        this.collidable = collidable;
    }
    
    public MovementTreeState getState(){
        return state;
    }
    
    public void start(){
        //TODO: check if can start moving
        state = MovementTreeState.STARTUP;
    }
    
    public void interrupt(){
        state = MovementTreeState.IDLE;
    }
    
    public void slowdown(){
        state = MovementTreeState.SLOWDOWN;
    }
    
    public void simulate(){
        float velocity = CreatureUtils.getVelocity(parent);
        float acceleration = CreatureUtils.getAcceleration(parent);
        float maxNaturalVelocity = CreatureUtils.getMaxNaturalVelocity(parent);
        Actor entityActor = EntityUtils.getActor(parent);
//        Model entityModel = Globals.assetManager.fetchModel(EntityUtils.getEntityModelPath(parent));
        Vector3d position = EntityUtils.getPosition(parent);
        Vector3f movementVector = CreatureUtils.getMovementVector(parent);
//        float movementYaw = CameraEntityUtils.getCameraYaw(Globals.playerCamera);
        Quaternionf movementQuaternion = new Quaternionf().rotationTo(new Vector3f(0,0,1), movementVector).normalize();
        Quaternionf rotation = EntityUtils.getRotation(parent);
        Vector3d newPosition;
        
        //parse attached network messages
        for(EntityMessage message : networkMessageQueue){
            networkMessageQueue.remove(message);
            long updateTime = message.gettime();
//            System.out.println("MOVE to " + message.getX() + " " + message.getY() + " " + message.getZ());
            switch(message.getMessageSubtype()){
                case MOVE:
                    position.set(message.getpositionX(), message.getpositionY(), message.getpositionZ());
                    if(Globals.RUN_SERVER){
                        Globals.server.broadcastMessage(
                                EntityMessage.constructMoveMessage(
                                    parent.getId(),
                                    System.currentTimeMillis(),
                                    message.getpositionX(),
                                    message.getpositionY(),
                                    message.getpositionZ()
                                )
                        );
                    }
                    break;
                case SETFACING:
                    break;
                case MOVEUPDATE:
                    if(updateTime > lastUpdateTime){
                        lastUpdateTime = updateTime;
                        switch(message.gettreeState()){
                            case 0:
                                state = MovementTreeState.STARTUP;
    //                            System.out.println("Set state STARTUP");
                                activateGravityTree();
                                break;
                            case 1:
                                state = MovementTreeState.MOVE;
//                                System.out.println("Set state MOVE");
                                activateGravityTree();
                                break;
                            case 2:
                                state = MovementTreeState.SLOWDOWN;
    //                            System.out.println("Set state SLOWDOWN");
                                activateGravityTree();
                                break;
                            case 3:
                                state = MovementTreeState.IDLE;
    //                            System.out.println("Set state IDLE");
                                break;
                        }
    //                    System.out.println(EntityUtils.getEntityPosition(parent));
    //                    System.out.println(message.getpositionX() + " " + message.getpositionY() + " " + message.getpositionZ());
                        EntityUtils.getPosition(parent).set(message.getpositionX(),message.getpositionY(),message.getpositionZ());
                        CreatureUtils.setMovementVector(parent, new Vector3f(message.getrotationX(),message.getrotationY(),message.getrotationZ()));
    //                    EntityUtils.getEntityRotation(parent).set(message.getrotationX(), message.getrotationY(), message.getrotationZ(), message.getrotationW()).normalize();
    //                    velocity = message.getvelocity();
                        break;
                    }
            }
        }
        
        
        //state machine
        switch(state){
            case STARTUP:
                //run startup code
                velocity = velocity + acceleration;
                CreatureUtils.setVelocity(parent, velocity);
                if(entityActor != null){
                    if(!entityActor.isPlayingAnimation() || !entityActor.getCurrentAnimation().equals(Animation.ANIMATION_MOVEMENT_STARTUP)){
                        entityActor.playAnimation(Animation.ANIMATION_MOVEMENT_STARTUP);
                        entityActor.incrementAnimationTime(0.01);
                    }
                }
                //check if can transition state
                if(velocity >= maxNaturalVelocity){
                    velocity = maxNaturalVelocity;
                    state = MovementTreeState.MOVE;
                }
//                body.applyCentralForce(PhysicsUtils.jomlToVecmathVector3f(new Vector3f(movementVector.x,0,movementVector.z).normalize().mul(velocity)));
                EntityUtils.getRotation(parent).set(movementQuaternion);
//                //move the entity
//                newPosition = new Vector3d(position).add(new Vector3d(movementVector).mul(velocity).mul(Main.deltaTime));
//                //check/update if collision
//                if(!Globals.collisionEngine.checkCanOccupyPosition(Globals.commonWorldData, parent, newPosition)){
//                    newPosition = Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData, parent, newPosition);
//                }
//                //actually update
                collidable.addImpulse(new Impulse(new Vector3d(movementVector), velocity, "movement"));
//                position.set(newPosition);
                rotation.set(movementQuaternion);
                
                activateGravityTree();
                
                if(Globals.RUN_SERVER){
//                    Globals.server.broadcastMessage(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    newPosition.x,
//                                    newPosition.y,
//                                    newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    0
//                            )
//                    );
                    Globals.dataCellManager.sendNetworkMessageToChunk(
                            EntityMessage.constructmoveUpdateMessage(
                                    parent.getId(),
                                    System.currentTimeMillis(),
                                    (float)position.x,
                                    (float)position.y,
                                    (float)position.z,
                                    movementVector.x,
                                    movementVector.y,
                                    movementVector.z,
                                    velocity,
                                    0
                            ),
                            Globals.serverWorldData.convertRealToChunkSpace(position.x),
                            Globals.serverWorldData.convertRealToChunkSpace(position.z)
                    );
                } else if(Globals.RUN_CLIENT && parent.getId() == Globals.clientCharacterID){
                    Globals.clientConnection.queueOutgoingMessage(
                            EntityMessage.constructmoveUpdateMessage(
                                    parent.getId(),
                                    System.currentTimeMillis(),
                                    (float)position.x,
                                    (float)position.y,
                                    (float)position.z,
                                    movementVector.x,
                                    movementVector.y,
                                    movementVector.z,
                                    velocity,
                                    0
                            )
                    );
                }
                break;
            case MOVE:
                //check if can restart animation
                //if yes, restart animation
                if(entityActor != null){
                    if(!entityActor.isPlayingAnimation() || !entityActor.getCurrentAnimation().equals(Animation.ANIMATION_MOVEMENT_MOVE)){
                        entityActor.playAnimation(Animation.ANIMATION_MOVEMENT_MOVE);
                        entityActor.incrementAnimationTime(0.01);
                    }
                }
//                body.applyCentralForce(PhysicsUtils.jomlToVecmathVector3f(force));
                EntityUtils.getRotation(parent).set(movementQuaternion);
                //check if can move forward (collision engine)
                //if can, move forward by entity movement stats
//                newPosition = new Vector3d(position).add(new Vector3d(movementVector).mul(velocity).mul(Main.deltaTime));
//                if(!Globals.collisionEngine.checkCanOccupyPosition(Globals.commonWorldData, parent, newPosition)){
//                    newPosition = Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData, parent, newPosition);
//                }
                collidable.addImpulse(new Impulse(new Vector3d(movementVector), velocity, "movement"));
//                position.set(newPosition);
                rotation.set(movementQuaternion);
                
                activateGravityTree();
                
                if(Globals.RUN_SERVER){
//                    Globals.server.broadcastMessage(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    newPosition.x,
//                                    newPosition.y,
//                                    newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    1
//                            )
//                    );
//                    Globals.dataCellManager.sendNetworkMessageToChunk(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    (float)newPosition.x,
//                                    (float)newPosition.y,
//                                    (float)newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    1
//                            ),
//                            Globals.serverWorldData.convertRealToChunkSpace(newPosition.x),
//                            Globals.serverWorldData.convertRealToChunkSpace(newPosition.z)
//                    );
                } else if(Globals.RUN_CLIENT && parent.getId() == Globals.clientCharacterID){
//                    Globals.clientConnection.queueOutgoingMessage(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    (float)newPosition.x,
//                                    (float)newPosition.y,
//                                    (float)newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    1
//                            )
//                    );
                }
                break;
            case SLOWDOWN:
                //run slowdown code
                velocity = velocity - acceleration;
                CreatureUtils.setVelocity(parent, velocity);
                if(entityActor != null){
                    if(!entityActor.isPlayingAnimation() || !entityActor.getCurrentAnimation().equals(Animation.ANIMATION_MOVEMENT_STARTUP)){
                        entityActor.playAnimation(Animation.ANIMATION_MOVEMENT_STARTUP);
                        entityActor.incrementAnimationTime(0.01);
                    }
                }
                //check if can transition state
                if(velocity <= 0){
                    velocity = 0;
                    state = MovementTreeState.IDLE;
                }
//                body.applyCentralForce(PhysicsUtils.jomlToVecmathVector3f(new Vector3f(movementVector).mul(-1.0f).normalize().mul(velocity)));
                EntityUtils.getRotation(parent).rotationTo(new Vector3f(0,0,1), movementVector);
                //move the entity
//                newPosition = new Vector3d(position).add(new Vector3d(movementVector).mul(velocity).mul(Main.deltaTime));
//                if(!Globals.collisionEngine.checkCanOccupyPosition(Globals.commonWorldData, parent, newPosition)){
//                    newPosition = Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData, parent, newPosition);
//                }
                collidable.addImpulse(new Impulse(new Vector3d(movementVector), velocity, "movement"));
//                position.set(newPosition);
                rotation.rotationTo(new Vector3f(0,0,1), movementVector);
                
                activateGravityTree();
                
                if(Globals.RUN_SERVER){
//                    Globals.server.broadcastMessage(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    newPosition.x,
//                                    newPosition.y,
//                                    newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    2
//                            )
//                    );
//                    Globals.dataCellManager.sendNetworkMessageToChunk(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    (float)newPosition.x,
//                                    (float)newPosition.y,
//                                    (float)newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    2
//                            ),
//                            Globals.serverWorldData.convertRealToChunkSpace(newPosition.x),
//                            Globals.serverWorldData.convertRealToChunkSpace(newPosition.z)
//                    );
                } else if(Globals.RUN_CLIENT && parent.getId() == Globals.clientCharacterID){
//                    Globals.clientConnection.queueOutgoingMessage(
//                            EntityMessage.constructmoveUpdateMessage(
//                                    parent.getId(),
//                                    System.currentTimeMillis(),
//                                    (float)newPosition.x,
//                                    (float)newPosition.y,
//                                    (float)newPosition.z,
//                                    movementVector.x,
//                                    movementVector.y,
//                                    movementVector.z,
//                                    velocity,
//                                    2
//                            )
//                    );
                }
                break;
            case IDLE:
//                body.clearForces();
//                if(entityActor != null){
//                    if(!entityActor.isPlayingAnimation() || !entityActor.getCurrentAnimation().equals(Animation.ANIMATION_IDLE_1)){
//                        entityActor.playAnimation(Animation.ANIMATION_IDLE_1);
//                        entityActor.incrementAnimationTime(0.01);
//                    }
//                }
//                if(Globals.collisionEngine.gravityCheck(Globals.commonWorldData, parent)){
//                    position.set(Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData,parent,new Vector3f(position.x,position.y - 9.8f,position.z)));
//                }
//                position.set(new Vector3f(position.x,position.y - 0.08f,position.z));
//                bodyTransformMatrix = new javax.vecmath.Matrix4f(PhysicsUtils.jomlToVecmathQuaternionf(rotation),PhysicsUtils.jomlToVecmathVector3f(position),1.0f);
//                body.setWorldTransform(new com.bulletphysics.linearmath.Transform(bodyTransformMatrix));
                break;
        }
    }

    public void addNetworkMessage(EntityMessage networkMessage) {
        networkMessageQueue.add(networkMessage);
    }
    
    public void activateGravityTree(){
        if(parent.getDataKeys().contains(EntityDataStrings.GRAVITY_ENTITY)){
            GravityTree tree = (GravityTree)parent.getData(EntityDataStrings.GRAVITY_TREE);
            tree.start();
        }
    }
    
}