State machine

controls/build.gradle

@@ -44,14 +44,13 @@ compileJava.dependsOn 'spotlessApply'
 dependencies {
 	// Use JUnit Jupiter for testing.
 	testImplementation libs.junit.jupiter
-
 	testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-
 	// This dependency is exported to consumers, that is to say found on their compile classpath.
 	api libs.commons.math3
 
 	// This dependency is used internally, and not exposed to consumers on their own compile classpath.
 	implementation libs.guava
+	implementation project(":parameter_tools")
 }
 
 // Apply a specific Java toolchain to ease working on different environments.
@@ -66,6 +65,12 @@ tasks.named('test') {
 	useJUnitPlatform()
 }
 
+
+
+test {
+	useJUnitPlatform()
+}
+
 compileJava.dependsOn 'spotlessApply'
 
 

controls/src/main/java/coppercore/controls/state_machine/StateMachine.java

@@ -0,0 +1,82 @@
+package coppercore.controls.state_machine;
+
+import coppercore.controls.state_machine.state.StateConfiguration;
+import coppercore.controls.state_machine.transition.Transition;
+import coppercore.controls.state_machine.transition.TransitionInfo;
+import java.util.Optional;
+
+public class StateMachine<State, Trigger> {
+    private final StateMachineConfiguration<State, Trigger> configuration;
+    private TransitionInfo<State, Trigger> transitionInfo;
+    private State currentState;
+
+    public StateMachine(StateMachineConfiguration<State, Trigger> config, State initialState) {
+        configuration = config;
+        currentState = initialState;
+    }
+
+    /**
+     * Method to transition States based on given trigger
+     *
+     * @param trigger
+     */
+    public void fire(Trigger trigger) {
+        transitionInfo = new TransitionInfo<>(currentState, trigger);
+        Optional<Transition<State, Trigger>> transitionOptional =
+                configuration.getTransition(currentState, trigger);
+        if (transitionOptional.isEmpty()) {
+            transitionInfo.fail();
+            return;
+        }
+        Transition<State, Trigger> transition = transitionOptional.get();
+        if (!transition.canTransition()) {
+            transitionInfo.fail();
+            return;
+        }
+        transitionInfo.setTransition(transition);
+        if (!transition.isInternal()) {
+            Optional<StateConfiguration<State, Trigger>> currentStateConfigurationOptional =
+                    configuration.getStateConfiguration(currentState);
+            Optional<StateConfiguration<State, Trigger>> nextStateConfigurationOptional =
+                    configuration.getStateConfiguration(transition.getDestination());
+            if (currentStateConfigurationOptional.isPresent()) {
+                StateConfiguration<State, Trigger> config = currentStateConfigurationOptional.get();
+                if (config.doRunDefaultExitAction()) {
+                    configuration.runOnExit(transition);
+                } else {
+                    config.runOnExit(transition);
+                }
+            } else {
+                configuration.runOnExit(transition);
+            }
+            transition.runAction();
+            if (nextStateConfigurationOptional.isPresent()) {
+                StateConfiguration<State, Trigger> config = nextStateConfigurationOptional.get();
+                if (config.doRunDefaultExitAction()) {
+                    configuration.runOnEntry(transition);
+                } else {
+                    config.runOnEntry(transition);
+                }
+            } else {
+                configuration.runOnEntry(transition);
+            }
+        }
+        currentState = transition.getDestination();
+    }
+
+    public State getCurrentState() {
+        return currentState;
+    }
+
+    public boolean successfulTransition() {
+        return !transitionInfo.wasFail();
+    }
+
+    public TransitionInfo<State, Trigger> getTransitionInfo() {
+        return transitionInfo;
+    }
+
+    public boolean inState(State state) {
+        return currentState.equals(state);
+    }
+}

controls/src/main/java/coppercore/controls/state_machine/StateMachineConfiguration.java

@@ -0,0 +1,113 @@
+package coppercore.controls.state_machine;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Consumer;
+
+import coppercore.controls.state_machine.state.StateConfiguration;
+import coppercore.controls.state_machine.transition.Transition;
+
+public class StateMachineConfiguration<State, Trigger> {
+    private final Map<State, StateConfiguration<State, Trigger>> stateConfigurations;
+    private Consumer<Transition<State, Trigger>> onEntryAction;
+    private Consumer<Transition<State, Trigger>> onExitAction;
+
+    public StateMachineConfiguration() {
+        // temp solution
+        stateConfigurations = new HashMap<>();
+    }
+
+    /**
+     * Starts configuration of a state returning a StateConfiguration and registers it for the state.
+     * @param state
+     * @return
+     */
+    public StateConfiguration<State, Trigger> configure(State source) {
+        StateConfiguration<State, Trigger> configuration = stateConfigurations.get(source);
+        if (configuration == null) {
+            configuration = new StateConfiguration<>(source);
+            stateConfigurations.put(source, configuration);
+        }
+        return configuration;
+    }
+
+    /**
+     * Gets a StateConfiguration specified by State
+     * @param state
+     * @return
+     */
+    public Optional<StateConfiguration<State, Trigger>> getStateConfiguration(State source) {
+        Optional<StateConfiguration<State, Trigger>> configurationOptional = Optional.empty();
+
+        if (stateConfigurations.containsKey(source)) {
+            StateConfiguration<State, Trigger> configuration = stateConfigurations.get(source);
+            if (configuration != null) {
+                configurationOptional = Optional.of(configuration);
+            }
+        }
+
+        return configurationOptional;
+    }
+
+    /**
+     * Gets a Transition defined by State and Trigger
+     * @param state
+     * @param trigger
+     * @return
+     */
+    public Optional<Transition<State, Trigger>> getTransition(State state, Trigger trigger) {
+        Optional<Transition<State, Trigger>> transition = Optional.empty();
+        Optional<StateConfiguration<State, Trigger>> configurationOptional =
+                getStateConfiguration(state);
+
+        if (configurationOptional.isPresent()) {
+            StateConfiguration<State, Trigger> configuration = configurationOptional.get();
+            Optional<Transition<State, Trigger>> transitionOptional =
+                    configuration.getFilteredTransition(trigger);
+            transition = transition.or(() -> transitionOptional);
+        }
+
+        return transition;
+    }
+
+    /**
+     * Set the default onEntry function.
+     * @param transition
+     */
+    public StateMachineConfiguration<State, Trigger> configureDefaultOnEntryAction(
+            Consumer<Transition<State, Trigger>> action) {
+        this.onEntryAction = action;
+        return this;
+    }
+
+    /**
+     * Set the default onExit function.
+     * @param transition
+     */
+    public StateMachineConfiguration<State, Trigger> configureDefaultOnExitAction(
+            Consumer<Transition<State, Trigger>> action) {
+        this.onExitAction = action;
+        return this;
+    }
+
+    /**
+     * Method used by statemachine to handle processing on entry of a state.
+     * @param transition
+     */
+    public void runOnEntry(Transition<State, Trigger> transition) {
+        if (onEntryAction != null) {
+            onEntryAction.accept(transition);
+        }
+    }
+
+    /**
+     * Method used by statemachine to handle processing on exiting of a state.
+     * @param transition
+     */
+    public void runOnExit(Transition<State, Trigger> transition) {
+        if (onExitAction != null) {
+            onExitAction.accept(transition);
+        }
+    }
+}

controls/src/main/java/coppercore/controls/state_machine/state/PeriodicStateInterface.java

@@ -0,0 +1,6 @@
+package coppercore.controls.state_machine.state;
+
+public interface PeriodicStateInterface extends StateInterface {
+
+    public default void periodic() {}
+}

controls/src/main/java/coppercore/controls/state_machine/state/StateConfiguration.java

@@ -0,0 +1,163 @@
+package coppercore.controls.state_machine.state;
+
+import coppercore.controls.state_machine.transition.ConditinalTransition;
+import coppercore.controls.state_machine.transition.Transition;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+public class StateConfiguration<State, Trigger> {
+
+    private List<Transition<State, Trigger>> transitions;
+    private State source;
+    private Consumer<Transition> onEntryAction;
+    private Consumer<Transition> onExitAction;
+    private boolean runDefaultEntryAction = true;
+    private boolean runDefaultExitAction = true;
+
+    public StateConfiguration(State source) {
+        this.source = source;
+        // Temp solution
+        transitions = new ArrayList<>();
+    }
+
+    /**
+     * Create Transition between States
+     *
+     * @param trigger
+     * @param destination
+     * @return
+     */
+    public StateConfiguration<State, Trigger> permit(Trigger trigger, State destination) {
+        if (getFilteredTransition(trigger).isEmpty()) {
+            transitions.add(new Transition<>(source, destination, trigger, false));
+        }
+        return this;
+    }
+
+    /**
+     * Create Transistion between states without trigger the enter or exit functions.
+     *
+     * @param trigger
+     * @param destination
+     * @return
+     */
+    public StateConfiguration<State, Trigger> permitInternal(Trigger trigger, State destination) {
+        if (getFilteredTransition(trigger).isEmpty()) {
+            transitions.add(new Transition<>(source, destination, trigger, true));
+        }
+        return this;
+    }
+
+    /**
+     * Creates a Conditional Transition that only fires if both the right Trigger is fired and the
+     * check lambda evaluates to true
+     *
+     * @param trigger
+     * @param destination
+     * @param check
+     * @return
+     */
+    public StateConfiguration<State, Trigger> permitIf(
+            Trigger trigger, State destination, BooleanSupplier check) {
+        if (getFilteredTransition(trigger).isEmpty()) {
+            transitions.add(new ConditinalTransition<>(source, destination, trigger, check, false));
+        }
+        return this;
+    }
+
+    /**
+     * Creates a Conditional Internal Transition that only fires if both the right Trigger is fired
+     * and the check lambda evaluates to true. This transition will not trigger the enter or exit
+     * functions.
+     *
+     * @param trigger
+     * @param destination
+     * @param check
+     * @return
+     */
+    public StateConfiguration<State, Trigger> permitInternalIf(
+            Trigger trigger, State destination, BooleanSupplier check) {
+        if (getFilteredTransition(trigger).isEmpty()) {
+            transitions.add(new ConditinalTransition<>(source, destination, trigger, check, true));
+        }
+        return this;
+    }
+
+    public List<Transition<State, Trigger>> getTransitions(Trigger trigger) {
+        List<Transition<State, Trigger>> matchedTransitions = new ArrayList<>();
+        if (trigger == null) return matchedTransitions;
+        for (Transition<State, Trigger> transition : transitions) {
+            if (trigger.equals(transition.getTrigger())) {
+                matchedTransitions.add(transition);
+            }
+        }
+        return matchedTransitions;
+    }
+
+    private Optional<Transition<State, Trigger>> filterTransitions(
+            List<Transition<State, Trigger>> transitions) {
+        Optional<Transition<State, Trigger>> returnOptional = Optional.empty();
+        boolean conditinal = false;
+        for (Transition<State, Trigger> transition : transitions) {
+            if (transition instanceof ConditinalTransition) {
+                if (conditinal
+                        && ((ConditinalTransition<State, Trigger>) transition).isCheckTrue()) {
+                    return Optional.empty();
+                } else {
+                    returnOptional = Optional.of(transition);
+                    conditinal = true;
+                }
+            } else if (!conditinal) {
+                returnOptional = Optional.of(transition);
+            }
+        }
+        return returnOptional;
+    }
+
+    public Optional<Transition<State, Trigger>> getFilteredTransition(Trigger trigger) {
+        return filterTransitions(getTransitions(trigger));
+    }
+
+    public void runOnEntry(Transition transition) {
+        if (onEntryAction != null) {
+            onEntryAction.accept(transition);
+        }
+    }
+
+    public void runOnExit(Transition transition) {
+        if (onExitAction != null) {
+            onExitAction.accept(transition);
+        }
+    }
+
+    public StateConfiguration<State, Trigger> disableDefualtOnEntry() {
+        this.runDefaultEntryAction = false;
+        return this;
+    }
+
+    public StateConfiguration<State, Trigger> disableDefualtOnExit() {
+        this.runDefaultExitAction = false;
+        return this;
+    }
+
+    public StateConfiguration<State, Trigger> configureOnEntryAction(Consumer<Transition> action) {
+        this.onEntryAction = action;
+        return this;
+    }
+
+    public StateConfiguration<State, Trigger> configureOnExitAction(Consumer<Transition> action) {
+        this.onExitAction = action;
+        return this;
+    }
+
+    public boolean doRunDefaultEntryAction() {
+        return runDefaultEntryAction;
+    }
+
+    public boolean doRunDefaultExitAction() {
+        return runDefaultExitAction;
+    }
+}