//Copyright (c) 2016, jMonkeyEngine. All rights reserved. 
//Distributed under the BSD licence. 
//Read "rottShortcut/graphical/steer/license.txt".

package rottShortcut.graphical.steer;

import com.jme3.ai.agents.Agent;
import com.jme3.ai.agents.behaviors.Behavior;
import com.jme3.ai.agents.behaviors.npc.SimpleMainBehavior;
import com.jme3.ai.agents.behaviors.npc.steering.CompoundSteeringBehavior;
import com.jme3.ai.agents.behaviors.npc.steering.LeaderFollowingBehavior;
import com.jme3.ai.agents.behaviors.npc.steering.PathFollowBehavior;
import com.jme3.ai.agents.behaviors.npc.steering.SeparationBehavior;
import com.jme3.ai.agents.util.GameEntity;
import com.jme3.app.SimpleApplication;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Node;
import com.jme3.scene.Spatial;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import rottShortcut.data.stops.PathStep;

/**
 * @author Jesús Martín Berlanga
 */
public class SteerBrain extends SteerChangesInformer
{
    public static final float DISTANCE_BETWEEN          = 0.3f;
    public static final float DISTANTCE_SQ_TRHESHOLD    = 0.11f;
    
    public static final float AG_MOVE_SPEED = 0.75f;
    public static final float AG_ROT_SPEED = 5f;
    public static final float AG_MASS = 1f;
    public static final float AG_MAX_FORCE = 0.1f;
    public static final float SP_RADIOUS = 0.25f;
    
    public static final float SEP_MIN_DISTANCE = DISTANTCE_SQ_TRHESHOLD;
    
    public static final float PATH_RADIUS = 1f;
    public static final float COHESION_STRENGTH = 0.25f;
    
    public static final float WAIT_TO_RESET_T_SEC = 1.5f;
            
    MonkeyBrainsAppStateRootAvaliable brainsAppState;
    private SimpleApplication app;
    private Spatial stops[];
    private Node root;
    
    private Agent trainHead;
    private Agent trainCabs[];
    
    private final Vector3f originalHeadPos;
    private final Quaternion originalHeadRot;
    private final Vector3f originalCabPos[];
    private final Quaternion originalCabRot[];
    
    private boolean thereIsReverse = false;
            
    public SteerBrain(
            SimpleApplication app,
            Node root,
            Spatial stops[],
            Spatial trainHead,
            Spatial trainCabs[]
            )
    {
        this.app        = app;
        this.root       = root;
        this.stops      = stops;
        
        brainsAppState = MonkeyBrainsAppStateRootAvaliable.getInstance(); 
        
        this.trainHead = new Agent("Train Head", trainHead);
        setStats(this.trainHead, 
            AG_MOVE_SPEED, AG_ROT_SPEED, AG_MASS, AG_MAX_FORCE);
        this.trainHead.setRadius(SP_RADIOUS);
        this.originalHeadPos = new Vector3f(trainHead.getLocalTranslation());
        this.originalHeadRot = new Quaternion(trainHead.getLocalRotation());
        
        this.trainCabs = new Agent[trainCabs.length];
        this.originalCabPos = new Vector3f[trainCabs.length];
        this.originalCabRot = new Quaternion[trainCabs.length];
        for(int i = 0; i < trainCabs.length; i++) {
            this.trainCabs[i] = new Agent("Train Cab " + i, trainCabs[i]);
            this.originalCabPos[i] = 
                    new Vector3f(trainCabs[i].getLocalTranslation());
            this.originalCabRot[i] = 
                    new Quaternion(trainCabs[i].getLocalRotation());
            setStats(this.trainCabs[i], 
                    AG_MOVE_SPEED, AG_ROT_SPEED, AG_MASS, AG_MAX_FORCE);
            this.trainCabs[i].setRadius(SP_RADIOUS);
        }    
    }
    
    //Behaviours needed
    private SimpleMainBehavior headFollowPath;
    private SimpleMainBehavior cabsFollowHead[] = null;
    public void doBrainedTravel(List<PathStep> path, float tpf)
    {        
        List<PathStep> pathC = new LinkedList<PathStep>(path);
        
        //Clean previous uses
        brainsAppState.cleanup();
        accTime = 0; 
        
        //defining rootNode for brainsAppState processing
        brainsAppState.setApp(app);
        brainsAppState.setRootNode(root);
        brainsAppState.setGameControl(new SteerGameControl());
        
        //Remove repetitions in path
        Iterator<PathStep> pathIterator = pathC.iterator();
        PathStep previous = null;
	while (pathIterator.hasNext()) 
        {
            PathStep next = pathIterator.next();
            if(previous != null && previous.getStop().equals(next.getStop()))
                pathIterator.remove();
            else
                previous = next;
	}
        
        //Create the Path
        ArrayList<Vector3f> orderedPointsList = new ArrayList<Vector3f>();
        for(PathStep step : pathC)
            orderedPointsList.add(
                    stops[step.getStop().ordinal()].getLocalTranslation());
        headFollowPath = new SimpleMainBehavior(trainHead);
        headFollowPath.addBehavior(new PathFollowBehavior(
                trainHead, orderedPointsList, PATH_RADIUS, COHESION_STRENGTH));
        trainHead.setMainBehavior(headFollowPath);
        brainsAppState.addAgent(this.trainHead);
        
        //Generate start position looking direction
        Vector3f startLookingDirection = orderedPointsList.get(1).
                subtract(orderedPointsList.get(0)).normalize();
        
        //Place the head in the correctplace
        brainsAppState.getGameControl().spawn(trainHead, orderedPointsList.get(0));
        rotateAgentTo(trainHead, startLookingDirection, tpf);
        
        //Check If there is reverse travelling in the path
        thereIsReverse = false;
        int repetitions[] = new int[stops.length];
        for(PathStep s: pathC) 
        {
            if(++repetitions[s.getStop().ordinal()] > 1)
            {
                thereIsReverse = true;
                break;
            }
        }
        
        cabsFollowHead = new SimpleMainBehavior[trainCabs.length];
        for (int i = 0; i < cabsFollowHead.length; i++)
        {
            Agent toFollow = i-1 >= 0 ? 
                    trainCabs[i-1] : trainHead;
            cabsFollowHead[i] = new SimpleMainBehavior(trainCabs[i]);
            
            Vector3f spawnPos;
            
            if(!thereIsReverse)
            {
                CompoundSteeringBehavior compoundCompound 
                        = new CompoundSteeringBehavior(trainCabs[i]);
                CompoundSteeringBehavior compound = 
                        new CompoundSteeringBehavior(trainCabs[i]);
                    LeaderFollowingBehavior lf 
                            = new LeaderFollowingBehavior(trainCabs[i], toFollow);
                    lf.setupStrengthControl(0.25f);
                    compound.addSteerBehavior(lf);
                    compound.addSteerBehavior(new PathFollowBehavior(
                        trainCabs[i], orderedPointsList,
                            PATH_RADIUS, COHESION_STRENGTH));

                compound.setupStrengthControl(1f);
                compoundCompound.addSteerBehavior(compound);
                                
                List<GameEntity> toSeparate = new ArrayList<GameEntity>();
                toSeparate.add(toFollow);
                float sepDistance = toFollow.equals(trainHead) ? 
                                        SEP_MIN_DISTANCE*2f:
                                        SEP_MIN_DISTANCE;
                SeparationBehavior sep = new SeparationBehavior(trainCabs[i],
                        toSeparate, sepDistance);
                sep.setupStrengthControl(0.75f);
                compoundCompound.addSteerBehavior(sep);
                
                cabsFollowHead[i].addBehavior(compoundCompound);
                
                //Place the cab in the correct place
                spawnPos = orderedPointsList.get(0).add(
                        startLookingDirection.negate().mult(
                            (i+1)*DISTANCE_BETWEEN)
                        );
                rotateAgentTo(trainCabs[i], startLookingDirection, tpf);
            }
            else
            {
                //Place the cabs in the correct place
                spawnPos = originalCabPos[i];
                trainCabs[i].setLocalRotation(originalCabRot[i]);
            }
           
            trainCabs[i].setMainBehavior(cabsFollowHead[i]);
            brainsAppState.addAgent(this.trainCabs[i]);
            brainsAppState.getGameControl().spawn(trainCabs[i], spawnPos);
        }
        
        //Prepare variables to inform changes
        currentPassed = new LinkedList<PathStep>();
        notPassed = pathC;
        insideThreshold = false;
        
        brainsAppState.start();
    }
    
    private List<PathStep> currentPassed;
    private List<PathStep> notPassed;
    private boolean insideThreshold;
    float accTime = 0;
    public void checkChanges(float tpf)
    {
        brainsAppState.update(tpf);
        if(notPassed.size() > 0)
        {
            accTime = 0; 
            //Check for end
            float hDistToObj = trainHead.distanceSquaredFromPosition(stops[
                    notPassed.get(notPassed.size()-1).getStop().ordinal()].
                    getLocalTranslation());

            //Not finished yet
            if(hDistToObj > DISTANTCE_SQ_TRHESHOLD && notPassed.size() >= 2)
            {
                Agent toTestAgent = this.thereIsReverse ? 
                        trainHead :
                        trainCabs[trainCabs.length-1];
                float distance = toTestAgent.distanceSquaredFromPosition(stops[
                        notPassed.get(1).getStop().ordinal()].getLocalTranslation());

                if(!insideThreshold && distance < DISTANTCE_SQ_TRHESHOLD)
                {
                    insideThreshold = true;
                }
                else if(insideThreshold && distance > DISTANTCE_SQ_TRHESHOLD)
                {
                    insideThreshold = false;

                    currentPassed.add(notPassed.get(0));
                    notPassed.remove(0);

                    informPathStepPassedThrough(currentPassed);
                }
            }
            //Travel finished
            else
            {
                currentPassed.addAll(notPassed);
                notPassed.clear();
                brainsAppState.stop();

                informPathStepPassedThrough(currentPassed);
            }
        } 
        else if(accTime < WAIT_TO_RESET_T_SEC)
            accTime += tpf;
        else if(accTime >= WAIT_TO_RESET_T_SEC && accTime != Float.MAX_VALUE)
        {
            resetTrainPositions();
            accTime = Float.MAX_VALUE;
        }
    }
    
    public void stopAndReset()
    {
        headFollowPath.setEnabled(false);
        for(Behavior b : cabsFollowHead)
            b.setEnabled(false);
        
        brainsAppState.stop();

        resetTrainPositions();
    }
    
    private void resetTrainPositions()
    {
        this.trainHead.setLocalTranslation(originalHeadPos);
        this.trainHead.setLocalRotation(originalHeadRot);
        
        for(int i = 0; i < this.trainCabs.length; i++)
        {
            this.trainCabs[i].setLocalTranslation(originalCabPos[i]);
            this.trainCabs[i].setLocalRotation(originalCabRot[i]);
        }      
    }
    
    private void setStats(
            Agent myAgent, 
            float moveSpeed, 
            float rotationSpeed, 
            float mass, 
            float maxForce
            ) 
    {
        myAgent.setMoveSpeed(moveSpeed);
        myAgent.setMaxMoveSpeed(moveSpeed);
        myAgent.setRotationSpeed(rotationSpeed);
        myAgent.setMass(mass);
        myAgent.setMaxForce(maxForce);
    }
    
    private void rotateAgentTo(Agent agent, Vector3f lookDirection, float tpf)
    {
        Quaternion q = new Quaternion();
        q.lookAt(lookDirection, new Vector3f(0, 1, 0));
        agent.getLocalRotation().slerp(q, agent.getRotationSpeed() * tpf);
    }
}