Enhance path finding using path cost

PamhagenSysCtrl/Helpers/PamhagenGraph.cs

@@ -87,19 +87,19 @@ namespace PamhagenSysCtrl.Helpers {
             CreatePipe(MP1, x1);
             CreatePipe(MP2, x2);
 
-            var v1 = CreateValve(1, mw1X, mpY + 70);
+            var v1 = CreateValve(1, mw1X, mpY + 80);
-            var v2 = CreateValve(2, mw2X, mpY + 70);
+            var v2 = CreateValve(2, mw2X, mpY + 80);
-            var v3 = CreateValve(3, mw2X + 30, mpY + 70);
+            var v3 = CreateValve(3, mw2X + 30, mpY + 80);
-            var v4 = CreateValve(4, mw1X - 30, mpY + 70);
+            var v4 = CreateValve(4, mw1X - 30, mpY + 80);
 
             CreatePipe(x1, v1);
             CreatePipe(x1, v4);
             CreatePipe(x2, v2);
             CreatePipe(x2, v3);
 
-            var x3 = new PipeJoin(mw1X, mpY + 100);
+            var x3 = new PipeJoin(mw1X, ltg1Y);
             var x4 = new PipeJoin(x1X, ltg1Y);
-            var x5 = new PipeJoin(mw2X, mpY + 130);
+            var x5 = new PipeJoin(mw1X - 30, ltg2Y);
             var x6 = new PipeJoin(x1X - 30, ltg2Y);
 
             CreatePipe(v1, x3, true);
@@ -413,6 +413,23 @@ namespace PamhagenSysCtrl.Helpers {
             return new($"Tank {n}", x, y, capacity, () => App.Plant?.TankFillLevels[n - 1] ?? FillState.Unknown);
         }
 
+        private int PathCost(Path p) {
+            return
+                1000 * p.Hops.Count(h => h.Node is Valve) +
+                (p.Hops.Any(h => h.Node is Valve v && v.Nr == 15) && !p.Hops.Any(h => h.Node is Valve v && (v.Nr == 16 || v.Nr == 20)) ? 10 : 0) +
+                (p.Hops.Any(h => h.Node is Valve v && v.Nr == 11) && !p.Hops.Any(h => h.Node is Valve v && (v.Nr == 14 || v.Nr == 21)) ? 10 : 0) +
+                (p.Hops.Any(h => h.Node is Valve v && v.Nr == 6) ? 10 : 0) +
+                p.Bends;
+        }
+
+        public Path? GetFreePath(IEnumerable<INode> points, bool overrideStart = false) {
+            if (overrideStart) {
+                return GetPath(points, (n) => n is not ISink && (n is not Valve v || !v.LockedForPaths.Any(p => p.Start != points.First())), PathCost);
+            } else {
+                return GetPath(points, (n) => n is not ISink && (n is not Valve v || !v.IsLocked), PathCost);
+            }
+        }
+
         public void TraverseSubgraph(INode start, Func<INode, bool> invalid, HashSet<IEdge>? _visited = null, Action<INode, bool, IEdge>? edgeAction = null, Action<Valve>? valveAction = null, bool reverse = false) {
             var visited = _visited ?? [];
             if (start is Valve v) {

PamhagenSysCtrl/Helpers/Pipeline/Graph.cs

@@ -54,54 +54,52 @@ namespace PamhagenSysCtrl.Helpers.Pipeline {
             return p;
         }
 
-        public Path? GetFreePath(IEnumerable<INode> points, bool overrideStart = false) {
+        public Path? GetPath(IEnumerable<INode> points, Func<INode, bool>? valid = null, Func<Path, int>? cost = null) {
-            if (overrideStart) {
-                return GetPath(points, (n) => n is not ISink && (n is not Valve v || !v.LockedForPaths.Any(p => p.Start != points.First())));
-            } else {
-                return GetPath(points, (n) => n is not ISink && (n is not Valve v || !v.IsLocked));
-            }
-        }
-
-        public Path? GetPath(IEnumerable<INode> points, Func<INode, bool>? valid = null) {
             if (points.Count() < 2) return null;
             var start = points.First();
-            var path = new Path(start, []);
+            List<Path> paths = [new(start, [])];
             foreach (var end in points.Skip(1)) {
-                if (valid != null && start != points.First() && !valid(start)) {
+                if (valid != null && start != points.First() && !valid(start))
                     return null;
-                } else if (GetPath(start, end, valid, visited: [.. path.Hops.Select(h => h.Node)]) is not Path p) {
+
-                    return null;
+                var newPaths = new List<Path>();
-                } else {
+                foreach (var path in paths) {
-                    path.Hops = [.. path.Hops, .. p.Hops];
+                    foreach (var subPath in GetPaths(start, end, visited: [.. path.Hops.Select(h => h.Node)], valid)) {
-                    start = end;
+                        newPaths.Add(new(path.Start, [.. path.Hops, .. subPath.Hops]));
+                    }
                 }
+                if (newPaths.Count == 0)
+                    return null;
+
+                paths = newPaths;
+                start = end;
+            }
+            if (paths.Count == 0) {
+                return null;
+            } else {
+                cost ??= p => 0;
+                return paths.OrderBy(cost).First();
             }
-            return path;
         }
 
-        public Path? GetPath(INode start, INode end, Func<INode, bool>? valid = null, HashSet<INode>? visited = null) {
+        public IEnumerable<Path> GetPaths(INode start, INode end, HashSet<INode>? visited = null, Func<INode, bool>? valid = null) {
             if (!Nodes.Contains(start) || !Nodes.Contains(end)) throw new ArgumentException("Start/end node not contained in graph");
             valid ??= a => true;
             visited = [.. visited ?? [], start];
             List<(IEdge, INode)> hops = [];
-            Path? path = null;
             foreach (var o in start.Outputs) {
                 var e = o.IsTwoWay && o.End == start ? o.Start : o.End;
                 if (e == end) {
-                    return new(start, [(o, e)]);
+                    yield return new(start, [(o, e)]);
+                    continue;
                 } else if (!valid(e) || visited.Contains(e)) {
                     continue;
                 }
 
-                var p = GetPath(e, end, valid, visited);
+                foreach (var p in GetPaths(e, end, visited, valid)) {
-                if (p != null) {
+                    yield return new(start, [(o, e), .. p.Hops]);
-                    var p2 = new Path(start, [(o, e), .. p.Value.Hops]);
-                    if (path == null || p2.Bends < path.Value.Bends || (p2.Bends == path.Value.Bends && p2.Length < path.Value.Length)) {
-                        path = p2;
-                    }
                 }
             }
-            return path != null ? new(start, path.Value.Hops) : null;
         }
 
         public void Draw(Canvas canvas) {

PamhagenSysCtrl/Helpers/Pipeline/Path.cs

@@ -3,7 +3,7 @@ namespace PamhagenSysCtrl.Helpers.Pipeline {
             INode Start,
             IEnumerable<(IEdge Edge, INode Node)> Hops)
     {
-        public readonly double Bends => Hops.Sum(h => (h.Node is not Valve && h.Node is not PipeJoin ? 1 : 0) + (h.Edge.Start.CenterX != h.Edge.End.CenterX && h.Edge.Start.CenterY != h.Edge.End.CenterY ? 1 : 0));
+        public readonly int Bends => Hops.Sum(h => (h.Node is not Valve && h.Node is not PipeJoin ? 1 : 0) + (h.Edge.Start.CenterX != h.Edge.End.CenterX && h.Edge.Start.CenterY != h.Edge.End.CenterY ? 1 : 0));
         public readonly double Length => Hops.Sum(h => Math.Abs(h.Edge.Start.CenterX - h.Edge.End.CenterX) + Math.Abs(h.Edge.Start.CenterY - h.Edge.End.CenterY));
     }
 }