package org.opentrafficsim.road.network.lane;

import javax.naming.NamingException;

import org.djunits.unit.AccelerationUnit;
import org.djunits.unit.DurationUnit;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.SpeedUnit;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djutils.draw.line.PolyLine3d;
import org.djutils.draw.point.Point3d;
import org.junit.Test;
import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
import org.opentrafficsim.core.gtu.GtuException;
import org.opentrafficsim.core.gtu.GtuType;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.road.car.CarTest;
import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGtu;
import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.FixedLaneChangeModel;
import org.opentrafficsim.road.network.RoadNetwork;
import org.opentrafficsim.road.network.defaults.RoadNetworkDefaults;
import org.opentrafficsim.road.network.factory.LaneFactory;

import nl.tudelft.simulation.dsol.SimRuntimeException;
import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;

/**
 * Verify that GTUs register and unregister at the correct times and locations when following a curve.
 * <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="https://opentrafficsim.org/docs/v2/license.html">OpenTrafficSim License</a>.
 * </p>
 * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jan 15, 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.citg.tudelft.nl">Guus Tamminga</a>
 */
public class CurveTest
{

    /**
     * Let GTUs drive through a curve and check (de-)registration times at each node.
     * @throws NamingException on error
     * @throws SimRuntimeException on error
     * @throws NetworkException on error
     * @throws GtuException on error
     */
    @Test
    public final void curveTest() throws SimRuntimeException, NamingException, NetworkException, GtuException
    {
        final int laneCount = 1;
        OTSSimulatorInterface simulator = CarTest.makeSimulator();
        RoadNetwork network = new RoadNetwork("curve test network", simulator);
        RoadNetworkDefaults.registerDefaults(network);
        GtuType gtuType = network.getGtuType(GtuType.DEFAULTS.CAR);
        LaneType laneType = network.getLaneType(LaneType.DEFAULTS.ROAD);
        Speed speedLimit = new Speed(50, SpeedUnit.KM_PER_HOUR);
        Node origin = new Node(network, "origin", new Point3d(10, 10, 0), 0.0);
        Node curveStart = new Node(network, "curveStart", new Point3d(100, 10, 0), 0.0);
        Node curveEnd = new Node(network, "curveEnd", new Point3d(150, 60, 0), Math.PI / 2);
        Node destination = new Node(network, "destination", new Point3d(150, 150, 0), Math.PI / 2);
        Lane[] straight1 = LaneFactory.makeMultiLane(network, "straight1", origin, curveStart, null, laneCount, laneType,
                speedLimit, simulator);
        Lane[] straight2 = LaneFactory.makeMultiLane(network, "straight2", curveEnd, destination, null, laneCount, laneType,
                speedLimit, simulator);
        PolyLine3d curveLine = LaneFactory.makeBezier(origin, curveStart, curveEnd, destination);
        Lane[] curve = LaneFactory.makeMultiLane(network, "bezier", curveStart, curveEnd, curveLine.getPoints(), laneCount,
                laneType, speedLimit, simulator);
        Lane[][] laneSets = new Lane[][] {straight1, curve, straight2};
        Length initialPosition = new Length(5, LengthUnit.METER);
        Speed speed = new Speed(10, SpeedUnit.SI);
        for (int lane = 0; lane < laneCount; lane++)
        {
            // System.out.println("Lane is " + lane);
            double cumulativeLength = 0;
            for (Lane[] set : laneSets)
            {
                cumulativeLength += set[lane].getLength().si;
                double timeAtEnd = simulator.getSimulatorTime().si + (cumulativeLength - initialPosition.si) / speed.si;
                System.out.println("lane " + set[lane] + " length is " + set[lane].getLength()
                        + " time for reference to get to end " + timeAtEnd);
            }
            LaneBasedIndividualGtu car = CarTest.makeReferenceCar("car", gtuType, straight1[lane], initialPosition, speed,
                    simulator,
                    new FixedAccelerationModel(new Acceleration(0, AccelerationUnit.SI), new Duration(25, DurationUnit.SI)),
                    new FixedLaneChangeModel(null), network);
            printEventList(simulator);
            System.out.println("STEP");
            simulator.step();
            printEventList(simulator);
            System.out.println("STEP");
            simulator.step();
            printEventList(simulator);
            // TODO finish writing this test
        }
    }

    /**
     * Print all scheduled events of an DEVSSimulatorInterface.TimeDoubleUnit.
     * @param simulator DEVSSimulatorInterface.TimeDoubleUnit; the DEVSSimulatorInterface.TimeDoubleUnit
     */
    public final void printEventList(final DEVSSimulatorInterface.TimeDoubleUnit simulator)
    {
        for (SimEventInterface<SimTimeDoubleUnit> se : simulator.getEventList())
        {
            System.out.println("se: " + se);
        }

    }

}