package org.opentrafficsim.road.gtu.strategical.route;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.Set;

import org.djunits.value.vdouble.scalar.Time;
import org.djutils.exceptions.Throw;
import org.djutils.exceptions.Try;
import org.opentrafficsim.core.gtu.GTUDirectionality;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.LinkDirection;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.route.CompleteRoute;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlanner;
import org.opentrafficsim.road.gtu.strategical.AbstractLaneBasedStrategicalPlanner;
import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
import org.opentrafficsim.road.network.lane.CrossSectionElement;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.DirectedLanePosition;
import org.opentrafficsim.road.network.lane.Lane;

/**
 * Strategical planner, route-based, with personal driving characteristics, which contain settings for the tactical planner. The
 * tactical planner will only consult the route when the GTU has multiple possibilities on a node, so the route does not have to
 * be complete. As long as all 'splitting' nodes are part of the route and have a valid successor node (connected by a Link),
 * the strategical planner is able to make a plan.
 * <p>
 * Copyright (c) 2013-2022 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 Nov 26, 2015 <br>
 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public class LaneBasedStrategicalRoutePlanner extends AbstractLaneBasedStrategicalPlanner implements
    LaneBasedStrategicalPlanner, Serializable
{
    /** */
    private static final long serialVersionUID = 20150724L;

    /** The route to drive. */
    private Route route;

    /** Origin node. */
    private final Node origin;

    /** Destination node. */
    private final Node destination;

    /** The fixed tactical planner to use for the GTU. */
    private final LaneBasedTacticalPlanner fixedTacticalPlanner;

    /** Route supplier. */
    private final RouteGeneratorOD routeGenerator;

    /**
     * Constructor for a strategical planner without route. This can only be used if the network does not have splits, or split
     * fractions are used.
     * @param fixedTacticalPlanner LaneBasedTacticalPlanner; the tactical planner to use for the GTU
     * @param gtu LaneBasedGTU; GTU
     * @throws GTUException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGTU gtu)
            throws GTUException
    {
        this(fixedTacticalPlanner, null, gtu, null, null, RouteGeneratorOD.NULL);
    }

    /**
     * Constructor for a strategical planner with route.
     * @param fixedTacticalPlanner LaneBasedTacticalPlanner; the tactical planner to use for the GTU
     * @param route Route; the route to drive
     * @param gtu LaneBasedGTU; GTU
     * @param origin Node; origin node
     * @param destination Node; destination node
     * @throws GTUException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
            final LaneBasedGTU gtu, final Node origin, final Node destination) throws GTUException
    {
        this(fixedTacticalPlanner, route, gtu, origin, destination, RouteGeneratorOD.NULL);
    }

    /**
     * Constructor for a strategical planner with route generator.
     * @param fixedTacticalPlanner LaneBasedTacticalPlanner; the tactical planner to use for the GTU
     * @param gtu LaneBasedGTU; GTU
     * @param origin Node; origin node
     * @param destination Node; destination node
     * @param routeGenerator RouteGeneratorOD; route generator
     * @throws GTUException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGTU gtu,
            final Node origin, final Node destination, final RouteGeneratorOD routeGenerator) throws GTUException
    {
        this(fixedTacticalPlanner, null, gtu, origin, destination, routeGenerator);
    }

    /**
     * Constructor for a strategical planner with route. If the route is {@code null}, a shortest path to the destination is
     * derived.
     * @param fixedTacticalPlanner LaneBasedTacticalPlanner; the tactical planner to use for the GTU
     * @param route Route; the route to drive
     * @param gtu LaneBasedGTU; GTU
     * @param origin Node; origin node
     * @param destination Node; destination node
     * @param routeGenerator RouteGeneratorOD; route generator
     * @throws GTUException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
            final LaneBasedGTU gtu, final Node origin, final Node destination, final RouteGeneratorOD routeGenerator)
            throws GTUException
    {
        super(gtu);
        this.route = route;
        this.origin = origin;
        this.destination = destination;
        this.fixedTacticalPlanner = fixedTacticalPlanner;
        Throw.when(fixedTacticalPlanner == null, GTUException.class,
            "Fixed Tactical Planner for a Strategical planner is null");
        this.routeGenerator = routeGenerator;
    }

    /** {@inheritDoc} */
    @Override
    public final LaneBasedTacticalPlanner getTacticalPlanner()
    {
        return this.fixedTacticalPlanner;
    }

    /** {@inheritDoc} */
    @Override
    public LaneBasedTacticalPlanner getTacticalPlanner(final Time time)
    {
        return this.fixedTacticalPlanner; // fixed anyway
    }

    /** {@inheritDoc} */
    @Override
    public final Node nextNode(final Link link, final GTUDirectionality direction, final GTUType gtuType)
            throws NetworkException
    {
        assureRoute(gtuType);
        LinkDirection linkDirection = nextLinkDirection(link, direction, gtuType);
        return linkDirection.getNodeTo();
    }

    /** {@inheritDoc} */
    @Override
    public final LinkDirection nextLinkDirection(final Link link, final GTUDirectionality direction, final GTUType gtuType)
            throws NetworkException
    {
        assureRoute(gtuType);
        Node nextNode = direction.equals(GTUDirectionality.DIR_PLUS) ? link.getEndNode() : link.getStartNode();
        if ((null != this.route) && (!this.route.contains(nextNode)))
        {
            link.getSimulator().getLogger().always().warn("nextNode {} is not in route {}", nextNode, this.route);
            Node prevNode = direction.equals(GTUDirectionality.DIR_PLUS) ? link.getStartNode() : link.getEndNode();
            link.getSimulator().getLogger().always().warn("   other node of link is {}", prevNode);
            int index = 0;
            for (Node node : this.route.getNodes())
            {
                link.getSimulator().getLogger().always().warn("{} {}{}", index, node.equals(prevNode) ? "--->" : "    ", node);
                index++;
            }
        }
        return nextLinkDirection(nextNode, link, gtuType);
    }

    /** {@inheritDoc} */
    @Override
    public final Node nextNode(final Node node, final Link previousLink, final GTUType gtuType) throws NetworkException
    {
        assureRoute(gtuType);
        LinkDirection linkDirection = nextLinkDirection(node, previousLink, gtuType);
        return linkDirection.getNodeTo();
    }

    /** {@inheritDoc} */
    @Override
    public final LinkDirection nextLinkDirection(final Node node, final Link previousLink, final GTUType gtuType)
            throws NetworkException
    {
        assureRoute(gtuType);

        // if there is no split, don't ask the route
        if (node.getLinks().size() == 1 && previousLink != null)
        {
            // end node
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link, but node " + node
                + " has no successors");
        }
        if (node.getLinks().size() == 1 && previousLink == null)
        {
            // start node
            Link link = node.getLinks().iterator().next();
            return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
                : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
        }
        if (node.getLinks().size() == 2)
        {
            for (Link link : node.getLinks())
            {
                if (!link.equals(previousLink))
                {
                    return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
                        : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
                }
            }
        }

        // if we only have one way to go, don't bother about the route yet
        Set<Link> links = node.getLinks().toSet();
        for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
        {
            Link link = linkIterator.next();
            if (link.equals(previousLink))
            {
                // No u-turn...
                linkIterator.remove();
            }
            else
            {
                // does the directionality of the link forbid us to go in?
                if ((link.getStartNode().equals(node) && !link.getDirectionality(gtuType).isForwardOrBoth()) || (link
                    .getEndNode().equals(node) && !link.getDirectionality(gtuType).isBackwardOrBoth()))
                {
                    linkIterator.remove();
                }
                else
                {
                    // are there no lanes from the node into this link in the outgoing direction?
                    boolean out = false;
                    CrossSectionLink csLink = (CrossSectionLink) link;
                    // TODO: Is there a reason not to iterate over csLink.getLanes()?
                    for (CrossSectionElement cse : csLink.getCrossSectionElementList())
                    {
                        if (cse instanceof Lane)
                        {
                            Lane lane = (Lane) cse;
                            if ((link.getStartNode().equals(node) && lane.getLaneType().isCompatible(gtuType,
                                GTUDirectionality.DIR_PLUS)) || (link.getEndNode().equals(node) && lane.getLaneType()
                                    .isCompatible(gtuType, GTUDirectionality.DIR_MINUS)))
                            {
                                out = true;
                            }
                        }
                    }
                    if (!out)
                    {
                        linkIterator.remove();
                    }
                }
            }
        }

        if (links.size() == 1)
        {
            Link link = links.iterator().next();
            return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
                : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
        }

        // more than 2 links... We have to check the route!
        if (getRoute() == null)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
        }
        int i = this.route.getNodes().indexOf(node);
        if (i == -1)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
                + previousLink + ", but node " + node + " not in route " + this.route);
        }
        if (i == this.route.getNodes().size() - 1)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
                + previousLink + ", but the GTU reached the last node for route " + this.route);
        }
        Node nextNode = this.route.getNode(i + 1);
        LinkDirection result = null;
        for (Link link : links)
        {
            // TODO this takes the first in the set of links that connects the correct nodes; does not handle parallel links
            // consistently
            LinkDirection ld = null;
            if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
            {
                ld = new LinkDirection(link, GTUDirectionality.DIR_MINUS);
            }
            if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
            {
                ld = new LinkDirection(link, GTUDirectionality.DIR_PLUS);
            }
            if (null != result && null != ld)
            {
                throw new NetworkException("Cannot choose among multiple links from " + node + " to " + nextNode);
            }
            else if (null == result)
            {
                result = ld;
            }
        }
        if (null == result)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
                + previousLink.getId() + ", but no link could be found connecting node " + node + " and node " + nextNode
                + " for route " + this.route);
        }
        return result;
    }

    /** {@inheritDoc} */
    @Override
    public final Route getRoute()
    {
        assureRoute(getGtu().getGTUType());
        if (this.route == null && this.destination != null)
        {
            try
            {
                DirectedLanePosition pos = getGtu().getReferencePosition();
                CrossSectionLink link = pos.getLane().getParentLink();
                Node from = pos.getGtuDirection().isPlus() ? link.getStartNode() : link.getEndNode();
                if (this.routeGenerator != null)
                {
                    this.route = this.routeGenerator.getRoute(from, this.destination, getGtu().getGTUType());
                }
                if (this.route == null)
                {
                    this.route = link.getNetwork().getShortestRouteBetween(getGtu().getGTUType(), from, this.destination);
                }
            }
            catch (GTUException | NetworkException exception)
            {
                throw new RuntimeException("Route could not be determined.", exception);
            }
        }
        return this.route;
    }

    /**
     * Assures a route is available if a route is already present, or a destination and route supplier are provided.
     * @param gtuType GTUType; the type of the GTU for which a route must be assured
     */
    private void assureRoute(final GTUType gtuType)
    {
        if (this.route == null && this.destination != null && !this.routeGenerator.equals(RouteGeneratorOD.NULL))
        {
            DirectedLanePosition ref =
                    Try.assign(() -> getGtu().getReferencePosition(), "Could not retrieve GTU reference position.");
            List<Node> nodes = new ArrayList<>();
            if (this.origin != null)
            {
                nodes.addAll(this.routeGenerator.getRoute(this.origin, ref.getLinkDirection().getNodeFrom(), gtuType)
                    .getNodes());
            }
            else
            {
                nodes.add(ref.getLinkDirection().getNodeFrom());
            }
            Route newRoute = this.routeGenerator.getRoute(ref.getLinkDirection().getNodeTo(), this.destination, gtuType);
            if (null == newRoute)
            {
                System.err.println("this.routeGenerator.getRoute() returned null");
                throw new RuntimeException("getRoute failed");
            }
            List<Node> newNodes = newRoute.getNodes();
            if (newNodes == null)
            {
                System.err.println("Route.getNodes() returned null");
                newRoute.getNodes();
            }
            nodes.addAll(newNodes);
            this.route = Try.assign(() -> new CompleteRoute("Route for " + gtuType + " from " + this.origin + "to "
                + this.destination + " via " + ref.getLinkDirection(), gtuType, nodes), "No route possible over nodes %s",
                nodes);
            // System.out.println("RouteSupplier route for GTU " + getGtu().getId() + ": " + this.route);
        }
    }

    /** {@inheritDoc} */
    @Override
    public final Node getOrigin()
    {
        return this.origin;
    }

    /** {@inheritDoc} */
    @Override
    public final Node getDestination()
    {
        return this.destination;
    }

    /** {@inheritDoc} */
    @Override
    public final String toString()
    {
        return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner="
            + this.fixedTacticalPlanner + "]";
    }

}