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

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

import org.djunits.value.vdouble.scalar.Length;
import org.djutils.draw.DrawRuntimeException;
import org.djutils.draw.line.PolyLine3d;
import org.djutils.immutablecollections.ImmutableSet;
import org.opentrafficsim.base.OTSClassUtil;
import org.opentrafficsim.base.parameters.ParameterTypeClass;
import org.opentrafficsim.base.parameters.ParameterTypeDuration;
import org.opentrafficsim.base.parameters.ParameterTypeLength;
import org.opentrafficsim.base.parameters.ParameterTypes;
import org.opentrafficsim.core.gtu.GtuException;
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.road.gtu.lane.InternalLaneBasedGtu;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
import org.opentrafficsim.road.gtu.lane.tactical.lmrs.LMRS;
import org.opentrafficsim.road.network.lane.CrossSectionElement;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.LanePosition;

/**
 * A lane-based tactical planner generates an operational plan for the lane-based GTU. It can ask the strategic planner for
 * assistance on the route to take when the network splits. This abstract class contains a number of helper methods that make it
 * easy to implement a tactical planner.
 * <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 Nov 25, 2015 <br>
 * @author <a href="https://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public abstract class AbstractLaneBasedTacticalPlanner implements LaneBasedTacticalPlanner, Serializable
{

    /** Tactical planner parameter. */
    public static final ParameterTypeClass<LaneBasedTacticalPlanner> TACTICAL_PLANNER;

    static
    {
        Class<LaneBasedTacticalPlanner> type = LaneBasedTacticalPlanner.class;
        TACTICAL_PLANNER = new ParameterTypeClass<>("tactical planner", "Tactical planner class.",
                OTSClassUtil.getTypedClass(type), LMRS.class);
    }

    /** */
    private static final long serialVersionUID = 20151125L;

    /** Look ahead parameter type. */
    protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;

    /** Time step parameter type. */
    protected static final ParameterTypeDuration DT = ParameterTypes.DT;

    /** The car-following model. */
    private CarFollowingModel carFollowingModel;

    /** The perception. */
    private final LanePerception lanePerception;

    /** GTU. */
    private final InternalLaneBasedGtu gtu;

    /**
     * Instantiates a tactical planner.
     * @param carFollowingModel CarFollowingModel; car-following model
     * @param gtu InternalLaneBasedGtu; GTU
     * @param lanePerception LanePerception; perception
     */
    public AbstractLaneBasedTacticalPlanner(final CarFollowingModel carFollowingModel, final InternalLaneBasedGtu gtu,
            final LanePerception lanePerception)
    {
        setCarFollowingModel(carFollowingModel);
        this.gtu = gtu;
        this.lanePerception = lanePerception;
    }

    /** {@inheritDoc} */
    @Override
    public final InternalLaneBasedGtu getEgoGtu()
    {
        return this.gtu;
    }

    /** {@inheritDoc} */
    @Override
    public LanePathInfo buildLanePathInfo(final Length maxHeadway) throws GtuException, NetworkException
    {
        LanePosition dlp = this.gtu.getReferencePosition();
        return buildLanePathInfo(maxHeadway, dlp.getLane(), dlp.getPosition());
    }

    /** {@inheritDoc} */
    @Override
    public LanePathInfo buildLanePathInfo(final Length maxHeadway, final Lane startLane, final Length position)
            throws GtuException, NetworkException
    {
        List<Lane> laneListForward = new ArrayList<>();
        Lane lane = startLane;
        Length startPosition = position;
        Lane lastLane = lane;
        laneListForward.add(lastLane);
        Length distanceToEndOfLane;
        PolyLine3d path;
        try
        {
            distanceToEndOfLane = lane.getLength().minus(position);
            path = lane.getCenterLine().extract(position.si, lane.getLength().si);
        }
        catch (DrawRuntimeException exception)
        {
            // System.err.println(gtu + ": " + exception.getMessage());
            // System.err.println(lane + ", len=" + lane.getLength());
            // System.err.println(position);
            // throw new GTUException(exception);

            // section on current lane too short, floating point operations cause only a single point at the end of the lane
            path = null;
            distanceToEndOfLane = Length.ZERO;
            laneListForward.clear();
            startPosition = Length.ZERO;
        }

        while (distanceToEndOfLane.lt(maxHeadway))
        {
            ImmutableSet<Lane> lanes = lane.nextLanes(this.gtu.getGTUType());
            if (lanes.size() == 0)
            {
                // Dead end. Return with the list as is.
                return new LanePathInfo(path, laneListForward, startPosition);
            }
            else if (lanes.size() == 1)
            {
                // Ask the strategical planner what the next link should be (if known), because the strategical planner knows
                // best!
                LinkInterface ld = this.gtu.getStrategicalPlanner().nextLink(lane.getParentLink(), this.gtu.getGTUType());
                lane = lanes.iterator().next();
                if (ld != null && !lane.getParentLink().equals(ld))
                {
                    // Lane not on route anymore. return with the list as is.
                    return new LanePathInfo(path, laneListForward, startPosition);
                }
            }
            else
            {
                // Multiple next lanes; ask the strategical planner where to go.
                // Note: this is not necessarily a split; it could e.g. be a bike path on a road
                LinkInterface ld;
                try
                {
                    ld = this.gtu.getStrategicalPlanner().nextLink(lane.getParentLink(), this.gtu.getGTUType());
                }
                catch (NetworkException ne)
                {
                    // no route found.
                    // return the data structure up to this point...
                    return new LanePathInfo(path, laneListForward, startPosition);
                }
                LinkInterface nextLink = ld;
                Lane newLane = null;
                for (Lane nextLane : lanes)
                {
                    if (nextLane.getParentLink().equals(nextLink))
                    {
                        newLane = nextLane;
                        break;
                    }
                }
                if (newLane == null)
                {
                    // we cannot reach the next node on this lane -- we have to make a lane change!
                    // return the data structure up to this point...
                    return new LanePathInfo(path, laneListForward, startPosition);
                }
                // otherwise: continue!
                lane = newLane;
            }

            path = concatenateNull(path, lane.getCenterLine());
            // path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine());
            lastLane = lane;
            laneListForward.add(lastLane);
            distanceToEndOfLane = distanceToEndOfLane.plus(lastLane.getLength());

        }
        return new LanePathInfo(path, laneListForward, startPosition);
    }

    /**
     * Concatenate two paths, where the first may be {@code null}.
     * @param path PolyLine3d; path, may be {@code null}
     * @param centerLine PolyLine3d; center line of lane to add
     * @return concatenated line
     */
    public static PolyLine3d concatenateNull(final PolyLine3d path, final PolyLine3d centerLine)
    {
        if (path == null)
        {
            return centerLine;
        }
        return PolyLine3d.concatenate(Lane.MARGIN.si, path, centerLine);
    }

    /** {@inheritDoc} */
    @Override
    public NextSplitInfo determineNextSplit(final Length maxHeadway) throws GtuException, NetworkException
    {
        NodeInterface nextSplitNode = null;
        Set<Lane> correctCurrentLanes = new LinkedHashSet<>();
        LanePosition dlp = this.gtu.getReferencePosition();
        Lane referenceLane = dlp.getLane();
        double refFrac = dlp.getPosition().si / referenceLane.getLength().si;
        LinkInterface lastLink = referenceLane.getParentLink();
        Length lengthForward;
        Length position = dlp.getPosition();
        NodeInterface lastNode;
        lengthForward = referenceLane.getLength().minus(position);
        lastNode = referenceLane.getParentLink().getEndNode();

        // see if we have a split within maxHeadway distance
        while (lengthForward.lt(maxHeadway) && nextSplitNode == null)
        {
            // calculate the number of "outgoing" links
            Set<LinkInterface> links = lastNode.getOutgoingLinks().toSet(); // safe copy

            // calculate the number of incoming and outgoing lanes on the link
            boolean laneChange = false;
            if (links.size() == 1)
            {
                for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
                {
                    if (cse instanceof Lane)
                    {
                        Lane lane = (Lane) cse;
                        if (lane.nextLanes(this.gtu.getGTUType()).size() == 0)
                        {
                            laneChange = true;
                        }
                    }
                }
            }

            // see if we have a lane drop
            if (laneChange)
            {
                nextSplitNode = lastNode;
                // which lane(s) we are registered on and adjacent lanes link to a lane
                // that does not drop?
                for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
                {
                    if (cse instanceof Lane)
                    {
                        Lane l = (Lane) cse;
                        // if (noLaneDrop(gtu, maxHeadway, l, position, referenceLaneDirectionality))
                        if (noLaneDrop(maxHeadway, l, l.getLength().times(refFrac)))
                        {
                            correctCurrentLanes.add(l);
                        }
                    }
                }
                return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
            }

            // see if we have a split
            if (links.size() > 1)
            {
                nextSplitNode = lastNode;
                LinkInterface ld = this.gtu.getStrategicalPlanner().nextLink(nextSplitNode, lastLink, this.gtu.getGTUType());
                // which lane(s) we are registered on and adjacent lanes link to a lane
                // that is on the route at the next split?
                for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
                {
                    if (cse instanceof Lane)
                    {
                        Lane l = (Lane) cse;
                        // if (connectsToPath(gtu, maxHeadway, l, position, referenceLaneDirectionality, ld.getLink()))
                        if (connectsToPath(maxHeadway, l, l.getLength().times(refFrac), ld))
                        {
                            correctCurrentLanes.add(l);
                        }
                    }
                }
                if (correctCurrentLanes.size() > 0)
                {
                    return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
                }
                // split, but no lane on current link to right direction
                Set<Lane> correctLanes = new LinkedHashSet<>();
                Set<Lane> wrongLanes = new LinkedHashSet<>();
                for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
                {
                    if (cse instanceof Lane)
                    {
                        Lane l = (Lane) cse;
                        if (connectsToPath(maxHeadway.plus(l.getLength()), l, Length.ZERO, ld))
                        {
                            correctLanes.add(l);
                        }
                        else
                        {
                            wrongLanes.add(l);
                        }
                    }
                }
                for (Lane wrongLane : wrongLanes)
                {
                    for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT,
                            this.gtu.getGTUType()))
                    {
                        if (correctLanes.contains(adjLane))
                        {
                            return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.LEFT);
                        }
                    }
                    for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT,
                            this.gtu.getGTUType()))
                    {
                        if (correctLanes.contains(adjLane))
                        {
                            return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.RIGHT);
                        }
                    }
                }
                return new NextSplitInfo(nextSplitNode, correctCurrentLanes, null);
            }

            if (links.size() == 0)
            {
                return new NextSplitInfo(null, correctCurrentLanes);
            }

            // just one link
            LinkInterface link = links.iterator().next();

            // determine direction for the path
            lastNode = link.getEndNode();
            lastLink = links.iterator().next();
            lengthForward = lengthForward.plus(lastLink.getLength());
        }

        return new NextSplitInfo(null, correctCurrentLanes);
    }

    /**
     * Determine whether the lane is directly connected to our route, in other words: if we would (continue to) drive on the
     * given lane, can we take the right branch at the nextSplitNode without switching lanes?
     * @param maxHeadway Length; the maximum length for use in the calculation
     * @param startLane Lane; the first lane in the list
     * @param startLanePosition Length; the position on the start lane
     * @param linkAfterSplit Link; the link after the split to which we should connect
     * @return boolean; true if the lane (XXXXX which lane?) is connected to our path
     * @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
     */
    protected boolean connectsToPath(final Length maxHeadway, final Lane startLane, final Length startLanePosition,
            final LinkInterface linkAfterSplit) throws GtuException, NetworkException
    {
        List<Lane> lanes = buildLanePathInfo(maxHeadway, startLane, startLanePosition).getLaneList();
        for (Lane lane : lanes)
        {
            if (lane.getParentLink().equals(linkAfterSplit))
            {
                return true;
            }
        }
        return false;
    }

    /**
     * Determine whether the lane does not drop, in other words: if we would (continue to) drive on the given lane, can we
     * continue to drive at the nextSplitNode without switching lanes?
     * @param maxHeadway Length; the maximum length for use in the calculation
     * @param startLane Lane; the first lane in the list
     * @param startLanePosition Length; the position on the start lane
     * @return boolean; true if the lane (XXX which lane?) is connected to our path
     * @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
     */
    protected boolean noLaneDrop(final Length maxHeadway, final Lane startLane, final Length startLanePosition)
            throws GtuException, NetworkException
    {
        LanePathInfo lpi = buildLanePathInfo(maxHeadway, startLane, startLanePosition);
        return lpi.getPath().getLength() >= maxHeadway.si;
    }

    /**
     * Make a list of links on which to drive next, with a maximum headway relative to the reference point of the GTU.
     * @param maxHeadway Length; the maximum length for which links should be returned
     * @return List&lt;LinkDirection&gt;; a list of links on which to drive next
     * @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
     */
    protected List<LinkInterface> buildLinkListForward(final Length maxHeadway) throws GtuException, NetworkException
    {
        List<LinkInterface> linkList = new ArrayList<>();
        LanePosition dlp = this.gtu.getReferencePosition();
        Lane referenceLane = dlp.getLane();
        LinkInterface lastLink = referenceLane.getParentLink();
        linkList.add(lastLink);
        Length lengthForward;
        Length position = dlp.getPosition();
        NodeInterface lastNode;
        lengthForward = referenceLane.getLength().minus(position);
        lastNode = referenceLane.getParentLink().getEndNode();

        // see if we have a split within maxHeadway distance
        while (lengthForward.lt(maxHeadway))
        {
            // calculate the number of "outgoing" links
            Set<LinkInterface> links = lastNode.getOutgoingLinks().toSet(); // is a safe copy
            Iterator<LinkInterface> linkIterator = links.iterator();
            while (linkIterator.hasNext())
            {
                LinkInterface link = linkIterator.next();
                if (link.equals(lastLink) || !link.getLinkType().isCompatible(this.gtu.getGTUType()))
                {
                    linkIterator.remove();
                }
            }

            if (links.size() == 0)
            {
                return linkList; // the path stops here...
            }

            LinkInterface link;
            if (links.size() > 1)
            {
                link = this.gtu.getStrategicalPlanner().nextLink(lastLink, this.gtu.getGTUType());
            }
            else
            {
                link = links.iterator().next();
            }

            // determine direction for the path
            lastNode = lastLink.getEndNode();
            lastLink = link;
            linkList.add(lastLink);
            lengthForward = lengthForward.plus(lastLink.getLength());
        }
        return linkList;
    }

    /** {@inheritDoc} */
    @Override
    public final CarFollowingModel getCarFollowingModel()
    {
        return this.carFollowingModel;
    }

    /**
     * Sets the car-following model.
     * @param carFollowingModel CarFollowingModel; Car-following model to set.
     */
    public final void setCarFollowingModel(final CarFollowingModel carFollowingModel)
    {
        this.carFollowingModel = carFollowingModel;
    }

    /** {@inheritDoc} */
    @Override
    public final LanePerception getPerception()
    {
        return this.lanePerception;
    }

}