package org.opentrafficsim.road.network.lane;

import static org.junit.Assert.assertEquals;
import static org.junit.Assert.assertFalse;
import static org.junit.Assert.assertTrue;

import java.rmi.RemoteException;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Set;

import org.djunits.unit.DirectionUnit;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.SpeedUnit;
import org.djunits.value.vdouble.scalar.Direction;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.scalar.Time;
import org.djutils.draw.line.PolyLine3d;
import org.djutils.draw.line.Polygon3d;
import org.djutils.draw.point.Point2d;
import org.djutils.draw.point.Point3d;
import org.djutils.event.EventInterface;
import org.djutils.event.EventListenerInterface;
import org.junit.Test;
import org.mockito.Mockito;
import org.opentrafficsim.core.dsol.OTSReplication;
import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
import org.opentrafficsim.core.gtu.GtuType;
import org.opentrafficsim.core.network.LinkType;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.perception.HistoryManagerDEVS;
import org.opentrafficsim.road.mock.MockDEVSSimulator;
import org.opentrafficsim.road.network.RoadNetwork;
import org.opentrafficsim.road.network.defaults.RoadNetworkDefaults;
import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
import org.opentrafficsim.road.network.lane.conflict.Conflict;
import org.opentrafficsim.road.network.lane.conflict.ConflictType;
import org.opentrafficsim.road.network.lane.conflict.DefaultConflictRule;

/**
 * Test the Conflict class.
 * <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>
 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public class ConflictTest implements EventListenerInterface
{
    /** ... */
    private static final long serialVersionUID = 20200708L;

    /** Storage for received events. */
    private List<EventInterface> collectedEvents = new ArrayList<>();

    /**
     * Test the Conflict class.
     * @throws NetworkException on error
     */
    @Test
    public void testConstructor() throws NetworkException
    {
        OTSSimulatorInterface simulator = MockDEVSSimulator.createMock();
        OTSReplication replication = Mockito.mock(OTSReplication.class);
        HistoryManagerDEVS hmd = Mockito.mock(HistoryManagerDEVS.class);
        Mockito.when(hmd.now()).thenReturn(Time.ZERO);
        Mockito.when(replication.getHistoryManager(simulator)).thenReturn(hmd);
        Mockito.when(simulator.getReplication()).thenReturn(replication);
        Mockito.when(simulator.getSimulatorTime()).thenReturn(Time.ZERO);
        RoadNetwork network = new RoadNetwork("Network for conflict test", simulator);
        RoadNetworkDefaults.registerDefaults(network);
        LinkType linkType = network.getLinkType(LinkType.DEFAULTS.ROAD);
        LaneType laneType = network.getLaneType(LaneType.DEFAULTS.ROAD);
        Point3d pointAFrom = new Point3d(0, 0, 0);
        Node nodeAFrom = new Node(network, "A from", pointAFrom, 0.0);
        Point3d pointATo = new Point3d(100, 0, 0);
        Node nodeATo = new Node(network, "A to", pointATo, 0.0);
        CrossSectionLink linkA = new CrossSectionLink(network, "Link A", nodeAFrom, nodeATo, linkType,
                new PolyLine3d(pointAFrom, pointATo), LaneKeepingPolicy.KEEPRIGHT);
        Lane laneA = new Lane(linkA, "lane A", Length.ZERO, new Length(2, LengthUnit.METER), laneType,
                new Speed(50, SpeedUnit.KM_PER_HOUR));
        laneA.addListener(this, Lane.OBJECT_ADD_EVENT);

        Point3d pointBFrom = new Point3d(30, -15, 0);
        Point3d pointBTo = new Point3d(60, 60, 0);
        Node nodeBFrom = new Node(network, "B from", pointBFrom, 0.0); // Direction is rather far off ...
        Node nodeBTo = new Node(network, "B to", pointBTo, 0.0);
        Direction bDirection =
                new Direction(Math.atan2(pointBTo.y - pointBFrom.y, pointBTo.x - pointBFrom.x), DirectionUnit.EAST_RADIAN);
        CrossSectionLink linkB = new CrossSectionLink(network, "Link B", nodeBFrom, nodeBTo, linkType,
                new PolyLine3d(pointBFrom, pointBTo), LaneKeepingPolicy.KEEPRIGHT);
        Lane laneB = new Lane(linkB, "lane B", Length.ZERO, new Length(4, LengthUnit.METER), laneType,
                new Speed(50, SpeedUnit.KM_PER_HOUR));
        laneB.addListener(this, Lane.OBJECT_ADD_EVENT);
        // The intersection of the link design lines is at 50, 0
        System.out.print(laneA.getContour().project().toPlot());
        System.out.print(laneB.getContour().project().toPlot());
        System.out.println("c0,1,0");
        System.out.print(laneA.getCenterLine().project().toPlot());
        System.out.print(laneB.getCenterLine().project().toPlot());
        System.out.println("c1,0,0");

        // Find out where the conflict area starts. With acute angles this is the point closest to pointAFrom among the
        // intersections of the lane contours. Similar for conflict area end.
        Point3d conflictStart = null;
        double closestDistance = Double.MAX_VALUE;
        Point3d conflictEnd = null;
        double furthestDistance = 0.0;
        for (Point3d intersection : intersections(laneA.getContour(), laneB.getContour()))
        {
            double distance = pointAFrom.distance(intersection);
            if (distance < closestDistance)
            {
                conflictStart = intersection;
                closestDistance = distance;
            }
            if (distance > furthestDistance)
            {
                conflictEnd = intersection;
                furthestDistance = distance;
            }
        }
        // System.out.println(conflictStart);
        // System.out.println(conflictEnd);

        // Next statements pretend that vehicle width equals lane width.
        Polygon3d geometry1 = new Polygon3d(conflictStart, new Point3d(conflictEnd.x, conflictStart.y, 0), conflictEnd,
                new Point3d(conflictStart.x, conflictEnd.y, 0), conflictStart);
        System.out.print(geometry1.project().toPlot());
        Polygon3d geometry2 = new Polygon3d(conflictStart,
                new Point3d(conflictStart.x + laneB.getWidth(0).si * Math.sin(bDirection.si),
                        conflictStart.y - laneB.getWidth(0).si * Math.cos(bDirection.si), 0),
                conflictEnd, new Point3d(conflictEnd.x - laneB.getWidth(0).si * Math.sin(bDirection.si),
                        conflictEnd.y + laneB.getWidth(0).si * Math.cos(bDirection.si), 0),
                conflictStart);
        System.out.print(geometry2.project().toPlot());

        System.out.println("#angle B:           " + bDirection.toString(DirectionUnit.EAST_DEGREE));
        Length conflictBStart =
                new Length(pointBFrom.distance(new Point3d(conflictStart.x + laneB.getWidth(0).si / 2 * Math.sin(bDirection.si),
                        conflictStart.y - laneB.getWidth(0).si / 2 * Math.cos(bDirection.si), 0)), LengthUnit.SI);
        System.out.println("#conflict B start:  " + conflictBStart);
        Length conflictBLength = new Length(
                laneA.getWidth(0).si / Math.sin(bDirection.si) + laneB.getWidth(0).si / Math.tan(bDirection.si), LengthUnit.SI);
        System.out.println("#conflict B length: " + conflictBLength);
        System.out.println("c0,0,1");
        System.out.println(String.format("M%.3f,%.3f <%f l%f,0", pointBFrom.x, pointBFrom.y, Math.toDegrees(bDirection.si),
                conflictBStart.si));
        System.out.println(String.format("c0,0,0 l%f,0", conflictBLength.si));

        GtuType bicycles = network.getGtuType("BICYCLE");
        GtuType cars = network.getGtuType("CAR");

        assertEquals("not events received yet", 0, this.collectedEvents.size());

        // That was a lot of code - just to prepare things to call generateConflictPair ...
        Conflict.generateConflictPair(ConflictType.CROSSING, new DefaultConflictRule(), false, laneA,
                new Length(conflictStart.x, LengthUnit.SI), new Length(conflictEnd.x - conflictStart.x, LengthUnit.SI),
                geometry1, bicycles, laneB, conflictBStart, conflictBLength, geometry2, cars, simulator);

        // Check that two conflicts have been created
        assertEquals("one conflict on lane A", 1, laneA.getLaneBasedObjects().size());
        assertEquals("one conflict on lane B", 1, laneB.getLaneBasedObjects().size());
        // Get the Conflicts
        Conflict conflictA = (Conflict) laneA.getLaneBasedObjects().get(0);
        System.out.println("Conflict A: " + conflictA);
        Conflict conflictB = (Conflict) laneB.getLaneBasedObjects().get(0);
        System.out.println("Conflict B: " + conflictB);

        assertEquals("the conflicts are each others counter part", conflictA, conflictB.getOtherConflict());
        assertEquals("the conflicts are each others counter part", conflictB, conflictA.getOtherConflict());
        assertEquals("longitudinal position", new Length(conflictStart.x, LengthUnit.SI), conflictA.getLongitudinalPosition());
        assertEquals("longitudinal position", conflictBStart, conflictB.getLongitudinalPosition());
        assertEquals("length", new Length(conflictEnd.x - conflictStart.x, LengthUnit.SI), conflictA.getLength());
        assertEquals("length", conflictBLength, conflictB.getLength());
        assertEquals("geometry", geometry1, conflictA.getGeometry());
        assertEquals("geometry", geometry2, conflictB.getGeometry());
        assertTrue("conflict rule", conflictA.getConflictRule() instanceof DefaultConflictRule);
        assertTrue("conflict rule", conflictB.getConflictRule() instanceof DefaultConflictRule);
        assertFalse("conflict A is not permitted", conflictA.isPermitted());
        assertFalse("conflict B is not permitted", conflictB.isPermitted());
        assertEquals("conflict A is about bicycles", bicycles, conflictA.getGtuType());
        assertEquals("conflict B is about cars", cars, conflictB.getGtuType());
        assertEquals("construction of two conflicts has generated two events", 2, this.collectedEvents.size());
        // Not checking the contents of those events; these are subject to change; as they indirectly link to the Network

    }

    /**
     * Find all 2D (ignoring Z) intersections between two OTSLine3D objects.
     * @param a OTSLine3D; the first polyline
     * @param b OTSLine3D; the second polyline
     * @return Set&lt;OTSPoint3D&gt;; the intersections
     */
    public Set<Point3d> intersections(final PolyLine3d a, final PolyLine3d b)
    {
        // TODO discuss if this method should be moved into the OTSLine3D class
        Set<Point3d> result = new LinkedHashSet<>();
        Point2d prevA = null;
        for (Iterator<Point3d> aIterator = a.getPoints(); aIterator.hasNext();)
        {
            Point2d nextA = aIterator.next().project();
            if (null != prevA)
            {
                Point2d prevB = null;
                for (Iterator<Point3d> bIterator = b.getPoints(); bIterator.hasNext();)
                {
                    Point2d nextB = bIterator.next().project();
                    if (null != prevB)
                    {
                        Point2d intersection = Point2d.intersectionOfLineSegments(prevA, nextA, prevB, nextB);
                        if (null != intersection)
                        {
                            result.add(new Point3d(intersection, 0));
                        }
                    }
                    prevB = nextB;
                }
            }
            prevA = nextA;
        }
        return result;
    }

    @Override
    public final void notify(final EventInterface event) throws RemoteException
    {
        // System.out.println("received event " + event);
        this.collectedEvents.add(event);
    }

}