package org.opentrafficsim.graphs;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.event.ActionEvent;
import java.awt.geom.Line2D;
import java.io.Serializable;
import java.rmi.RemoteException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;

import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JPopupMenu;
import javax.swing.SwingConstants;

import org.djunits.unit.LengthUnit;
import org.djunits.unit.TimeUnit;
import org.djunits.value.vdouble.scalar.DoubleScalar;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Time;
import org.jfree.chart.ChartFactory;
import org.jfree.chart.ChartPanel;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.StandardChartTheme;
import org.jfree.chart.axis.NumberAxis;
import org.jfree.chart.axis.ValueAxis;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.chart.plot.XYPlot;
import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer;
import org.jfree.data.DomainOrder;
import org.jfree.data.general.DatasetChangeEvent;
import org.jfree.data.general.DatasetChangeListener;
import org.jfree.data.general.DatasetGroup;
import org.jfree.data.xy.XYDataset;
import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.network.lane.Lane;

import nl.tudelft.simulation.dsol.SimRuntimeException;
import nl.tudelft.simulation.event.EventInterface;
import nl.tudelft.simulation.event.EventListenerInterface;
import nl.tudelft.simulation.event.TimedEvent;

/**
 * Trajectory plot.
 * <p>
 * Copyright (c) 2013-2016 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-09-14 01:33:02 +0200 (Mon, 14 Sep 2015) $, @version $Revision: 1401 $, by $Author: averbraeck $,
 * initial version Jul 24, 2014 <br>
 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public class TrajectoryPlot extends AbstractOTSPlot implements XYDataset, LaneBasedGTUSampler, EventListenerInterface

{
    /** */
    private static final long serialVersionUID = 20140724L;

    /** Sample interval of this TrajectoryPlot. */
    private final Duration sampleInterval;

    /** The simulator. */
    private final OTSDEVSSimulatorInterface simulator;

    /**
     * @return sampleInterval if this TrajectoryPlot samples at a fixed rate, or null if this TrajectoryPlot samples on the GTU
     *         move events
     */
    public final Duration getSampleInterval()
    {
        return this.sampleInterval;
    }

    /** The cumulative lengths of the elements of path. */
    private final double[] cumulativeLengths;

    /**
     * Retrieve the cumulative length of the sampled path at the end of a path element.
     * @param index int; the index of the path element; if -1, the total length of the path is returned
     * @return double; the cumulative length at the end of the specified path element in meters (si)
     */
    public final double getCumulativeLength(final int index)
    {
        return index == -1 ? this.cumulativeLengths[this.cumulativeLengths.length - 1] : this.cumulativeLengths[index];
    }

    /** Maximum of the time axis. */
    private Time maximumTime = new Time(300, TimeUnit.SECOND);

    /**
     * @return maximumTime
     */
    public final Time getMaximumTime()
    {
        return this.maximumTime;
    }

    /**
     * @param maximumTime set maximumTime
     */
    public final void setMaximumTime(final Time maximumTime)
    {
        this.maximumTime = maximumTime;
    }

    /** Not used internally. */
    private DatasetGroup datasetGroup = null;

    /**
     * Create a new TrajectoryPlot.
     * @param caption String; the text to show above the TrajectoryPlot
     * @param sampleInterval DoubleScalarRel&lt;TimeUnit&gt;; the time between samples of this TrajectoryPlot, or null in which
     *            case the GTUs are sampled whenever they fire a MOVE_EVENT
     * @param path ArrayList&lt;Lane&gt;; the series of Lanes that will provide the data for this TrajectoryPlot
     * @param simulator OTSDEVSSimulatorInterface; the simulator
     */
    public TrajectoryPlot(final String caption, final Duration sampleInterval, final List<Lane> path,
            final OTSDEVSSimulatorInterface simulator)
    {
        super(caption, path);
        this.sampleInterval = sampleInterval;
        this.simulator = simulator;
        double[] endLengths = new double[path.size()];
        double cumulativeLength = 0;
        for (int i = 0; i < path.size(); i++)
        {
            Lane lane = path.get(i);
            lane.addListener(this, Lane.GTU_ADD_EVENT, true);
            lane.addListener(this, Lane.GTU_REMOVE_EVENT, true);
            try
            {
                // Register the GTUs currently (i.e. already) on the lane (if any) for statistics sampling.
                for (LaneBasedGTU gtu : lane.getGtuList())
                {
                    notify(new TimedEvent<OTSSimTimeDouble>(Lane.GTU_ADD_EVENT, lane, new Object[] { gtu.getId(), gtu },
                            gtu.getSimulator().getSimulatorTime()));
                }
            }
            catch (RemoteException exception)
            {
                exception.printStackTrace();
            }
            cumulativeLength += lane.getLength().getSI();
            endLengths[i] = cumulativeLength;
        }
        this.cumulativeLengths = endLengths;
        setChart(createChart(this));
        this.reGraph(); // fixes the domain axis
        if (null != this.sampleInterval)
        {
            try
            {
                this.simulator.scheduleEventRel(Duration.ZERO, this, this, "sample", null);
            }
            catch (SimRuntimeException exception)
            {
                exception.printStackTrace();
            }
        }
    }

    /** {@inheritDoc} */
    @Override
    public final GraphType getGraphType()
    {
        return GraphType.TRAJECTORY;
    }

    /**
     * Sample all the GTUs on the observed lanes.
     */
    public void sample()
    {
        Time now = this.simulator.getSimulatorTime().getTime();
        for (LaneBasedGTU gtu : this.gtusOfInterest)
        {
            try
            {
                Map<Lane, Length> positions = gtu.positions(gtu.getReference(), now);
                int hits = 0;
                for (Lane lane : positions.keySet())
                {
                    if (getPath().contains(lane))
                    {
                        Length position = positions.get(lane);
                        if (position.si >= 0 && position.si <= lane.getLength().si)
                        {
                            addData(gtu, lane, positions.get(lane).si);
                            hits++;
                        }
                    }
                }
                if (1 != hits)
                {
                    System.err.println("GTU " + gtu + " scored " + hits + " (expected 1 hit)");
                }
            }
            catch (GTUException exception)
            {
                exception.printStackTrace();
            }
        }
        // Schedule the next sample
        try
        {
            this.simulator.scheduleEventRel(this.sampleInterval, this, this, "sample", null);
        }
        catch (SimRuntimeException exception)
        {
            exception.printStackTrace();
        }
    }

    /** The GTUs that might be of interest to gather statistics about. */
    private Set<LaneBasedGTU> gtusOfInterest = new HashSet<>();

    /** {@inheritDoc} */
    @Override
    @SuppressWarnings("checkstyle:designforextension")
    public void notify(final EventInterface event) throws RemoteException
    {
        LaneBasedGTU gtu;
        if (event.getType().equals(Lane.GTU_ADD_EVENT))
        {
            Object[] content = (Object[]) event.getContent();
            gtu = (LaneBasedGTU) content[1];
            if (!this.gtusOfInterest.contains(gtu))
            {
                this.gtusOfInterest.add(gtu);
                if (null == this.sampleInterval)
                {
                    gtu.addListener(this, LaneBasedGTU.LANEBASED_MOVE_EVENT);
                }
            }
        }
        else if (event.getType().equals(Lane.GTU_REMOVE_EVENT))
        {
            Object[] content = (Object[]) event.getContent();
            gtu = (LaneBasedGTU) content[1];
            Lane lane = null;
            try
            {
                lane = gtu.getReferencePosition().getLane();
            }
            catch (GTUException exception)
            {
                // ignore - lane will be null
            }
            if (lane == null || !getPath().contains(lane))
            {
                this.gtusOfInterest.remove(gtu);
                if (null != this.sampleInterval)
                {
                    gtu.removeListener(this, LaneBasedGTU.LANEBASED_MOVE_EVENT);
                }
                else
                {
                    String key = gtu.getId();
                    VariableSampleRateTrajectory carTrajectory = (VariableSampleRateTrajectory) this.trajectories.get(key);
                    if (null != carTrajectory)
                    {
                        carTrajectory.recordGTULeftTrajectoryEvent();
                    }
                }
            }
        }
        else if (event.getType().equals(LaneBasedGTU.LANEBASED_MOVE_EVENT))
        {
            Object[] content = (Object[]) event.getContent();
            Lane lane = (Lane) content[6];
            Length posOnLane = (Length) content[7];
            gtu = (LaneBasedGTU) event.getSource();
            if (getPath().contains(lane))
            {
                addData(gtu, lane, posOnLane.si);
            }
        }
    }

    /** {@inheritDoc} */
    @Override
    protected JFreeChart createChart(final JFrame container)
    {
        final JLabel statusLabel = new JLabel(" ", SwingConstants.CENTER);
        container.add(statusLabel, BorderLayout.SOUTH);
        ChartFactory.setChartTheme(new StandardChartTheme("JFree/Shadow", false));
        final JFreeChart result =
                ChartFactory.createXYLineChart(getCaption(), "", "", this, PlotOrientation.VERTICAL, false, false, false);
        // Overrule the default background paint because some of the lines are invisible on top of this default.
        result.getPlot().setBackgroundPaint(new Color(0.9f, 0.9f, 0.9f));
        FixCaption.fixCaption(result);
        NumberAxis xAxis = new NumberAxis("\u2192 " + "time [s]");
        xAxis.setLowerMargin(0.0);
        xAxis.setUpperMargin(0.0);
        NumberAxis yAxis = new NumberAxis("\u2192 " + "Distance [m]");
        yAxis.setAutoRangeIncludesZero(false);
        yAxis.setLowerMargin(0.0);
        yAxis.setUpperMargin(0.0);
        yAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits());
        result.getXYPlot().setDomainAxis(xAxis);
        result.getXYPlot().setRangeAxis(yAxis);
        Length minimumPosition = Length.ZERO;
        Length maximumPosition = new Length(getCumulativeLength(-1), LengthUnit.SI);
        configureAxis(result.getXYPlot().getRangeAxis(), DoubleScalar.minus(maximumPosition, minimumPosition).getSI());
        final XYLineAndShapeRenderer renderer = (XYLineAndShapeRenderer) result.getXYPlot().getRenderer();
        renderer.setBaseLinesVisible(true);
        renderer.setBaseShapesVisible(false);
        renderer.setBaseShape(new Line2D.Float(0, 0, 0, 0));
        final ChartPanel cp = new ChartPanel(result);
        cp.setMouseWheelEnabled(true);
        final PointerHandler ph = new PointerHandler()
        {
            /** {@inheritDoc} */
            @Override
            void updateHint(final double domainValue, final double rangeValue)
            {
                if (Double.isNaN(domainValue))
                {
                    statusLabel.setText(" ");
                    return;
                }
                String value = "";
                /*-
                XYDataset dataset = plot.getDataset();
                double bestDistance = Double.MAX_VALUE;
                Trajectory bestTrajectory = null;
                final int mousePrecision = 5;
                java.awt.geom.Point2D.Double mousePoint = new java.awt.geom.Point2D.Double(t, distance);
                double lowTime =
                        plot.getDomainAxis().java2DToValue(p.getX() - mousePrecision, pi.getDataArea(),
                                plot.getDomainAxisEdge()) - 1;
                double highTime =
                        plot.getDomainAxis().java2DToValue(p.getX() + mousePrecision, pi.getDataArea(),
                                plot.getDomainAxisEdge()) + 1;
                double lowDistance =
                        plot.getRangeAxis().java2DToValue(p.getY() + mousePrecision, pi.getDataArea(),
                                plot.getRangeAxisEdge()) - 20;
                double highDistance =
                        plot.getRangeAxis().java2DToValue(p.getY() - mousePrecision, pi.getDataArea(),
                                plot.getRangeAxisEdge()) + 20;
                // System.out.println(String.format("Searching area t[%.1f-%.1f], x[%.1f,%.1f]", lowTime, highTime,
                // lowDistance, highDistance));
                for (Trajectory trajectory : this.trajectories)
                {
                    java.awt.geom.Point2D.Double[] clippedTrajectory =
                            trajectory.clipTrajectory(lowTime, highTime, lowDistance, highDistance);
                    if (null == clippedTrajectory)
                        continue;
                    java.awt.geom.Point2D.Double prevPoint = null;
                    for (java.awt.geom.Point2D.Double trajectoryPoint : clippedTrajectory)
                    {
                        if (null != prevPoint)
                        {
                            double thisDistance = Planar.distancePolygonToPoint(clippedTrajectory, mousePoint);
                            if (thisDistance < bestDistance)
                            {
                                bestDistance = thisDistance;
                                bestTrajectory = trajectory;
                            }
                        }
                        prevPoint = trajectoryPoint;
                    }
                }
                if (null != bestTrajectory)
                {
                    for (SimulatedObject so : indices.keySet())
                        if (this.trajectories.get(indices.get(so)) == bestTrajectory)
                        {
                            Point2D.Double bestPosition = bestTrajectory.getEstimatedPosition(t);
                            if (null == bestPosition)
                                continue;
                            value =
                                    String.format(
                                            Main.locale,
                                            ": vehicle %s; location on measurement path at t=%.1fs: "
                                            + "longitudinal %.1fm, lateral %.1fm",
                                            so.toString(), t, bestPosition.x, bestPosition.y);
                        }
                }
                else
                    value = "";
                 */
                statusLabel.setText(String.format("t=%.0fs, distance=%.0fm%s", domainValue, rangeValue, value));
            }
        };
        cp.addMouseMotionListener(ph);
        cp.addMouseListener(ph);
        container.add(cp, BorderLayout.CENTER);
        // TODO ensure that shapes for all the data points don't get allocated.
        // Currently JFreeChart allocates many megabytes of memory for Ellipses that are never drawn.
        JPopupMenu popupMenu = cp.getPopupMenu();
        popupMenu.add(new JPopupMenu.Separator());
        popupMenu.add(StandAloneChartWindow.createMenuItem(this));
        return result;
    }

    /** {@inheritDoc} */
    @Override
    public final void reGraph()
    {
        for (DatasetChangeListener dcl : getListenerList().getListeners(DatasetChangeListener.class))
        {
            if (dcl instanceof XYPlot)
            {
                configureAxis(((XYPlot) dcl).getDomainAxis(), this.maximumTime.getSI());
            }
        }
        notifyListeners(new DatasetChangeEvent(this, null)); // This guess work actually works!
    }

    /**
     * Configure the range of an axis.
     * @param valueAxis ValueAxis
     * @param range double; the upper bound of the axis
     */
    private static void configureAxis(final ValueAxis valueAxis, final double range)
    {
        valueAxis.setUpperBound(range);
        valueAxis.setLowerMargin(0);
        valueAxis.setUpperMargin(0);
        valueAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits());
        valueAxis.setAutoRange(true);
        valueAxis.setAutoRangeMinimumSize(range);
        valueAxis.centerRange(range / 2);
    }

    /** {@inheritDoc} */
    @Override
    public void actionPerformed(final ActionEvent e)
    {
        // not yet
    }

    /** All stored trajectories. */
    private HashMap<String, Trajectory> trajectories = new HashMap<String, Trajectory>();

    /** Quick access to the Nth trajectory. */
    private ArrayList<Trajectory> trajectoryIndices = new ArrayList<Trajectory>();

    /**
     * Add data for a GTU on a lane to this graph.
     * @param gtu the gtu to add the data for
     * @param lane the lane on which the GTU is registered
     * @param posOnLane the position on the lane as a double si Length
     */
    protected final void addData(final LaneBasedGTU gtu, final Lane lane, final double posOnLane)
    {
        int index = getPath().indexOf(lane);
        if (index < 0)
        {
            // error -- silently ignore for now. Graphs should not cause errors.
            System.err.println("TrajectoryPlot: GTU " + gtu.getId() + " is not registered on lane " + lane.toString());
            return;
        }
        double lengthOffset = index == 0 ? 0 : this.cumulativeLengths[index - 1];

        String key = gtu.getId();
        Trajectory carTrajectory = this.trajectories.get(key);
        if (null == carTrajectory)
        {
            // Create a new Trajectory for this GTU
            carTrajectory =
                    null == this.sampleInterval ? new VariableSampleRateTrajectory(key) : new FixedSampleRateTrajectory(key);
            this.trajectoryIndices.add(carTrajectory);
            this.trajectories.put(key, carTrajectory);
        }
        try
        {
            carTrajectory.addSample(gtu, lane, lengthOffset + posOnLane);
        }
        catch (NetworkException | GTUException exception)
        {
            // error -- silently ignore for now. Graphs should not cause errors.
            System.err.println("TrajectoryPlot: GTU " + gtu.getId() + " on lane " + lane.toString() + " caused exception "
                    + exception.getMessage());
        }
    }

    /**
     * Common interface for both (all?) types of trajectories.
     */
    interface Trajectory
    {
        /**
         * Retrieve the time of the last stored event.
         * @return Time; the time of the last stored event
         */
        Time getCurrentEndTime();

        /**
         * Retrieve the last recorded non-null position, or null if no non-null positions have been recorded yet.
         * @return Double; the last recorded position of this Trajectory in meters
         */
        Double getLastPosition();

        /**
         * Retrieve the id of this Trajectory.
         * @return Object; the id of this Trajectory
         */
        String getId();

        /**
         * Add a trajectory segment sample and update the currentEndTime and currentEndPosition.
         * @param gtu AbstractLaneBasedGTU; the GTU whose currently committed trajectory segment must be added
         * @param lane Lane; the Lane that the positionOffset is valid for
         * @param position Double; distance in meters from the start of the trajectory
         * @throws NetworkException when car is not on lane anymore
         * @throws GTUException on problems obtaining data from the GTU
         */
        void addSample(LaneBasedGTU gtu, Lane lane, double position) throws NetworkException, GTUException;

        /**
         * Retrieve the number of stored samples in this Trajectory.
         * @return int; number of stored samples
         */
        int size();

        /**
         * Return the time of the Nth stored sample.
         * @param item int; the index of the sample
         * @return double; the time of the sample
         */
        double getTime(int item);

        /**
         * Return the distance of the Nth stored sample.
         * @param item int; the index of the sample
         * @return double; the distance of the sample
         */
        double getDistance(int item);

    }

    /**
     * Store trajectory data for use with a variable sample rate.
     * <p>
     * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
     */
    class VariableSampleRateTrajectory implements Trajectory, Serializable
    {
        /** */
        private static final long serialVersionUID = 20140000L;

        /** Time of (current) end of trajectory. */
        private Time currentEndTime;

        /** ID of the GTU. */
        private final String id;

        /** Storage for the samples of the GTU. */
        private ArrayList<DistanceAndTime> samples = new ArrayList<DistanceAndTime>();

        /**
         * Construct a new VariableSamplerateTrajectory.
         * @param id String; id of the new Trajectory (id of the GTU)
         */
        public VariableSampleRateTrajectory(final String id)
        {
            this.id = id;
        }

        /** {@inheritDoc} */
        @Override
        public Time getCurrentEndTime()
        {
            return this.currentEndTime;
        }

        /** {@inheritDoc} */
        @Override
        public Double getLastPosition()
        {
            return null;
        }

        /** {@inheritDoc} */
        @Override
        public String getId()
        {
            return this.id;
        }

        /** {@inheritDoc} */
        @Override
        public void addSample(LaneBasedGTU gtu, Lane lane, double position) throws NetworkException, GTUException
        {
            if (this.samples.size() > 0)
            {
                DistanceAndTime lastSample = this.samples.get(this.samples.size() - 1);
                if (null != lastSample)
                {
                    Double lastPosition = lastSample.getDistance();
                    if (null != lastPosition && Math.abs(lastPosition - position) > 0.9 * getCumulativeLength(-1))
                    {
                        // wrap around... probably circular lane, insert a GTU left trajectory event.
                        recordGTULeftTrajectoryEvent();
                    }
                }
            }
            this.currentEndTime = gtu.getSimulator().getSimulatorTime().getTime();
            this.samples.add(new DistanceAndTime(position, this.currentEndTime.si));
        }

        /**
         * Store that the GTU went off of the trajectory.
         */
        public void recordGTULeftTrajectoryEvent()
        {
            this.samples.add(null);
        }

        /** {@inheritDoc} */
        @Override
        public int size()
        {
            return this.samples.size();
        }

        /**
         * Retrieve the Nth sample.
         * @param item int; the number of the sample
         * @return DistanceAndTime; the Nth sample (samples can be null to indicate that GTU went off the trajectory).
         */
        private DistanceAndTime getSample(int item)
        {
            return this.samples.get(item);
        }

        /** {@inheritDoc} */
        @Override
        public double getTime(int item)
        {
            DistanceAndTime sample = getSample(item);
            if (null == sample)
            {
                return Double.NaN;
            }
            return this.samples.get(item).getTime();
        }

        /** {@inheritDoc} */
        @Override
        public double getDistance(int item)
        {
            DistanceAndTime sample = getSample(item);
            if (null == sample)
            {
                return Double.NaN;
            }
            return sample.getDistance();
        }
        
        /** {@inheritDoc} */
        @Override
        public String toString()
        {
            return "VariableSampleRateTrajectory [id=" + this.id + ", currentEndTime=" + this.currentEndTime + "]";
        }

        /**
         * Store a position and a time.
         */
        class DistanceAndTime
        {
            /** The position [m]. */
            final double distance;

            /** The time [s]. */
            final double time;

            /**
             * Construct a new DistanceAndTime object.
             * @param distance double; the position
             * @param time double; the time
             */
            public DistanceAndTime(final double distance, final double time)
            {
                this.distance = distance;
                this.time = time;
            }

            /**
             * Retrieve the position.
             * @return double; the position
             */
            public double getDistance()
            {
                return this.distance;
            }

            /**
             * Retrieve the time.
             * @return double; the time
             */
            public double getTime()
            {
                return this.time;
            }

            /** {@inheritDoc} */
            @Override
            public String toString()
            {
                return "DistanceAndTime [distance=" + this.distance + ", time=" + this.time + "]";
            }

        }
    }

    /**
     * Store trajectory data for use with a fixed sample rate.
     * <p>
     * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
     */
    class FixedSampleRateTrajectory implements Trajectory, Serializable
    {
        /** */
        private static final long serialVersionUID = 20140000L;

        /** Time of (current) end of trajectory. */
        private Time currentEndTime;

        /** ID of the GTU. */
        private final String id;

        /** Storage for the position of the GTU. */
        private ArrayList<Double> positions = new ArrayList<Double>();

        /** Sample number of sample with index 0 in positions (following entries will each be one sampleTime later). */
        private int firstSample;

        /**
         * Construct a FixedSampleRateTrajectory.
         * @param id String; id of the new Trajectory (id of the GTU)
         */
        FixedSampleRateTrajectory(final String id)
        {
            this.id = id;
        }

        /** {@inheritDoc} */
        public final Time getCurrentEndTime()
        {
            return this.currentEndTime;
        }

        /** {@inheritDoc} */
        public final Double getLastPosition()
        {
            for (int i = this.positions.size(); --i >= 0;)
            {
                Double result = this.positions.get(i);
                if (null != result)
                {
                    return result;
                }
            }
            return null;
        }

        /** {@inheritDoc} */
        public final String getId()
        {
            return this.id;
        }

        /** {@inheritDoc} */
        public final void addSample(final LaneBasedGTU gtu, final Lane lane, final double position)
                throws NetworkException, GTUException
        {
            final int sample = (int) Math.ceil(gtu.getOperationalPlan().getStartTime().si / getSampleInterval().si);
            if (0 == this.positions.size())
            {
                this.firstSample = sample;
            }
            while (sample - this.firstSample > this.positions.size())
            {
                // insert nulls as place holders for unsampled data (usually because vehicle was in a parallel Lane)
                this.positions.add(null);
            }
            Double adjustedPosition = position;
            Double lastPosition = this.positions.size() > 0 ? this.positions.get(this.positions.size() - 1) : null;
            if (null != lastPosition && Math.abs(lastPosition - position) > 0.9 * getCumulativeLength(-1))
            {
                // wrap around... probably circular lane.
                adjustedPosition = null;
            }
            this.positions.add(adjustedPosition);

            this.currentEndTime = gtu.getSimulator().getSimulatorTime().getTime();

            /*-
            try
            {
                final int startSample =
                        (int) Math.ceil(car.getOperationalPlan().getStartTime().getSI() / getSampleInterval());
                final int endSample =
                        (int) (Math.ceil(car.getOperationalPlan().getEndTime().getSI() / getSampleInterval()));
                for (int sample = startSample; sample < endSample; sample++)
                {
                    Time sampleTime = new Time(sample * getSampleInterval(), TimeUnit.SI);
                    Double position = car.position(lane, car.getReference(), sampleTime).getSI() + positionOffset;
                    if (this.positions.size() > 0 && null != this.currentEndPosition
                            && position < this.currentEndPosition.getSI() - 0.001)
                    {
                        if (0 != positionOffset)
                        {
                            // System.out.println("Already added " + car);
                            break;
                        }
                        // System.out.println("inserting null for " + car);
                        position = null; // Wrapping on circular path?
                    }
                    if (this.positions.size() == 0)
                    {
                        this.firstSample = sample;
                    }
                    while (sample - this.firstSample > this.positions.size())
                    {
                        // System.out.println("Inserting nulls");
                        this.positions.add(null); // insert nulls as place holders for unsampled data (usually because
                                                  // vehicle was temporarily in a parallel Lane)
                    }
                    if (null != position && this.positions.size() > sample - this.firstSample)
                    {
                        // System.out.println("Skipping sample " + car);
                        continue;
                    }
                    this.positions.add(position);
                }
                this.currentEndTime = car.getOperationalPlan().getEndTime();
                this.currentEndPosition = new Length(
                        car.position(lane, car.getReference(), this.currentEndTime).getSI() + positionOffset, LengthUnit.SI);
            }
            catch (Exception e)
            {
                // TODO lane change causes error...
                System.err.println("Trajectoryplot caught unexpected Exception: " + e.getMessage());
                e.printStackTrace();
            }
             */
            if (gtu.getSimulator().getSimulatorTime().getTime().gt(getMaximumTime()))
            {
                setMaximumTime(gtu.getSimulator().getSimulatorTime().getTime());
            }
        }

        /** {@inheritDoc} */
        public int size()
        {
            return this.positions.size();
        }

        /** {@inheritDoc} */
        public double getTime(final int item)
        {
            return (item + this.firstSample) * getSampleInterval().si;
        }

        /**
         * @param item Integer; the sample number
         * @return Double; the position indexed by item
         */
        public double getDistance(final int item)
        {
            Double distance = this.positions.get(item);
            if (null == distance)
            {
                return Double.NaN;
            }
            return this.positions.get(item);
        }

        /** {@inheritDoc} */
        @Override
        public final String toString()
        {
            return "FixedSampleRateTrajectory [currentEndTime=" + this.currentEndTime + ", id=" + this.id + ", positions.size="
                    + this.positions.size() + ", firstSample=" + this.firstSample + "]";
        }

    }

    /** {@inheritDoc} */
    @Override
    public final int getSeriesCount()
    {
        return this.trajectories.size();
    }

    /** {@inheritDoc} */
    @Override
    public final Comparable<Integer> getSeriesKey(final int series)
    {
        return series;
    }

    /** {@inheritDoc} */
    @SuppressWarnings("rawtypes")
    @Override
    public final int indexOf(final Comparable seriesKey)
    {
        if (seriesKey instanceof Integer)
        {
            return (Integer) seriesKey;
        }
        return -1;
    }

    /** {@inheritDoc} */
    @Override
    public final DatasetGroup getGroup()
    {
        return this.datasetGroup;
    }

    /** {@inheritDoc} */
    @Override
    public final void setGroup(final DatasetGroup group)
    {
        this.datasetGroup = group;
    }

    /** {@inheritDoc} */
    @Override
    public final DomainOrder getDomainOrder()
    {
        return DomainOrder.ASCENDING;
    }

    /** {@inheritDoc} */
    @Override
    public final int getItemCount(final int series)
    {
        return this.trajectoryIndices.get(series).size();
    }

    /** {@inheritDoc} */
    @Override
    public final Number getX(final int series, final int item)
    {
        double v = getXValue(series, item);
        if (Double.isNaN(v))
        {
            return null;
        }
        return v;
    }

    /** {@inheritDoc} */
    @Override
    public final double getXValue(final int series, final int item)
    {
        return this.trajectoryIndices.get(series).getTime(item);
    }

    /** {@inheritDoc} */
    @Override
    public final Number getY(final int series, final int item)
    {
        double v = getYValue(series, item);
        if (Double.isNaN(v))
        {
            return null;
        }
        return v;
    }

    /** {@inheritDoc} */
    @Override
    public final double getYValue(final int series, final int item)
    {
        return this.trajectoryIndices.get(series).getDistance(item);
    }

    /** {@inheritDoc} */
    @Override
    public final String toString()
    {
        return "TrajectoryPlot [sampleInterval=" + this.sampleInterval + ", path=" + getPath() + ", cumulativeLengths.length="
                + this.cumulativeLengths.length + ", maximumTime=" + this.maximumTime + ", caption=" + getCaption()
                + ", trajectories.size=" + this.trajectories.size() + "]";
    }

}