package org.opentrafficsim.road.gtu.lane.tactical;

import java.util.Collections;
import java.util.Iterator;
import java.util.Set;

import org.djunits.value.vdouble.scalar.Length;
import org.djutils.immutablecollections.ImmutableSet;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.base.parameters.ParameterTypes;
import org.opentrafficsim.core.gtu.GtuException;
import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
import org.opentrafficsim.core.network.Connector;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.LinkInterface;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.NodeInterface;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.gtu.lane.InternalLaneBasedGtu;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
import org.opentrafficsim.road.network.lane.Lane;

/**
 * <p>
 * Copyright (c) 2013-2021 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
 * </p>
 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
 * initial version May 27, 2016 <br>
 * @author <a href="https://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 * @author <a href="https://www.transport.citg.tudelft.nl">Wouter Schakel</a>
 */
public interface LaneBasedTacticalPlanner extends TacticalPlanner<InternalLaneBasedGtu, LanePerception>
{
    /**
     * Returns the car-following model.
     * @return car following model
     */
    CarFollowingModel getCarFollowingModel();

    /**
     * Selects a lane from a possible set. This set contains all viable lanes in to which a lanes splits.
     * @param from Lane; lane we come from
     * @param lanes Set&lt;Lane&gt;; set of lanes possible
     * @return Lane; preferred lane
     * @throws ParameterException in case of a missing parameter
     */
    default Lane chooseLaneAtSplit(final Lane from, final Set<Lane> lanes) throws ParameterException
    {
        if (getEgoGtu().getOperationalPlan() instanceof LaneBasedOperationalPlan
                && ((LaneBasedOperationalPlan) getEgoGtu().getOperationalPlan()).isDeviative())
        {
            // take the lane adjacent to lane we are registered on, if any
            LateralDirectionality forceSide = LateralDirectionality.NONE;
            try
            {
                ImmutableSet<Lane> leftLanes =
                        from.accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, getEgoGtu().getGTUType());
                if (!Collections.disjoint(getEgoGtu().positions(getEgoGtu().getReference()).keySet(), leftLanes.toSet()))
                {
                    forceSide = LateralDirectionality.LEFT;
                }
                else
                {
                    ImmutableSet<Lane> rightLanes =
                            from.accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, getEgoGtu().getGTUType());
                    if (!Collections.disjoint(getEgoGtu().positions(getEgoGtu().getReference()).keySet(), rightLanes.toSet()))
                    {
                        forceSide = LateralDirectionality.RIGHT;
                    }
                }
            }
            catch (GtuException exception)
            {
                throw new RuntimeException("Exception obtaining reference position.", exception);
            }
            if (!forceSide.isNone())
            {
                if (lanes.isEmpty())
                {
                    // A sink should delete the GTU, or a lane change should end, before reaching the end of the lane
                    return null;
                }
                else
                {
                    Iterator<Lane> iter = lanes.iterator();
                    Lane next = iter.next();
                    while (iter.hasNext())
                    {
                        Lane candidate = iter.next();
                        next = LaneBasedTacticalPlanner.mostOnSide(next, candidate, forceSide);
                    }
                    return next;
                }
            }
        }
        Route route = getEgoGtu().getStrategicalPlanner().getRoute();
        if (route == null)
        {
            // select right-most lane
            Lane rightMost = null;
            for (Lane lane : lanes)
            {
                rightMost = rightMost == null ? lane : mostOnSide(rightMost, lane, LateralDirectionality.RIGHT);
            }
            return rightMost;
        }
        Length maxDistance = Length.NEGATIVE_INFINITY;
        Lane best = null;
        for (Lane lane : lanes)
        {
            Lane next = getEgoGtu().getNextLane(lane);
            if (next != null)
            {
                Length okDistance = okDistance(next, lane.getLength(), route,
                        getEgoGtu().getParameters().getParameter(ParameterTypes.PERCEPTION));
                if (maxDistance.eq(okDistance))
                {
                    best = mostOnSide(best, lane, LateralDirectionality.RIGHT);
                }
                else if (okDistance.gt(maxDistance))
                {
                    maxDistance = okDistance;
                    best = lane;
                }
            }
        }
        return best;
    }

    /**
     * Helper method for default chooseLaneAtSplit implementation that returns the distance from this lane onwards where the
     * route can be followed.
     * @param lane Lane; lane
     * @param distance Length; distance so far
     * @param route Route; route
     * @param maxDistance Length; max search distance
     * @return Length; distance from this lane onwards where the route can be followed
     */
    // TODO private when we use java 9
    default Length okDistance(final Lane lane, final Length distance, final Route route, final Length maxDistance)
    {
        if (distance.gt(maxDistance))
        {
            return maxDistance;
        }
        Lane next = getEgoGtu().getNextLane(lane);
        if (next == null)
        {
            NodeInterface endNode = lane.getParentLink().getEndNode();
            Set<LinkInterface> links = endNode.getOutgoingLinks().toSet(); // XXX: Is this correct?
            links.remove(lane.getParentLink());
            if (route.contains(endNode) && (links.isEmpty() || links.iterator().next() instanceof Connector))
            {
                // dead-end link, must be destination
                return maxDistance;
            }
            // there is no next lane on the route, return the distance to the end of this lane
            return distance.plus(lane.getLength());
        }
        return okDistance(next, distance.plus(lane.getLength()), route, maxDistance);
    }

    /**
     * Returns the right-most of two lanes.
     * @param lane1 Lane; lane 1
     * @param lane2 Lane; lane 2
     * @param lat LateralDirectionality; lateral side
     * @return Lane; right-most of two lanes
     */
    static Lane mostOnSide(final Lane lane1, final Lane lane2, final LateralDirectionality lat)
    {
        Length offset1 = lane1.getDesignLineOffsetAtBegin().plus(lane1.getDesignLineOffsetAtEnd());
        Length offset2 = lane2.getDesignLineOffsetAtBegin().plus(lane2.getDesignLineOffsetAtEnd());
        if (lat.isLeft())
        {
            return offset1.gt(offset2) ? lane1 : lane2;
        }
        return offset1.gt(offset2) ? lane2 : lane1;
    }

    /**
     * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
     * @param maxHeadway Length; the maximum length for which lanes should be returned
     * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow,
     *         and the path to follow when staying on the same lane.
     * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
     * @throws NetworkException when the strategic planner is not able to return a next node in the route
     */
    LanePathInfo buildLanePathInfo(Length maxHeadway) throws GtuException, NetworkException;

    /**
     * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
     * @param maxHeadway Length; the maximum length for which lanes should be returned
     * @param startLane Lane; the lane in which the path starts
     * @param position Length; the position on the start lane
     * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow,
     *         and the path to follow when staying on the same lane.
     * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
     * @throws NetworkException when the strategic planner is not able to return a next node in the route
     * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
     * @throws NetworkException when the strategic planner is not able to return a next node in the route
     */
    LanePathInfo buildLanePathInfo(Length maxHeadway, Lane startLane, Length position) throws GtuException, NetworkException;

    /**
     * Calculate the next location where the network splits, with a maximum headway relative to the reference point of the GTU.
     * Note: a lane drop is also considered a split (!).
     * @param maxHeadway Length; the maximum length for which lanes should be returned
     * @return NextSplitInfo; an instance that provides the following information for an operational plan: whether the network
     *         splits, the node where it splits, and the current lanes that lead to the right node after the split node.
     * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
     * @throws NetworkException when the strategic planner is not able to return a next node in the route
     */
    NextSplitInfo determineNextSplit(Length maxHeadway) throws GtuException, NetworkException;

}