diff --git a/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/ExecuteConfig.kt b/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/ExecuteConfig.kt
index 693fcee517..2aada02ca7 100644
--- a/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/ExecuteConfig.kt
+++ b/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/ExecuteConfig.kt
@@ -171,9 +171,7 @@ fun frontend(
     parseContext.architecture = cConfig.architecture
   }
 
-  val xcfa = getXcfa(config, parseContext, logger, uniqueLogger)
-
-  xcfa.optimizeFurther(ApplyWitnessPasses(parseContext, logger))
+  val xcfa = getXcfa(config, parseContext, logger, uniqueLogger).optimizeFurther(ApplyWitnessPasses(parseContext, logger))
 
   val mcm =
     if (config.inputConfig.catFile != null) {
diff --git a/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/witnesstransformation/ApplyWitnessPass.kt b/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/witnesstransformation/ApplyWitnessPass.kt
index d334c2b4c8..2ae1e5d792 100644
--- a/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/witnesstransformation/ApplyWitnessPass.kt
+++ b/subprojects/xcfa/xcfa-cli/src/main/java/hu/bme/mit/theta/xcfa/cli/witnesstransformation/ApplyWitnessPass.kt
@@ -15,9 +15,11 @@
  */
 package hu.bme.mit.theta.xcfa.cli.witnesstransformation
 
+import com.charleskorn.kaml.YamlPathSegment.Root.location
 import hu.bme.mit.theta.c2xcfa.CMetaData
 import hu.bme.mit.theta.frontend.ParseContext
 import hu.bme.mit.theta.xcfa.model.XcfaEdge
+import hu.bme.mit.theta.xcfa.model.XcfaLocation
 import hu.bme.mit.theta.xcfa.model.XcfaProcedureBuilder
 import hu.bme.mit.theta.xcfa.passes.ProcedurePass
 import hu.bme.mit.theta.xcfa.witnesses.*
@@ -92,7 +94,6 @@ class ApplyWitnessPass(parseContext: ParseContext) : ProcedurePass {
     // collect edges corresponding to recurrence location, cycle and follow waypoints
     val edgesOfWaypoints = mutableSetOf<XcfaEdge>()
     for (wayPoint in allWaypoints) {
-      println("witness wp: ${wayPoint.location.line}, ${wayPoint.location.column}")
       for (edge in builder.getEdges()) {
         val edgeMetadata = (edge.metadata as? CMetaData)
         if (edgeMetadata == null) {
@@ -119,12 +120,35 @@ class ApplyWitnessPass(parseContext: ParseContext) : ProcedurePass {
     }
     edgesToKeep.addAll(edgesOfWaypoints)
 
-    val edgesToDelete = builder.getEdges().filter { edge -> edge !in edgesOfWaypoints }.toMutableSet()
+    val edgesToDelete = builder.getEdges().filter { edge -> edge !in edgesToKeep }.toMutableSet()
 
     for(edge in edgesToDelete) {
       builder.removeEdge(edge)
     }
 
+    val locsToDelete = mutableSetOf<XcfaLocation>()
+
+    var foundOne = true
+    while(foundOne) {
+      foundOne = false
+      for(location in builder.getLocs()) {
+        if(!location.initial && (location.incomingEdges.isEmpty() || location.outgoingEdges.isEmpty())) {
+          foundOne = true
+          locsToDelete.add(location)
+          break
+        }
+      }
+      for(loc in locsToDelete) {
+        for (edge in loc.incomingEdges) {
+          builder.removeEdge(edge)
+        }
+        for (edge in loc.outgoingEdges) {
+          builder.removeEdge(edge)
+        }
+        builder.removeLoc(loc)
+      }
+    }
+
     return builder
   }
 
@@ -154,7 +178,11 @@ class ApplyWitnessPass(parseContext: ParseContext) : ProcedurePass {
       for (i in edges) {
         for (j in edges) {
           if (dist[i to k]!! != inf && dist[k to j]!! != inf) {
-            dist[i to j] = minOf(dist[i to j]!!, dist[i to k]!! + dist[k to j]!!)
+            if (dist[i to j]!! == inf) {
+              dist[i to j] = dist[i to k]!! + dist[k to j]!!
+            } else {
+              dist[i to j] = minOf(dist[i to j]!!, dist[i to k]!! + dist[k to j]!!)
+            }
           }
         }
       }