Added Finite State Machine implementation

5 files changed, 148 insertions, 2 deletions

diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 59d1156..8af9402 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -11,8 +11,9 @@ set(PICO_BOARD pico_w)
 
 pico_sdk_init()
 
-add_executable(main
+add_executable(main 
 	main.cpp
+	FSM.cpp
 )
 
 pico_enable_stdio_usb(main 1)
@@ -20,4 +21,3 @@ pico_enable_stdio_usb(main 1)
 pico_add_extra_outputs(main)
 target_include_directories(main PRIVATE ${CMAKE_CURRENT_LIST_DIR})
 target_link_libraries(main pico_cyw43_arch_lwip_threadsafe_background pico_stdlib)
-
diff --git a/main/puzzle/fsm/FSM.cpp b/main/puzzle/fsm/FSM.cpp
new file mode 100644
index 0000000..fad7fb3
--- /dev/null
+++ b/main/puzzle/fsm/FSM.cpp
@@ -0,0 +1,3 @@
+#include "FSM.h"
+
+#include "IBehaviouralState.hpp"
\ No newline at end of file
diff --git a/main/puzzle/fsm/FSM.h b/main/puzzle/fsm/FSM.h
new file mode 100644
index 0000000..a363c88
--- /dev/null
+++ b/main/puzzle/fsm/FSM.h
@@ -0,0 +1,50 @@
+#ifndef _FSM_H_
+#define _FSM_H_
+
+#include <map>
+#include <memory>
+
+#include "IBehaviouralState.hpp"
+
+/// <summary>
+///
+/// </summary>
+/// <typeparam name="TState">IBehaviouralState is the only accepted
+/// class.</typeparam>
+template <class... TState> class FSM {
+public:
+  template <class... TPState> FSM(TPState &...args) {
+    int i = 0;
+
+    ((void)_states.emplace(i++, args), ...);
+  }
+
+  /// <summary>
+  /// Implement with FSM::act()
+  /// </summary>
+  void act();
+
+  /// <summary>
+  /// Used to check current state.
+  /// </summary>
+  /// <returns>Current state.</returns>
+  std::shared_ptr<TState> &get_state() { return _currentState; }
+
+  /// <summary>
+  /// Used to get all states.
+  /// </summary>
+  /// <returns>Current states.</returns>
+  std::map<int, std::shared_ptr<TState>> get_states();
+
+  /// <summary>
+  /// Sets current state, calls appropiate functions.
+  /// </summary>
+  /// <param name="">State to transition into.</param>
+  void set_state(std::shared_ptr<TState>);
+
+private:
+  std::map<int, std::shared_ptr<TState>> _states;
+  std::shared_ptr<TState> _currentState;
+};
+
+#endif // _FSM_H_
\ No newline at end of file
diff --git a/main/puzzle/fsm/FSM.hpp b/main/puzzle/fsm/FSM.hpp
new file mode 100644
index 0000000..c86ed80
--- /dev/null
+++ b/main/puzzle/fsm/FSM.hpp
@@ -0,0 +1,68 @@
+#ifndef _FSM_HPP_
+#define _FSM_HPP_
+
+#include "FSM.h"
+
+#include <iostream>
+
+/// <summary>
+/// This is a generic update method, unknown classes are not supported.
+/// </summary>
+/// <typeparam name="TState">IBehaviouralState is the only accepted
+/// class.</typeparam>
+template <class TState> void FSM<TState>::act() {
+  // No implementation for unknown class.
+  std::cout << "No implementation for unknown class" << std::endl;
+}
+//
+///// <summary>
+///// Calls the IBehaviouralState act method.
+///// </summary>
+///// <typeparam name="TState">IBehaviouralState is the only accepted
+/// class.</typeparam>
+template <> inline void FSM<IBehaviouralState>::act() { _currentState->act(); }
+
+/// <summary>
+/// This is a generic return of the type.
+/// </summary>
+/// <typeparam name="TState">Any class type works with this return.</typeparam>
+/// <returns>The current IBehaviouralState map.</returns>
+template <class TState>
+std::map<int, std::shared_ptr<TState>> FSM<TState>::get_states() {
+  return _states;
+}
+
+///// <summary>
+///// State transitioning from current state to newState.
+///// Calls Exit on the current state and Enter on the new state.
+///// </summary>
+///// <typeparam name="TState">IBehaviouralState is the only accepted
+/// class.</typeparam>
+///// <param name="newState">New state to transition into.</param>
+template <>
+inline void
+FSM<IBehaviouralState>::set_state(std::shared_ptr<IBehaviouralState> newState) {
+  // We can guarantee all statemachines are based on IBehaviouralState
+  if (_currentState != nullptr)
+    _currentState->exit();
+
+  _currentState = nullptr;
+  _currentState = newState;
+  _currentState->enter();
+}
+
+/// <summary>
+/// State transitioning from current state to newState.
+/// Calls Exit on the current state and Enter on the new state.
+/// </summary>
+/// <typeparam name="TState">IBehaviouralState is the only accepted
+/// class.</typeparam> <param name="newState">New state to transition
+/// into.</param>
+template <class TState>
+void FSM<TState>::set_state(std::shared_ptr<TState> newState) {
+  // We can guarantee all statemachines are based on IBehaviouralState
+  // No implementation for an unknown state change class.
+  std::cout << "No implementation for unknown state change class" << std::endl;
+}
+
+#endif // _FSM_HPP_
\ No newline at end of file
diff --git a/main/puzzle/fsm/IBehaviouralState.hpp b/main/puzzle/fsm/IBehaviouralState.hpp
new file mode 100644
index 0000000..c41fef3
--- /dev/null
+++ b/main/puzzle/fsm/IBehaviouralState.hpp
@@ -0,0 +1,25 @@
+#ifndef _FSM_IBEHAVIOURALSTATE_HPP_
+#define _FSM_IBEHAVIOURALSTATE_HPP_
+
+/// <summary>
+/// Sub class used to define methods implemented by behavioural specific states.
+/// </summary>
+class IBehaviouralState {
+public:
+  /// <summary>
+  /// Enters the current state. Used for setup.
+  /// </summary>
+  virtual void enter() = 0;
+
+  /// <summary>
+  /// Updates the current state, used for physics, etc.
+  /// </summary>
+  virtual void act() = 0;
+
+  /// <summary>
+  /// Exits the state, used for cleanup.
+  /// </summary>
+  virtual void exit() = 0;
+};
+
+#endif // _FSM_IBEHAVIOURALSTATE_HPP_
\ No newline at end of file