/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package cz.cuni.amis.pogamut.base.agent.navigation;

import cz.cuni.amis.pogamut.base.exceptions.PogamutException;
import java.util.Iterator;
import java.util.List;
import java.util.ListIterator;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 * Moves along the path using this algorithm:
 * <ol>
 *  <li>Move directly to first checkpoint in the list</li>
 *  <li>Remove the first checkpoint</li>
 * </ol>
 * @author ik
 */
public abstract class SimplePathExecutor<LOC, PATH_ELEMENT> extends AbstractPathExecutor<LOC> {

    /**
     * Planner used to compute the desired path.
     */
    protected PathPlanner planner = null;
    /**
     * Listener on path planner events.
     */
    private PathPlannerListener listener = null;
    /**
     * Path the agent is following.
     */
    private List<PATH_ELEMENT> actualPath = null;
    /**
     * Iterator used when following elements of the path.
     */
    private ListIterator<PATH_ELEMENT> pathIterator = null;

    /**
     * Path element the agent is currently heading to.
     */
    private PATH_ELEMENT nextPathElement = null;

    public SimplePathExecutor(PathPlanner planner) {
        this.planner = planner;
    }

    @Override
    public void goTo(LOC location) {
        // listen when the final path will be computed
        planner.addPathListener(listener = new PathPlannerListener<PATH_ELEMENT>() {

            @Override
            public void pathEvent(List<PATH_ELEMENT> path) {
                pathReceived(path);
            }
        });
        try {
            // TODO ask the path planner for aproximate path
            List<PATH_ELEMENT> path = planner.computePath(getAgentLocation(), location);
            pathReceived(path);
        } catch (PathNotConstructable ex) {
            fireEvent(PathEventType.TARGET_UNREACHABLE);
        }
    }

    /**
     * @return Next element in the path.
     */
    protected PATH_ELEMENT getNextPathElement() {
        return nextPathElement;
    }

    /**
     *
     * @return Iterator beginning at the first unreached path element.
     */
    protected Iterator<PATH_ELEMENT> getPathIterator() {
        return actualPath.listIterator(pathIterator.nextIndex());
    }

    /**
     * Called when the planner computes the final path.
     * @param path the final path
     */
    private void pathReceived(List<PATH_ELEMENT> path) {

        // TODO interrupt curent aproximate path
        actualPath = path;
        if (path != null) {
            pathIterator = path.listIterator();
            moveToNext();
        }
    }

    /**
     * Called when the agent reaches target area of next path element. If the element
     * was the last element then the event TARGET_REACHED is fired.
     * @return next location in the path or null when the path ended.
     */
    protected void moveToNext() {
        try {
            if (pathIterator.hasNext()) {
                // the path has more elements proceed to the next
                nextPathElement = pathIterator.next();
                issueMotoricCommands();
            } else {
                // the path has ended
                nextPathElement = null;
                fireEvent(PathEventType.TARGET_REACHED);
            }
        } catch (PogamutException ex) {
            // TODO
            fireEvent(PathEventType.FAILURE);
        }
    }

    /**
     * Returns actual location of the agent. Often used as the starting location for PathPlanner.
     * @return
     */
    protected abstract LOC getAgentLocation();

    /**
     *
     * @return true if the executor is moving along path, false otherwise.
     */
    public boolean isMoving() {
        return nextPathElement != null;
    }

    /**
     * Method called every time the
     * TODO
     */
    protected abstract void issueMotoricCommands() throws PogamutException;
}
