package org.opentrafficsim.core.network;
   
   import java.io.Serializable;
   import java.util.HashMap;
   import java.util.HashSet;
   import java.util.LinkedHashSet;
   import java.util.Map;
   import java.util.Set;
   
  import javax.media.j3d.BoundingSphere;
  import javax.media.j3d.Bounds;
  import javax.vecmath.Point3d;
  
  import org.djunits.value.vdouble.scalar.Angle;
  import org.djunits.value.vdouble.scalar.Direction;
  import org.opentrafficsim.core.geometry.OTSPoint3D;
  import org.opentrafficsim.core.gtu.GTUType;
  
  import nl.tudelft.simulation.dsol.animation.Locatable;
  import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
  import nl.tudelft.simulation.immutablecollections.ImmutableHashSet;
  import nl.tudelft.simulation.immutablecollections.ImmutableSet;
  import nl.tudelft.simulation.language.Throw;
  import nl.tudelft.simulation.language.d3.DirectedPoint;
  
  /**
   * The Node is a point with an id. It is used in the network to connect Links.
   * <p>
   * Copyright (c) 2013-2018 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: 2018-09-19 13:55:45 +0200 (Wed, 19 Sep 2018) $, @version $Revision: 4006 $, by $Author: averbraeck $,
   * initial version Aug 19, 2014 <br>
   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
   * @author <a href="http://www.citg.tudelft.nl">Guus Tamminga</a>
   */
  public class OTSNode implements Node, Locatable, Serializable
  {
      /** */
      private static final long serialVersionUID = 20150722L;
  
      /** the Network. */
      private final Network network;
  
      /** The node id. */
      private final String id;
  
      /** The point. */
      private final OTSPoint3D point;
  
      /** The 3D direction. "East" is 0 degrees. "North" is 90 degrees (1/2 pi radians). */
      private final Direction direction;
  
      /** The slope as an angle. Horizontal is 0 degrees. */
      private final Angle slope;
  
      /** The links connected to the Node. */
      private final Set<Link> links = new LinkedHashSet<>();
  
      /** The cached immutable set of links to return. */
      private ImmutableSet<Link> cachedLinks = null;
  
      /**
       * Map with connections per GTU type. When this map is null, the all connections that are possible for the GTU type will be
       * included, with the exception of the U-turn. When exceptions are taken into account, the map has to be completely filled
       * as it replaces the default. The map gives per GTU type a map of incoming links that are connected to outgoing links,
       * which are stored in a Set.
       */
      private Map<GTUType, Map<Link, Set<Link>>> connections = null;
  
      /**
       * Construction of a Node.
       * @param network the network.
       * @param id the id of the Node.
       * @param point the point with usually an x and y setting.
       * @param direction the 3D direction. "East" is 0 degrees. "North" is 90 degrees (1/2 pi radians).
       * @param slope the slope as an angle. Horizontal is 0 degrees.
       * @throws NetworkException if node already exists in the network, or if name of the node is not unique.
       */
      public OTSNode(final Network network, final String id, final OTSPoint3D point, final Direction direction, final Angle slope)
              throws NetworkException
      {
          Throw.whenNull(network, "network cannot be null");
          Throw.whenNull(id, "id cannot be null");
          Throw.whenNull(point, "point cannot be null");
          Throw.whenNull(direction, "direction cannot be null");
          Throw.whenNull(slope, "slope cannot be null");
  
          this.network = network;
          this.id = id;
          this.point = point;
          this.direction = direction;
          this.slope = slope;
  
          this.network.addNode(this);
      }
  
      /**
      * Construction of a Node.
      * @param network the network.
      * @param id the id of the Node.
      * @param point the point with usually an x and y setting.
      * @throws NetworkException if node already exists in the network, or if name of the node is not unique.
      */
     public OTSNode(final Network network, final String id, final OTSPoint3D point) throws NetworkException
     {
         this(network, id, point, Direction.ZERO, Angle.ZERO);
     }
 
     /** {@inheritDoc} */
     @Override
     public final Network getNetwork()
     {
         return this.network;
     }
 
     /**
      * @return node id.
      */
     @Override
     public final String getId()
     {
         return this.id;
     }
 
     /**
      * @return point.
      */
     @Override
     public final OTSPoint3D getPoint()
     {
         return this.point;
     }
 
     /** {@inheritDoc} */
     @Override
     public final void addLink(final Link link)
     {
         this.links.add(link);
         this.cachedLinks = null; // invalidate the cache
     }
 
     /** {@inheritDoc} */
     @Override
     public final void removeLink(final Link link)
     {
         this.links.remove(link);
         this.cachedLinks = null; // invalidate the cache
     }
 
     /**
      * Add a single connection for a GTU type to the connections map. The data structures will be created is it does not exist
      * yet.
      * @param gtuType the GTU type for which this connection is made
      * @param incomingLink the link that connects to this Node
      * @param outgoingLink the link that the GTU can use to depart from this Node when coming from the incoming link
      * @throws NetworkException in case one of the links is not (correctly) connected to this Node
      */
     public final void addConnection(final GTUType gtuType, final Link incomingLink, final Link outgoingLink)
             throws NetworkException
     {
         // ------------------------------------------- check consistency
         if (!this.links.contains(incomingLink))
         {
             throw new NetworkException(
                     "addConnection: incoming link " + incomingLink + " for node " + this + " not in links set");
         }
 
         if (!this.links.contains(outgoingLink))
         {
             throw new NetworkException(
                     "addConnection: outgoing link " + outgoingLink + " for node " + this + " not in links set");
         }
 
         if (!(incomingLink.getEndNode().equals(this) && incomingLink.getDirectionality(gtuType).isForwardOrBoth()
                 || incomingLink.getStartNode().equals(this) && incomingLink.getDirectionality(gtuType).isBackwardOrBoth()))
         {
             throw new NetworkException("addConnection: incoming link " + incomingLink + " not connected to node " + this
                     + " for GTU type " + gtuType);
         }
 
         if (!(outgoingLink.getStartNode().equals(this) && outgoingLink.getDirectionality(gtuType).isForwardOrBoth()
                 || outgoingLink.getEndNode().equals(this) && outgoingLink.getDirectionality(gtuType).isBackwardOrBoth()))
         {
             throw new NetworkException("addConnection: outgoing link " + outgoingLink + " not connected to node " + this
                     + " for GTU type " + gtuType);
         }
 
         // ------------------------------------------- make datasets if needed
         if (this.connections == null)
         {
             this.connections = new HashMap<>();
         }
 
         if (!this.connections.containsKey(gtuType))
         {
             this.connections.put(gtuType, new HashMap<>());
         }
 
         Map<Link, Set<Link>> gtuMap = this.connections.get(gtuType);
         if (!gtuMap.containsKey(incomingLink))
         {
             gtuMap.put(incomingLink, new HashSet<>());
         }
 
         // ------------------------------------------- add the connection
         gtuMap.get(incomingLink).add(outgoingLink);
     }
 
     /**
      * Add a set of connections for a GTU type to the connections map. The data structures will be created if it does not exist
      * yet.
      * @param gtuType the GTU type for which this connection is made
      * @param incomingLink the link that connects to this Node
      * @param outgoingLinks a set of links that the GTU can use to depart from this Node when coming from the incoming link
      * @throws NetworkException in case one of the links is not (correctly) connected to this Node
      */
     public final void addConnections(final GTUType gtuType, final Link incomingLink, final Set<Link> outgoingLinks)
             throws NetworkException
     {
         // ------------------------------------------- check consistency
         if (!this.links.contains(incomingLink))
         {
             throw new NetworkException(
                     "addConnections: incoming link " + incomingLink + " for node " + this + " not in links set");
         }
 
         if (!this.links.containsAll(outgoingLinks))
         {
             throw new NetworkException(
                     "addConnections: outgoing links " + outgoingLinks + " for node " + this + " not all in links set");
         }
 
         if (!((incomingLink.getEndNode().equals(this) && incomingLink.getDirectionality(gtuType).isForwardOrBoth())
                 || (incomingLink.getStartNode().equals(this) && incomingLink.getDirectionality(gtuType).isBackwardOrBoth())))
         {
             throw new NetworkException("addConnections: incoming link " + incomingLink + " not connected to node " + this
                     + " for GTU type " + gtuType);
         }
 
         for (Link outgoingLink : outgoingLinks)
         {
             if (!((outgoingLink.getStartNode().equals(this) && outgoingLink.getDirectionality(gtuType).isForwardOrBoth())
                     || (outgoingLink.getEndNode().equals(this) && outgoingLink.getDirectionality(gtuType).isBackwardOrBoth())))
             {
                 throw new NetworkException("addConnections: outgoing link " + outgoingLink + " not connected to node " + this
                         + " for GTU type " + gtuType);
             }
         }
 
         // ------------------------------------------- make datasets if needed
         if (this.connections == null)
         {
             this.connections = new HashMap<>();
         }
 
         if (!this.connections.containsKey(gtuType))
         {
             this.connections.put(gtuType, new HashMap<>());
         }
 
         Map<Link, Set<Link>> gtuMap = this.connections.get(gtuType);
         if (!gtuMap.containsKey(incomingLink))
         {
             gtuMap.put(incomingLink, new HashSet<>());
         }
 
         // ------------------------------------------- add the connections
         gtuMap.get(incomingLink).addAll(outgoingLinks);
     }
 
     /** {@inheritDoc} */
     @Override
     public final ImmutableSet<Link> getLinks()
     {
         if (this.cachedLinks == null)
         {
             this.cachedLinks = new ImmutableHashSet<>(this.links);
         }
         return this.cachedLinks;
     }
 
     /** {@inheritDoc} */
     @Override
     public final Set<Link> nextLinks(final GTUType gtuType, final Link prevLink) throws NetworkException
     {
         // ------------------------------------------- check consistency
         if (!this.links.contains(prevLink))
         {
             throw new NetworkException("nextLinks: incoming link " + prevLink + " for node " + this + " not in links set");
         }
 
         if (!(prevLink.getEndNode().equals(this) && prevLink.getDirectionality(gtuType).isForwardOrBoth()
                 || prevLink.getStartNode().equals(this) && prevLink.getDirectionality(gtuType).isBackwardOrBoth()))
         {
             throw new NetworkException(
                     "nextLinks: incoming link " + prevLink + " not connected to node " + this + " for GTU type " + gtuType);
         }
 
         Set<Link> result = new LinkedHashSet<>();
 
         // -------------------------------- check if explicit connections are present
         if (this.connections != null)
         {
             if (!this.connections.containsKey(gtuType))
             {
                 return result;
             }
             if (!this.connections.get(gtuType).containsKey(prevLink))
             {
                 return result;
             }
             result.addAll(this.connections.get(gtuType).get(prevLink)); // defensive copy
             return result;
         }
 
         // ----------------------------- defensive copy of the connections for the gtuType
         for (Link link : getLinks())
         {
             if ((link.getStartNode().equals(this) && link.getDirectionality(gtuType).isForwardOrBoth())
                     || (link.getEndNode().equals(this) && link.getDirectionality(gtuType).isBackwardOrBoth()))
             {
                 if (!link.equals(prevLink)) // no U-turn
                 {
                     result.add(link);
                 }
             }
         }
         return result;
     }
 
     /**
      * Note: this method does not take into account explicitly defined connections, as the previous link is not given. <br>
      * {@inheritDoc}
      */
     @Override
     public final boolean isDirectionallyConnectedTo(final GTUType gtuType, final Node toNode)
     {
         for (Link link : getLinks())
         {
             if (toNode.equals(link.getEndNode()) && link.getDirectionality(gtuType).isForwardOrBoth())
             {
                 return true;
             }
             if (toNode.equals(link.getStartNode()) && link.getDirectionality(gtuType).isBackwardOrBoth())
             {
                 return true;
             }
         }
         return false;
     }
 
     /** {@inheritDoc} */
     @Override
     public final Direction getDirection()
     {
         return this.direction;
     }
 
     /** {@inheritDoc} */
     @Override
     public final Angle getSlope()
     {
         return this.slope;
     }
 
     /** {@inheritDoc} */
     @Override
     @SuppressWarnings("checkstyle:designforextension")
     public DirectedPoint getLocation()
     {
         return this.point.getDirectedPoint();
     }
 
     /** {@inheritDoc} */
     @Override
     @SuppressWarnings("checkstyle:designforextension")
     public Bounds getBounds()
     {
         return new BoundingSphere(new Point3d(0.0d, 0.0d, 0.0d), 10.0d);
     }
 
     /** {@inheritDoc} */
     @Override
     @SuppressWarnings("checkstyle:designforextension")
     public String toString()
     {
         return "OTSNode [id=" + this.id + ", point=" + this.point + "]";
     }
 
     /** {@inheritDoc} */
     @Override
     @SuppressWarnings("checkstyle:designforextension")
     public int hashCode()
     {
         final int prime = 31;
         int result = 1;
         result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
         return result;
     }
 
     /** {@inheritDoc} */
     @Override
     @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
     public boolean equals(final Object obj)
     {
         if (this == obj)
             return true;
         if (obj == null)
             return false;
         if (getClass() != obj.getClass())
             return false;
         OTSNode other = (OTSNode) obj;
         if (this.id == null)
         {
             if (other.id != null)
                 return false;
         }
         else if (!this.id.equals(other.id))
             return false;
         return true;
     }
 
     /**
      * Clone the OTSode for e.g., copying a network.
      * @param newNetwork the new network to which the clone belongs
      * @param newSimulator the new simulator for this network
      * @return a clone of this object
      * @throws NetworkException in case the cloning fails
      */
     @SuppressWarnings("checkstyle:designforextension")
     public OTSNode clone1(final Network newNetwork, final SimulatorInterface.TimeDoubleUnit newSimulator)
             throws NetworkException
     {
         return new OTSNode(newNetwork, this.id, this.point, this.direction, this.slope);
     }
 
     /**
      * Complete the cloning of the OTSode for e.g., copying a network. Call this method after all the links have been
      * constructed in the new network.
      * @param newNetwork the new network to which the clone belongs
      * @param newSimulator the new simulator for this network
      * @param animation whether to clone the animation or not (could be used in subclasses)
      * @return the completed clone
      * @throws NetworkException in case the cloning fails
      */
     @SuppressWarnings("checkstyle:designforextension")
     public OTSNode clone2(final Network newNetwork, final SimulatorInterface.TimeDoubleUnit newSimulator,
             final boolean animation) throws NetworkException
     {
         @SuppressWarnings("unchecked")
         OTSNode clone = (OTSNode) newNetwork.getNode(this.id);
         if (this.connections != null)
         {
             Map<GTUType, Map<Link, Set<Link>>> newConnections = new HashMap<>();
             for (GTUType gtuType : this.connections.keySet())
             {
                 Map<Link, Set<Link>> newConnMap = new HashMap<>();
                 for (Link link : this.connections.get(gtuType).keySet())
                 {
                     Set<Link> newLinkSet = new HashSet<>();
                     for (Link setLink : this.connections.get(gtuType).get(link))
                     {
                         newLinkSet.add(newNetwork.getLink(setLink.getId()));
                     }
                     newConnMap.put(newNetwork.getLink(link.getId()), newLinkSet);
                 }
                 newConnections.put(gtuType, newConnMap);
             }
             clone.connections = newConnections;
         }
         return clone;
     }
 
 }