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

import cz.cuni.amis.pogamut.base.agent.navigation.PathPlanner;
import cz.cuni.amis.pogamut.base.agent.navigation.SimplePathExecutor;
import cz.cuni.amis.pogamut.base.agent.worldview.IWorldView;
import cz.cuni.amis.pogamut.base.agent.worldview.WorldEventListener;
import cz.cuni.amis.pogamut.base.agent.worldview.WorldObjectEventListener;
import cz.cuni.amis.pogamut.base.agent.worldview.objects.IWorldObject;
import cz.cuni.amis.pogamut.base.communication.commands.CommandSerializer;
import cz.cuni.amis.pogamut.base.communication.commands.IAct;
import cz.cuni.amis.pogamut.base.exceptions.PogamutException;
import cz.cuni.amis.pogamut.base.exceptions.PogamutRuntimeException;
import cz.cuni.amis.pogamut.base3d.worldview.objects.ILocated;
import cz.cuni.amis.pogamut.base3d.worldview.objects.Location;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.Move;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
import java.util.Collection;
import java.util.Iterator;

/**
 *
 * @author ik
 */
public class UTPathExecutor extends SimplePathExecutor<ILocated, ILocated> {

    /**
     * Distance to the nav point when the bot is considered to reach the nav point.
     * TODO load from pogamut platform
     */
    protected final double TARGET_AREA_RADIUS = 120;
    IAct commandSerializer = null;
    IWorldView worldView = null;
    WorldObjectEventListener<Self> movementListener = null;

    public UTPathExecutor(IWorldView worldView, IAct commandSerializer, PathPlanner planner) {
        super(planner);
        this.commandSerializer = commandSerializer;
        this.worldView = worldView;

        // add a listener on wv that will be called each time the bot's location has changed
        worldView.addUpdatedListener(Self.class, movementListener = new WorldObjectEventListener<Self>() {

            double lastDist = Double.MAX_VALUE;

            public void notify(Self event) {
                if (isMoving() && event.getLocation().getDistance(getNextPathElement().getLocation()) < TARGET_AREA_RADIUS) {
                    // bot has reached the target location, move to next element in the path
                    moveToNext();
                }
            }
        });
    }

    @Override
    protected ILocated getAgentLocation() {
        Collection<Self> slfCol = worldView.getAll(Self.class).values();
        if (slfCol.size() == 1) {
            return slfCol.iterator().next().getLocation();
        } else {
            throw new PogamutRuntimeException("None or more than one Self messages present.", this);
        }
    }

    @Override
    protected void issueMotoricCommands() throws PogamutException {
        Iterator<ILocated> it = getPathIterator();
        Location l1 = getNextPathElement().getLocation();//it.next().getLocation();
        if (it.hasNext()) {
            // there are at least two points in the path
            Location l2 = it.next().getLocation();
            commandSerializer.act(new Move().setFirstLocation(l1).setSecondLocation(l2));
        } else {
            // there is only one point to go to
            commandSerializer.act(new Move().setFirstLocation(l1));
        }
    }
}
