diff --git a/SchematicDrawing/Enums.cs b/SchematicDrawing/Enums.cs
index b5892c6b0..d896297bb 100644
--- a/SchematicDrawing/Enums.cs
+++ b/SchematicDrawing/Enums.cs
@@ -11,6 +11,7 @@ namespace SchematicDrawing
     public static class Const
     {
         public const int GridSize = 10;
+        public const int SecondaryStart = 10000;
         public const int Vacuum = 1000000;
     }
 
diff --git a/SchematicDrawing/GNode.cs b/SchematicDrawing/GNode.cs
index eeb4a916e..d9e585e39 100644
--- a/SchematicDrawing/GNode.cs
+++ b/SchematicDrawing/GNode.cs
@@ -1,23 +1,35 @@
-using System;
+///
+/// Copyright (c) 2020 Sensus Slovensko a.s.
+///
+using System;
 using System.Collections.Generic;
 
 namespace SchematicDrawing
 {
     public class GNode
     {
+        /// Graph node properties
         public IDrawingItem Item;
         public int NodeId;
         public readonly IList<GEdge> Edges;
 
+        /// Distance from a start node
         public int Dist;
-        public bool Visited;
 
+        /// Minimal heap index
+        public int HeapIx;
+
+        /// <summary>
+        /// Constructor
+        /// </summary>
+        /// <param name="item">Item</param>
+        /// <param name="nodeId"Node ID></param>
         public GNode(IDrawingItem item, int nodeId)
         {
-            this.Item = item;
-            this.NodeId = nodeId;
-            this.Edges = new List<GEdge>();
-            Reset();
+            Item = item;
+            NodeId = nodeId;
+            Edges = new List<GEdge>();
+            Dist = int.MaxValue;
         }
 
         public void AddEdge(GEdge edge)
@@ -25,15 +37,16 @@ namespace SchematicDrawing
             Edges.Add(edge);
         }
 
-        public void Reset()
+        public GNode Reset(int heapIx, int dist = int.MaxValue)
         {
-            Visited = false;
-            Dist = int.MaxValue;
+            HeapIx = heapIx;
+            Dist = dist;
+            return this;
         }
 
         public override string ToString()
         {
-            return string.Format("{0}/{1} {2} dist={3}", Item.Name, NodeId, Visited ? "V" : "", Dist);
+            return string.Format("{0}/{1} dist={2}", Item.Name, NodeId, Dist);
         }
     }
 }
diff --git a/SchematicDrawing/Graph.cs b/SchematicDrawing/Graph.cs
index 061b35725..05cd6421b 100644
--- a/SchematicDrawing/Graph.cs
+++ b/SchematicDrawing/Graph.cs
@@ -1,4 +1,8 @@
-using System;
+///
+/// Copyright (c) 2020 Sensus Slovensko a.s.
+///
+using System;
+using System.Diagnostics;
 using System.Collections.Generic;
 
 namespace SchematicDrawing
@@ -6,25 +10,47 @@ namespace SchematicDrawing
     public class Graph
     {
         IList<GNode> nodes;
+        GNode startNode;
+        int secondaryStartNodesCount;
+
+        MinHeap minHeap;
         GNode currentNode;
 
         public Graph()
         {
             nodes = new List<GNode>();
+            startNode = null;
+            secondaryStartNodesCount = 0;
+            minHeap = new MinHeap();
         }
 
         public void Clear()
         {
             nodes.Clear();
+            startNode = null;
+            secondaryStartNodesCount = 0;
         }
 
         /// <summary>
         /// Add a node to the list of graph nodes
         /// </summary>
         /// <param name="node">Graph node</param>
-        public void AddNode(GNode node)
+        public void AddNode(GNode node, bool isStartNode = false, bool isSecondaryStartNode = false)
         {
-            nodes.Add(node);
+            if (isStartNode && startNode == null)
+            {
+                startNode = node;
+                nodes.Insert(0, node);
+            }
+            else if (isStartNode || isSecondaryStartNode)
+            {
+                secondaryStartNodesCount++;
+                nodes.Insert(startNode != null ? 1 : 0, node);
+            }
+            else
+            {
+                nodes.Add(node);
+            }
         }
 
         /// <summary>
@@ -47,44 +73,30 @@ namespace SchematicDrawing
         /// </summary>
         /// <param name="startNode">Start node</param>
         /// <param name="secondaryStartNodes">List of secondary start nodes</param>
-        public void RunDijkstraAlgo(GNode startNode, IList<GNode> secondaryStartNodes = null)
+        public void RunDijkstraAlgo()
         {
-            if (nodes == null || startNode == null || !nodes.Contains(startNode)) return;
+            if (nodes == null || startNode == null) return;
 
             /// Reset all nodes
-            foreach (var node in nodes) node.Reset();
+            minHeap.ResetAndBuild(nodes, secondaryStartNodesCount);
 
-            startNode.Dist = 0;
-            if (secondaryStartNodes != null) foreach (var ssn in secondaryStartNodes) ssn.Dist = 10000;
-            currentNode = startNode;
-
-            int minDistance = int.MaxValue;
+            currentNode = minHeap.ExtractMinNode();
             do
             {
                 /// Consider all unvisited neighbors of the current node
                 foreach (var e in currentNode.Edges)
                 {
                     int distViaCurrNode = (e.Dist == int.MaxValue) ? int.MaxValue : currentNode.Dist + e.Dist;
-                    if (!e.EndNode.Visited && distViaCurrNode < e.EndNode.Dist)
+                    if (distViaCurrNode < e.EndNode.Dist)
                     {
                         e.EndNode.Dist = distViaCurrNode;
+                        minHeap.SiftUp(e.EndNode.HeapIx);
                     }
                 }
-                currentNode.Visited = true;
 
-                /// Find new current node from all unvisited nodes
-                currentNode = null;
-                minDistance = int.MaxValue;
-                foreach (var n in nodes)
-                {
-                    if (!n.Visited && n.Dist < minDistance)
-                    {
-                        minDistance = n.Dist;
-                        currentNode = n;
-                    }
-                }
+                currentNode = minHeap.ExtractMinNode();
             }
-            while (minDistance < int.MaxValue && currentNode != null);
+            while (currentNode.Dist < int.MaxValue && currentNode != null);
         }
     }
 }
diff --git a/SchematicDrawing/MinHeap.cs b/SchematicDrawing/MinHeap.cs
new file mode 100644
index 000000000..afd1f22a4
--- /dev/null
+++ b/SchematicDrawing/MinHeap.cs
@@ -0,0 +1,134 @@
+///
+/// Copyright (c) 2020 Sensus Slovensko a.s.
+///
+using System;
+using System.Collections.Generic;
+
+namespace SchematicDrawing
+{
+    public class MinHeap
+    {
+        GNode[] heap;   /// Minimum heap, a binary tree in form of an array
+        int heapSize;   /// Heap size (not necessarily a size of the array heap[])
+
+        /// <summary>
+        /// Constructor
+        /// </summary>
+        public MinHeap()
+        {
+            heap = null;
+            heapSize = 0;
+        }
+
+        /// <summary>
+        /// Reset nodes and build the initial minimum heap tree.
+        /// 
+        /// First node on the list is the start node (mostly the main water tank),
+        /// followed by 'secondaryStartNodesCount' secondary start nodes (typically
+        /// tanks above scales), followed by all remaining nodes of the drawing.
+        /// In this way the heap is heapified from the beginning.
+        /// </summary>
+        /// <param name="nodes">Graph nodes</param>
+        /// <param name="secondaryStartNodesCount">Number of secondary start nodes</param>
+        public void ResetAndBuild(IList<GNode> nodes, int secondaryStartNodesCount)
+        {
+            if (nodes == null || nodes.Count == 0) return;
+
+            if (heapSize != nodes.Count)
+            {
+                heapSize = nodes.Count;
+                heap = new GNode[nodes.Count];
+            }
+
+            heap[0] = nodes[0].Reset(0, 0);
+            for (int i = 1; i <= secondaryStartNodesCount; i++)
+            {
+                heap[i] = nodes[i].Reset(i, Const.SecondaryStart);
+            }
+            for (int i = secondaryStartNodesCount + 1; i < nodes.Count; i++)
+            {
+                heap[i] = nodes[i].Reset(i, int.MaxValue);
+            }
+        }
+
+        /// <summary>
+        /// Returns the node with minimal distance at the top of the heap binary tree.
+        /// This node is removed from the heap, replaced by the last node and the heap is 'Heapified'.
+        /// </summary>
+        /// <returns>Node with minimal distance</returns>
+        public GNode ExtractMinNode()
+        {
+            GNode minNode = heap[0];
+            if (--heapSize > 0)
+            {
+                heap[0] = heap[heapSize];   /// Swap with the last one
+                heap[0].HeapIx = 0;
+                heap[heapSize] = minNode;
+                SiftDown(0);                /// Heapify the heap, sift down the swapped (formerly the last) item
+            }
+            return minNode;
+        }
+
+        /// <summary>
+        /// Sift up the node in the minimum heap binary tree.
+        /// Function is called when distance of a node in the graph is updated.
+        /// As the distance is always decreased, the node sifts up in the tree.
+        /// </summary>
+        /// <param name="heapIx">Index of the node to sift up</param>
+        public void SiftUp(int heapIx)
+        {
+            if (heapIx == 0) return;
+            int parentIx = (heapIx - 1) / 2;
+            if (heap[heapIx].Dist < heap[parentIx].Dist)
+            {
+                SwapNodes(heapIx, parentIx);
+                SiftUp(parentIx);
+            }
+        }
+
+        /// <summary>
+        /// Sift down the node in the minimum heap binary tree.
+        /// Function is called when node at the top of the tree is extracted and replaced
+        /// by the last node. This last node is then sift down according to its distance.
+        /// </summary>
+        /// <param name="heapIx">Index of the node to sift down</param>
+        void SiftDown(int heapIx)
+        {
+            int leftIx = 2 * heapIx + 1;
+            if (leftIx < heapSize)
+            {
+                int minIx = heapIx;
+                if (heap[leftIx].Dist < heap[minIx].Dist)
+                {
+                    minIx = leftIx;
+                }
+
+                int rightIx = 2 * heapIx + 2;
+                if (rightIx < heapSize && heap[rightIx].Dist < heap[minIx].Dist)
+                {
+                    minIx = rightIx;
+                }
+
+                if (minIx != heapIx)
+                {
+                    SwapNodes(heapIx, minIx);
+                    SiftDown(minIx);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Swap two nodes in the minimum heap binary tree.
+        /// </summary>
+        /// <param name="ix1">Index of the 1st node</param>
+        /// <param name="ix2">Index of the 2nd node</param>
+        void SwapNodes(int ix1, int ix2)
+        {
+            GNode swap = heap[ix1];
+            heap[ix1] = heap[ix2];
+            heap[ix1].HeapIx = ix1;
+            heap[ix2] = swap;
+            heap[ix2].HeapIx = ix2;
+        }
+    }
+}
diff --git a/SchematicDrawing/SchematicDrawing.csproj b/SchematicDrawing/SchematicDrawing.csproj
index 3a65d2203..6998cc7d8 100644
--- a/SchematicDrawing/SchematicDrawing.csproj
+++ b/SchematicDrawing/SchematicDrawing.csproj
@@ -56,6 +56,7 @@
     <Compile Include="IDrawingItemWithMeasuredVal.cs" />
     <Compile Include="IRouteBasedDrawingItem.cs" />
     <Compile Include="IDrawingItemWithSetpoint.cs" />
+    <Compile Include="MinHeap.cs" />
     <Compile Include="Pipe.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
     <Compile Include="Properties\Resources.Designer.cs">
diff --git a/SchematicDrawing/SchematicDrawingCtrl.cs b/SchematicDrawing/SchematicDrawingCtrl.cs
index 991250212..51d3755f1 100644
--- a/SchematicDrawing/SchematicDrawingCtrl.cs
+++ b/SchematicDrawing/SchematicDrawingCtrl.cs
@@ -7,6 +7,7 @@ using System.Drawing;
 using System.Windows.Forms;
 using Dirichlet.Numerics;
 using System.Numerics;
+using System.Diagnostics;
 
 namespace SchematicDrawing
 {
@@ -54,7 +55,8 @@ namespace SchematicDrawing
                             Utils.UpdateGraphEdges(route, it as IRouteBasedDrawingItem);
                         }
                     }
-                    graph.RunDijkstraAlgo(startNode, secondaryStartNodes);
+
+                    graph.RunDijkstraAlgo();
                     Redraw();
                 }
             }
@@ -231,8 +233,6 @@ namespace SchematicDrawing
         IList<IDrawingItem> items;
 
         Graph graph;
-        GNode startNode;
-        IList<GNode> secondaryStartNodes;
 
         ///
         /// Rectangles of elements of drawn items
@@ -287,8 +287,6 @@ namespace SchematicDrawing
             pipesXL = new List<Pipe>();
 
             graph = new Graph();
-            startNode = null;
-            secondaryStartNodes = new List<GNode>();
 
             ItemElementRectangles = new List<ItemElementRect>();
 
@@ -313,7 +311,7 @@ namespace SchematicDrawing
                     {
                         GNode gnode = new GNode(item, nid);
                         item.GNodes.Add(gnode);
-                        graph.AddNode(gnode);
+                        graph.AddNode(gnode, dshape.Shape == Shape.Tank, dshape.Shape == Shape.Scale);
                     }
 
                     foreach (var edge in dshape.Edges)
@@ -324,16 +322,6 @@ namespace SchematicDrawing
                             (item as IRouteBasedDrawingItem).EdgesToSet.Add(gedge);
                         }
                     }
-
-                    if (dshape.Shape == Shape.Tank && item.GNodes.Count > 0)
-                    {
-                        startNode = item.GNodes[0];
-                    }
-
-                    if (dshape.Shape == Shape.Scale && item.GNodes.Count > 0)
-                    {
-                        secondaryStartNodes.Add(item.GNodes[0]);
-                    }
                 }
             }
             
@@ -344,8 +332,6 @@ namespace SchematicDrawing
             items.Clear();
             Name2Item.Clear();
             graph.Clear();
-            startNode = null;
-            secondaryStartNodes.Clear();
             Redraw();
         }
         public IList<IDrawingItem> GetItems()
@@ -388,11 +374,6 @@ namespace SchematicDrawing
             return tempPipe;
         }
 
-        public void RunDijkstraAlgo()
-        {
-            graph.RunDijkstraAlgo(startNode, secondaryStartNodes);
-        }
-
         void PrepareFontsPensBrushesEtc()
         {
             /// Font for drawing labels
@@ -474,6 +455,10 @@ namespace SchematicDrawing
         /// <param name="e">PaintEventArgs</param>
         void SchematicDrawingCtrl_Paint(object sender, PaintEventArgs e)
         {
+            /// Profiling
+            //stopwatch.Reset();
+            //stopwatch.Start();
+
             e.Graphics.Clear(Color.White);
 
             ItemElementRectangles.Clear();
@@ -669,15 +654,37 @@ namespace SchematicDrawing
                     }
                 }
             }
+
+            /// Profiling
+            //stopwatch.Stop();
+            //if (firstTime)
+            //{
+            //    firstTime = false;
+            //}
+            //else if (totalTimes < long.MaxValue)
+            //{
+            //    totalMs += stopwatch.ElapsedMilliseconds;
+            //    totalTicks += stopwatch.ElapsedTicks;
+            //    totalTimes++;
+            //    Console.WriteLine("Average Paint duration is {0} ms = {1} ticks", totalMs / totalTimes, totalTicks / totalTimes);
+            //}
         }
 
+        /// Profiling
+        //Stopwatch stopwatch = new Stopwatch();
+        //bool firstTime = true;
+        //long totalMs = 0;
+        //long totalTicks = 0;
+        //long totalTimes = 0;
+
+
         /// <summary>
-        /// Draw a pipe
+        /// Draw a pipe.
         /// </summary>
         /// <param name="e">PaintEventArgs</param>
-        /// <param name="pipe">Edge string</param>
-        /// <param name="pipeSize">Edge size</param>
-        /// <param name="isWaterOpen">true = filled, false = empty</param>
+        /// <param name="pipe">Pipe object</param>
+        /// <param name="distance">Distance from source of water for coloring</param>
+        /// <param name="highlighted">true = highlighted (edit mode)</param>
         void DrawPipe(PaintEventArgs e, Pipe pipe, int distance, bool highlighted = false)
         {
             int x1, y1, x2, y2;
@@ -689,6 +696,13 @@ namespace SchematicDrawing
             }
         }
 
+        /// <summary>
+        /// Draw an item (body).
+        /// </summary>
+        /// <param name="e">PaintEventArgs</param>
+        /// <param name="item">IDrawingItem object</param>
+        /// <param name="dsh">DrawingShape</param>
+        /// <returns>Rectangle where item body is drawn</returns>
         Rectangle DrawItem(PaintEventArgs e, IDrawingItem item, DrawingShape dsh)
         {
             bool routeBit = false;
@@ -810,9 +824,10 @@ namespace SchematicDrawing
         /// <param name="x">X-coordinate</param>
         /// <param name="y">Y-coordinate</param>
         /// <param name="elements">Bitfield of elements to be searched for</param>
-        /// <param name="itemNameOrEdge">Found component or edge or null</param>
-        /// <param name="nodeId">Fount node id or 0</param>
-        /// <returns>Element enum value (Element.None if nothing found)</returns>
+        /// <param name="item">Found item or null (in case of body, node, label, measured value or setpoint)</param>
+        /// <param name="pipe">Found pipe or null (in case of a pipe)</param>
+        /// <param name="nodeId">Found node id or 0 (in case of an item node)</param>
+        /// <returns>Element enum value (Element.None when nothing was found)</returns>
         public Element FindElement(int x, int y, Element elements, out IDrawingItem item, out Pipe pipe, out int nodeId)
         {
             const int NodeTolerance = 5;
@@ -848,7 +863,7 @@ namespace SchematicDrawing
         }
 
         /// <summary>
-        /// 
+        /// Searches for an item body, label, measured value or setpoint.
         /// </summary>
         /// <param name="x">X coordinate</param>
         /// <param name="y">Y coordinate</param>