package com.edinarobotics.utils.autonomous;

/**
 * An autonomous step that allows multiple substeps to be run in parallel.
 * It simulates 
 */
public class ParallelAutonomousStepGroup extends AutonomousStep{
    private AutonomousStep[] steps;
    
    /**
     * Constructs a ParallelAutonomousStepGroup that will run the given steps
     * in parallel.
     * @param steps the steps to be run in parallel. 
     */
    public ParallelAutonomousStepGroup(AutonomousStep[] steps){
        this.steps = steps;
    }
    
    /**
     * Calls {@link AutonomousStep#start()} on all the {@link AutonomousStep}
     * objects contained by this ParallelAutonomousStepGroup.
     */
    public void start(){
        for(int i=0; i<steps.length; i++){
            steps[i].start();
        }
    }
    
    /**
     * Calls {@link AutonomousStep#run()} on all steps that are not yet
     * finished. This simulates running them in parallel.
     */
    public void run(){
        for(int i=0; i<steps.length; i++){
            if(!steps[i].isFinished()){
                steps[i].run();
            }
        }
    }
    
    /**
     * Calls {@link AutonomousStep#stop()} on all the steps in this
     * ParallelAutonomousStepGroup.
     */
    public void stop(){
        for(int i=0; i<steps.length; i++){
            steps[i].stop();
        }
    }
    
    /**
     * Returns whether or not all steps in this in ParallelAutonomousStepGroup
     * are finished.
     * @return {@code true} if all steps have finished running, {@code false}
     * otherwise.
     */
    public boolean isFinished(){
        for(int i=0; i<steps.length; i++){
            if(!steps[i].isFinished()){
                return false;
            }
        }
        return true;
    }
}

@@ -239,21 +239,58 @@
  steps;%0A
+    private boolean%5B%5D stepsFinished;%0A
     %0A
-
     /**%0A
@@ -537,24 +537,80 @@
 ps = steps;%0A
+        this.stepsFinished = new boolean%5Bsteps.length%5D;%0A
     %7D%0A    %0A 
@@ -1173,32 +1173,249 @@
 ;%0A            %7D%0A
+            if(steps%5Bi%5D.isFinished() && (!stepsFinished%5Bi%5D))%7B%0A                //Call stop() on the step as soon as it's finished%0A                steps%5Bi%5D.stop();%0A                stepsFinished%5Bi%5D = true;%0A            %7D%0A
         %7D%0A    %7D%0A
@@ -1598,32 +1598,131 @@
 s.length; i++)%7B%0A
+            if(!stepsFinished%5Bi%5D)%7B%0A                //Stop all steps that have not been stopped%0A    
             step
@@ -1734,33 +1734,162 @@
 stop();%0A        
+ 
-%7D
+   %7D%0A        %7D%0A        //Reset the steps array so that this step can be reused%0A        stepsFinished = new boolean%5Bsteps.length%5D;
 %0A    %7D%0A    %0A    

Change ParallelAutonomousStepGroup to stop finished steps after run().