diff --git a/lib/state_machine/state_machine.cpp b/lib/state_machine/state_machine.cpp
index 7c45b8d579c7..4a93188961e5 100644
--- a/lib/state_machine/state_machine.cpp
+++ b/lib/state_machine/state_machine.cpp
@@ -8,18 +8,6 @@ StateMachine::StateMachine() : current_state_{State::kIdle}
 {
 }
 
-// Transition to next state
-bool StateMachine::handleMessage(const Message &message)
-{
-  previous_message_.push(message);
-  const auto transition = transition_to_state_.find({current_state_, message});
-  if (transition != transition_to_state_.end()) {
-    current_state_ = transition->second;
-    return true;
-  }
-  return false;
-}
-
 Message StateMachine::stringToMessage(const std::string &message_name)
 {
   return string_to_message_.at(message_name);
@@ -35,15 +23,4 @@ State StateMachine::getCurrentState()
   return current_state_;
 }
 
-Message StateMachine::getPreviousMessage()
-{
-  if (previous_message_.empty()) {
-    return Message::kNone;
-  } else {
-    Message previous_message = previous_message_.front();
-    previous_message_.pop();
-    return previous_message;
-  }
-}
-
 }  // namespace hyped::state_machine
\ No newline at end of file
diff --git a/lib/state_machine/state_machine.hpp b/lib/state_machine/state_machine.hpp
index 48ecbb66def1..5152c7739c68 100644
--- a/lib/state_machine/state_machine.hpp
+++ b/lib/state_machine/state_machine.hpp
@@ -14,11 +14,9 @@ namespace hyped::state_machine {
 class StateMachine {
  public:
   StateMachine();
-  bool handleMessage(const Message &message);
   Message stringToMessage(const std::string &message_name);
   std::string messageToString(const Message &message);
   State getCurrentState();
-  Message getPreviousMessage();
 
  private:
   const std::unordered_map<std::string, Message> string_to_message_
@@ -74,7 +72,6 @@ class StateMachine {
        {{State::kFailureStopped, Message::kFailureOff}, State::kOff}};
 
   State current_state_;
-  std::queue<Message> previous_message_;
 };
 
 }  // namespace hyped::state_machine
\ No newline at end of file
diff --git a/test/state_machine/stm_tests.cpp b/test/state_machine/stm_tests.cpp
index 8d1720872e85..d9eaa23c2922 100644
--- a/test/state_machine/stm_tests.cpp
+++ b/test/state_machine/stm_tests.cpp
@@ -4,202 +4,9 @@
 
 namespace hyped::test {
 
-void testTransition(std::unique_ptr<state_machine::StateMachine> &stm,
-                    state_machine::Message transition_message,
-                    state_machine::State expected_state)
-{
-  stm->handleMessage(transition_message);
-  ASSERT_TRUE(stm->getCurrentState() == expected_state);
-}
-
-TEST(StateMachine, cleanRun)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kMotorBrake, state_machine::State::kMotorBraking);
-  testTransition(
-    stm, state_machine::Message::kPreFrictionBrake, state_machine::State::kPreFrictionBraking);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrake, state_machine::State::kFrictionBraking);
-  testTransition(stm, state_machine::Message::kStopped, state_machine::State::kStopped);
-  testTransition(stm, state_machine::Message::kOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, cleanRunDuplicatedMessages)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kMotorBrake, state_machine::State::kMotorBraking);
-  testTransition(stm, state_machine::Message::kMotorBrake, state_machine::State::kMotorBraking);
-  testTransition(
-    stm, state_machine::Message::kPreFrictionBrake, state_machine::State::kPreFrictionBraking);
-  testTransition(
-    stm, state_machine::Message::kPreFrictionBrake, state_machine::State::kPreFrictionBraking);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrake, state_machine::State::kFrictionBraking);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrake, state_machine::State::kFrictionBraking);
-  testTransition(stm, state_machine::Message::kStopped, state_machine::State::kStopped);
-  testTransition(stm, state_machine::Message::kStopped, state_machine::State::kStopped);
-  testTransition(stm, state_machine::Message::kOff, state_machine::State::kOff);
-  testTransition(stm, state_machine::Message::kOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, failureBrakeFromAccelerating)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, failureBrakeFromCruising)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, failureBrakeFromMotorBraking)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kMotorBrake, state_machine::State::kMotorBraking);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, frictionBrakeFailFromAccelerating)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, frictionBrakeFailFromCruising)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, frictionBrakeFailFromMotorBraking)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kCruising, state_machine::State::kCruising);
-  testTransition(stm, state_machine::Message::kMotorBrake, state_machine::State::kMotorBraking);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, failureBrakeFromFrictionBraking)
-{
-  std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-}
-
-TEST(StateMachine, duplicatedMessagesFailureStates)
+TEST(StateMachine, initalize)
 {
   std::unique_ptr stm = std::make_unique<state_machine::StateMachine>();
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kIdle);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kCalibrating, state_machine::State::kCalibrating);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kReady, state_machine::State::kReady);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm, state_machine::Message::kAccelerating, state_machine::State::kAccelerating);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(stm,
-                 state_machine::Message::kPreFrictionBrakeFail,
-                 state_machine::State::kPreFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(
-    stm, state_machine::Message::kFrictionBrakeFail, state_machine::State::kFrictionBrakingFail);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(stm, state_machine::Message::kFailureBrake, state_machine::State::kFailureBraking);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(
-    stm, state_machine::Message::kFailureStopped, state_machine::State::kFailureStopped);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
-  testTransition(stm, state_machine::Message::kFailureOff, state_machine::State::kOff);
 }
 
 }  // namespace hyped::test
\ No newline at end of file