package robocup.lab;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;

import org.apache.log4j.Logger;

import plplan.javaapi.EnumAlgorithm;
import plplan.javaapi.PLPlan;
import plplan.javaapi.PlPlanException;

/**
 * The Class Brain.
 */
public class Brain {
	
    /**
     * The log.
     */
    private static Logger 					log = Logger.getLogger(Brain.class);
	
	/**
	 * The memory.
	 */
	private static Set<Literal>				memory = new HashSet<Literal>();
	
    /**
     * The sensors.
     */
    private static Set<Literal> 			sensors = new HashSet<Literal>();
    
    /**
     * The targets.
     */
    private static PriorityQueue<Literal> 	targets = new PriorityQueue<Literal>();
    
    
    /**
     * The planner.
     */
    private static PLPlan 					planner = new PLPlan();
    
    /**
     * The plan.
     */
    private static List<String>				plan = new ArrayList<String>();
    
    /**
     * Instantiates a new brain.
     */
    public Brain() {
    	initPlanner();
    }
    
    /**
     * Inits the planner.
     */
    private void initPlanner() {

    	
    	plan.clear();
    	
    	planner = new PLPlan();
    	
    	planner.setAlgorithm(EnumAlgorithm.GRAPHPLAN);
    	
    	List<String> precond = new ArrayList<String>();
    	List<String> neg = new ArrayList<String>();
        List<String> pos = new ArrayList<String>();
        
        //action find-ball
        
    	precond = new ArrayList<String>();
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("can-see-ball");
        planner.addOperator("find-ball", precond, neg, pos);
        
        //action find-area
        
    	precond = new ArrayList<String>();
    	precond.add("have-ball");
    	precond.add("clear-field");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("can-see-area");
        planner.addOperator("find-area", precond, neg, pos);
        
        //action find-goal
        
    	precond = new ArrayList<String>();
    	precond.add("have-ball");
    	precond.add("clear-field");
    	precond.add("can-see-area");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("can-see-goal");
        planner.addOperator("find-goal", precond, neg, pos);
        
        //action run-to-ball
        
    	precond = new ArrayList<String>();
        precond.add("can-see-ball");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("have-ball");
        planner.addOperator("run-to-ball", precond, neg, pos);
        
        
        //action run-to-goal
        
    	precond = new ArrayList<String>();
        precond.add("have-ball");
    	precond.add("clear-field");
    	precond.add("can-see-goal");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("can-score");
        planner.addOperator("run-to-goal", precond, neg, pos);
        
        //action dribble
        
    	precond = new ArrayList<String>();
    	precond.add("have-ball");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("clear-field");
        planner.addOperator("dribble", precond, neg, pos);
        
    	//action score-goal
    	
    	precond = new ArrayList<String>();
        precond.add("have-ball");
        precond.add("can-score");
        neg = new ArrayList<String>();
        pos = new ArrayList<String>();
        pos.add("ball-in-net");
        planner.addOperator("score-goal", precond, neg, pos);
    }

    /**
     * Replan. Run the planner in order to find a better planning.
     */
    @SuppressWarnings("unchecked")
	private void replan() {
    	
    	log.error("MEMORY " + memory);
    	
    	initPlanner();
    	
        log.error("STATE " + sensors);
    	// add brain facts to the planner initial state
    	if (!sensors.isEmpty()) {
            Iterator<Literal> iter = sensors.iterator();
            while (iter.hasNext()) {
            	String fact = iter.next().getName();
            	planner.addFact(fact);
            	log.error("ADDED " + fact);
            }
        }
    	
    	log.error("GOALS " + targets);
        // find out the best local target
        if (!targets.isEmpty()) {
        	Literal goal = targets.peek();
        	planner.addGoalFact(goal.getName());
        	log.error("GOAL " + goal.getName());
        }
        
        List result = new ArrayList();
        
        // try running the planner
        try {
			result = planner.findPlan();
		} catch (PlPlanException e) {
		}	
		
		while (!result.isEmpty()) {
    		List nextActionList = (List) result.remove(0);
    		String nextAction = (String) nextActionList.remove(0);
    		plan.add(nextAction);
		}
			
    	log.error("PLAN " + plan);
    	log.error("---------------------------------------------");
    	
    	
    }
    
    /**
     * Adds a {@link Literal} to the sensors set. 
     *
     * @param l the literal
     */
    public void addSense(Literal l) {
    	if (!sensors.contains(l)) {
    		sensors.add(l);
    		if (!memory.contains(l)) {
    			// nuova percezione
    			Literal localGoal = evaluate(l, "add");
    			if (localGoal != null && !targets.contains(localGoal)) {
    				targets.offer(localGoal);
    			}
    		}
    	}
    }
    
    /**
     * Removes a {@link Literal} from the sensors set.
     *
     * @param l the literal
     */
    public void removeSense(Literal l) {
    	if (sensors.contains(l)) {
    		sensors.remove(l);
    		if (memory.contains(l)) {
    			// nuova percezione
    			Literal localGoal = evaluate(l, "remove");
    			if (localGoal != null && !targets.contains(localGoal)) {
    				targets.offer(localGoal);
    			}
    		}
    	}
    }
    
    /**
     * Evaluate the literal, in order to find a corresponding local goal.
     *
     * @param l the literal
     * @param mode the mode (add, remove)
     * @return the evaluated literal
     */
    private Literal evaluate(Literal l, String mode) {
    	
    	if (mode.equals("add")) {
    	
    		if (l.getName().equals("ball-in-net"))
    			return null;
    		
    		else if (l.getName().equals("can-score"))
    			return new Literal("ball-in-net");
    		
    		else if (l.getName().equals("can-see-goal"))
    			return new Literal("can-score");
    		
    		else if (l.getName().equals("can-see-area"))
    			return new Literal("can-see-goal");
    		
    		else if (l.getName().equals("have-ball"))
    			return new Literal("can-see-area");
    		
    		else if (l.getName().equals("can-see-ball"))
    			return new Literal("have-ball");
    		
    		else if (l.getName().equals("clear-field"))
    			return new Literal("can-see-ball");
    		
    	} else if (mode.equals("remove")) {
    		
    		if (l.getName().equals("clear-field"))
    			return new Literal("clear-field");
    		
    	}
		
		return null;
    }
    
    /**
     * Reset sensors.
     */
    public void resetSensors() {
    	memory.clear();
    	memory.addAll(sensors);
    	sensors.clear();
    }

    
    /**
     * Read the current action in the plan.
     *
     * @return the action
     */
    public String readAction() {
    	if (!plan.isEmpty())
    		return plan.get(0);
    	return null;
    }
    
    /**
     * Consume the current action in the plan.
     */
    public void consumeAction() {
    	if (!plan.isEmpty())
    		plan.remove(0);
    }
    
    /**
     * Restart all the brain instance.
     */
    public void restart() {
    	memory.clear();
    	sensors.clear();
    	targets.clear();
    	initPlanner();
    }
    
    /**
     * Try to replan.
     */
    public void tryReplan() {
    	if (memory.size() != sensors.size()) {
    		replan();
    	} else if (!memory.containsAll(sensors)) {
    		replan();
    	}
    }
}
