More tweaks.

src/main/java/edu/rpi/rair/DepthFirstPlanner.java

@@ -36,7 +36,7 @@ public class DepthFirstPlanner implements Planner {
 
         if (Operations.satisfies(background, start, goal)) {
             //Already satisfied. Do nothing. Return a set with an empty plan.
-            return Optional.of(Sets.with(Plan.newEmptyPlan(goal, background)));
+            return Optional.of(Sets.with(Plan.newEmptyPlan(start, background)));
         }
 
 

src/main/java/edu/rpi/rair/GoalTracker.java

@@ -3,7 +3,9 @@ package edu.rpi.rair;
 import com.naveensundarg.shadow.prover.representations.formula.Formula;
 import com.naveensundarg.shadow.prover.utils.CollectionUtils;
 import com.naveensundarg.shadow.prover.utils.Pair;
+import com.naveensundarg.shadow.prover.utils.Sets;
 
+import java.util.Comparator;
 import java.util.List;
 import java.util.Optional;
 import java.util.Set;
@@ -50,16 +52,19 @@ public class GoalTracker {
 
             Set<Plan> plans = possiblePlans.get();
 
-            Optional<Plan> noConflictPlan = plans.stream().filter(plan -> plan.noConflicts(currentGoals)).findAny();
+            Optional<Plan> possibleNoConflictPlan = plans.stream().filter(plan -> plan.noConflicts(currentGoals)).
+                    sorted(Comparator.comparing(plan->plan.getActions().size())).findAny();
 
 
-            if (noConflictPlan.isPresent()) {
+            if (possibleNoConflictPlan.isPresent()) {
 
               /*
                * If there is any plan without any goal conflicts, then adopt the goal.
                */
+                Plan noConflictPlan = possibleNoConflictPlan.get();
                 currentGoals.add(goal);
-                return noConflictPlan;
+                currentState = noConflictPlan.getExpectedStates().get(noConflictPlan.getExpectedStates().size()-1);
+                return possibleNoConflictPlan;
 
             } else {
 
@@ -72,30 +77,35 @@ public class GoalTracker {
                */
 
                 boolean feasiblePlanExists = false;
+                int bestPlanSize = Integer.MAX_VALUE;
                 double bestPriorityGap = 0;
                 Set<Goal> bestRemovalCandidates = null;
-                Plan feasiblePlan = null;
+                Set<Plan> feasiblePlans = Sets.newSet();
                 for (Plan plan : plans) {
 
                     Set<Goal> conflictingGoals = plan.getConflictingGoals(currentGoals);
                     double conflictSum = conflictingGoals.stream().mapToDouble(Goal::getPriority).sum();
                     double gap = goal.getPriority() - conflictSum;
 
-                    if(gap > 0 && gap > bestPriorityGap ){
+                    if(gap > 0 && gap >= bestPriorityGap && plan.getActions().size() <= bestPlanSize){
 
                         feasiblePlanExists = true;
                         bestPriorityGap = gap;
-                        feasiblePlan = plan;
+                        bestPlanSize = plan.getActions().size();
+                        feasiblePlans.add(plan);
                         bestRemovalCandidates= conflictingGoals;
                     }
                 }
 
-                if(feasiblePlan!=null){
+                if(!feasiblePlans.isEmpty()){
 
+                    Plan bestPlan = feasiblePlans.stream().
+                            min(Comparator.comparing(plan->plan.getActions().stream().mapToInt(Action::getWeight).sum())).get();
                     currentGoals.removeAll(bestRemovalCandidates);
                     currentGoals.add(goal);
+                    currentState = bestPlan.getExpectedStates().get(bestPlan.getExpectedStates().size()-1);
 
-                    return Optional.of(feasiblePlan);
+                    return Optional.of(bestPlan);
                 }
                 else {
 

src/main/java/edu/rpi/rair/Plan.java

@@ -31,6 +31,10 @@ public class Plan {
         return actions;
     }
 
+    public List<State> getExpectedStates() {
+        return expectedStates;
+    }
+
     public Plan getPlanByStartingWith(Action action, State state){
         List<Action> newActions = CollectionUtils.newEmptyList();
         newActions.addAll(actions);

src/main/resources/edu/rpi/rair/goal_tracking_tests.clj

@@ -60,26 +60,21 @@
                   ]
 }
 
-  :goals      {G1 {:priority 1.0
+  :goals      {G1 {:priority 6.0
                    :state    [(not (open (door room1)))]}
 
-               G2 {:priority 1.0
+               G2 {:priority 6.0
                    :state    [(in prisoner room1)]}
 
-               G3 {:priority 1.0
+               G3 {:priority 6.0
                    :state    [(forall [?room]
                                       (if (in prisoner ?room)
                                         (in self ?room)))]}
-
+               G4  {:priority 3.0
-               G4 {:priority 2.0
-
-                   :state [(interrogates commander prisoner)]}
-
-               G5 {:priority 1.0
                    :state [(in prisoner room2)
                            (in self room2)]}
+               G5 {:priority 2.0
 
-
+                   :state [(interrogates commander prisoner)]}}
-               }
 
  }

src/main/resources/edu/rpi/rair/temp.clj

@@ -0,0 +1,16 @@
+{G1 {:priority 6.0
+     :state    [(not (open (door room1)))]}
+
+ G2 {:priority 6.0
+     :state    [(in prisoner room1)]}
+
+ G3 {:priority 6.0
+     :state    [(forall [?room]
+                        (if (in prisoner ?room)
+                          (in self ?room)))]}
+ G4 {:priority 3.0
+     :state    [(in prisoner room2)
+                (in self room2)]}
+ G5 {:priority 2.0
+
+     :state    [(interrogates commander prisoner)]}}

target/classes/edu/rpi/rair/goal_tracking_tests.clj

@@ -60,26 +60,21 @@
                   ]
 }
 
-  :goals      {G1 {:priority 1.0
+  :goals      {G1 {:priority 6.0
                    :state    [(not (open (door room1)))]}
 
-               G2 {:priority 1.0
+               G2 {:priority 6.0
                    :state    [(in prisoner room1)]}
 
-               G3 {:priority 1.0
+               G3 {:priority 6.0
                    :state    [(forall [?room]
                                       (if (in prisoner ?room)
                                         (in self ?room)))]}
-
+               G4  {:priority 3.0
-               G4 {:priority 2.0
-
-                   :state [(interrogates commander prisoner)]}
-
-               G5 {:priority 1.0
                    :state [(in prisoner room2)
                            (in self room2)]}
+               G5 {:priority 2.0
 
-
+                   :state [(interrogates commander prisoner)]}}
-               }
 
  }

target/classes/edu/rpi/rair/temp.clj

@@ -0,0 +1,16 @@
+{G1 {:priority 6.0
+     :state    [(not (open (door room1)))]}
+
+ G2 {:priority 6.0
+     :state    [(in prisoner room1)]}
+
+ G3 {:priority 6.0
+     :state    [(forall [?room]
+                        (if (in prisoner ?room)
+                          (in self ?room)))]}
+ G4 {:priority 3.0
+     :state    [(in prisoner room2)
+                (in self room2)]}
+ G5 {:priority 2.0
+
+     :state    [(interrogates commander prisoner)]}}