diff --git a/CMakeLists.txt b/CMakeLists.txt
index 586a29c..1803c5d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ add_subdirectory(AutonomousMode)
 add_subdirectory(Communication)
 add_subdirectory(Socket)
 
-add_executable(lfr_central main.cpp lfr_state_machine.cpp)
+add_executable(lfr_central main.cpp lfr_state_machine.cpp lfr_states.cpp)
 
 target_include_directories(lfr_central PRIVATE ${lfr_image_processing_SOURCE_DIRS})
 target_include_directories(lfr_central PRIVATE ${lfr_uart_SOURCE_DIRS})
diff --git a/lfr_state_interface.h b/lfr_state_interface.h
new file mode 100644
index 0000000..52d27ba
--- /dev/null
+++ b/lfr_state_interface.h
@@ -0,0 +1,11 @@
+#pragma once
+
+class LFR_StateMachine;
+
+class LFR_IState
+{
+ public:
+    virtual void enter(LFR_StateMachine* stateMachine) = 0;
+    virtual void exit(LFR_StateMachine* stateMachine) = 0;
+    virtual ~LFR_IState() {}
+};
\ No newline at end of file
diff --git a/lfr_state_machine.cpp b/lfr_state_machine.cpp
index 24cbf20..8c6b7d5 100644
--- a/lfr_state_machine.cpp
+++ b/lfr_state_machine.cpp
@@ -1,54 +1 @@
 #include "lfr_state_machine.h"
-
-/* void State::Idle::toggle(LFR_StateMachine* stateMachine)
-{
-    stateMachine->setState()
-} */
-
-void State::Idle::enter(LFR_StateMachine* stateMachine)
-{
-
-}
-
-void State::Idle::exit(LFR_StateMachine* stateMachine)
-{
-    
-}
-
-LFR_State& State::Idle::getInstance()
-{
-    static State::Idle singleton;
-    return singleton;
-}
-
-void State::Autonomous::enter(LFR_StateMachine* stateMachine)
-{
-
-}
-
-void State::Autonomous::exit(LFR_StateMachine* stateMachine)
-{
-    stateMachine->setState(State::Idle::getInstance());
-}
-
-LFR_State& State::Autonomous::getInstance()
-{
-    static State::Autonomous singleton;
-    return singleton;
-}
-
-void State::Manual::enter(LFR_StateMachine* stateMachine)
-{
-
-}
-
-void State::Manual::exit(LFR_StateMachine* stateMachine)
-{
-    stateMachine->setState(State::Idle::getInstance());
-}
-
-LFR_State& State::Manual::getInstance()
-{
-    static State::Manual singleton;
-    return singleton;
-}
\ No newline at end of file
diff --git a/lfr_state_machine.h b/lfr_state_machine.h
index 0ee53d2..3f3fe36 100644
--- a/lfr_state_machine.h
+++ b/lfr_state_machine.h
@@ -1,66 +1,17 @@
 #pragma once
-
-class LFR_StateMachine;
-
-class LFR_State
-{
- public:
- virtual void enter(LFR_StateMachine* stateMachine) = 0;
- virtual void exit(LFR_StateMachine* stateMachine) = 0;
- virtual ~LFR_State() {}
-};
-
-
-namespace State
-{
-    class Idle: public LFR_State
-    {
-        Idle() {}
-        Idle(const Idle& other) {};
-        Idle& operator= (const Idle& other);
-
-    public:
-        void enter(LFR_StateMachine*);
-        void exit(LFR_StateMachine*);
-        static LFR_State& getInstance();
-    };
-
-
-    class Autonomous: public LFR_State
-    {
-        Autonomous() {}
-        Autonomous(const Autonomous& other) {};
-        Autonomous& operator= (const Autonomous& other);
-
-    public:
-        void enter(LFR_StateMachine*);
-        void exit(LFR_StateMachine*);
-        static LFR_State& getInstance();
-    };
-
-
-    class Manual: public LFR_State
-    {
-        Manual() {}
-        Manual(const Manual& other) {};
-        Manual& operator= (const Manual& other);
-
-    public:
-        void enter(LFR_StateMachine*);
-        void exit(LFR_StateMachine*);
-        static LFR_State& getInstance();
-    };
-}
+#include "lfr_state_interface.h"
 
 class LFR_StateMachine
 {
-    LFR_State* currentState;
+    LFR_IState* currentState;
+    LFR_StateMachine() = delete;
 public:
-    LFR_StateMachine(){
-        setState(State::Idle::getInstance());
+    LFR_StateMachine(LFR_IState& startState){
+        currentState = &startState;
+        currentState->enter(this);
     }
-    inline LFR_State* getCurrentState() const {return currentState;}
-    void setState(LFR_State& newState)
+    inline LFR_IState* getCurrentState() const {return currentState;}
+    void setState(LFR_IState& newState)
     {
         currentState->exit(this);
         currentState = &newState;
diff --git a/lfr_states.cpp b/lfr_states.cpp
new file mode 100644
index 0000000..4b1aae9
--- /dev/null
+++ b/lfr_states.cpp
@@ -0,0 +1,49 @@
+#include "lfr_states.h"
+
+void State::Idle::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Idle::exit(LFR_StateMachine* stateMachine)
+{
+    
+}
+
+LFR_IState& State::Idle::getInstance()
+{
+    static State::Idle singleton;
+    return singleton;
+}
+
+void State::Autonomous::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Autonomous::exit(LFR_StateMachine* stateMachine)
+{
+    stateMachine->setState(State::Idle::getInstance());
+}
+
+LFR_IState& State::Autonomous::getInstance()
+{
+    static State::Autonomous singleton;
+    return singleton;
+}
+
+void State::Manual::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Manual::exit(LFR_StateMachine* stateMachine)
+{
+    stateMachine->setState(State::Idle::getInstance());
+}
+
+LFR_IState& State::Manual::getInstance()
+{
+    static State::Manual singleton;
+    return singleton;
+}
\ No newline at end of file
diff --git a/lfr_states.h b/lfr_states.h
new file mode 100644
index 0000000..aadfbc6
--- /dev/null
+++ b/lfr_states.h
@@ -0,0 +1,44 @@
+#pragma once
+#include "lfr_state_interface.h"
+#include "lfr_state_machine.h"
+
+namespace State
+{
+    class Idle: public LFR_IState
+    {
+        Idle() {}
+        Idle(const Idle& other) {};
+        Idle& operator= (const Idle& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Autonomous: public LFR_IState
+    {
+        Autonomous() {}
+        Autonomous(const Autonomous& other) {};
+        Autonomous& operator= (const Autonomous& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Manual: public LFR_IState
+    {
+        Manual() {}
+        Manual(const Manual& other) {};
+        Manual& operator= (const Manual& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+}
\ No newline at end of file
diff --git a/main.cpp b/main.cpp
index 209f2d3..eb1d4fe 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,6 +3,7 @@
 #include <lfr_socket.h>
 #include <uart_communication.h>
 #include "lfr_state_machine.h"
+#include "lfr_states.h"
 
 int main(void)
 {
@@ -17,7 +18,7 @@ int main(void)
 
     // Init State Machine;
     std::cout << "create State Machine" << std::endl;
-    LFR_StateMachine stateMachine();
+    LFR_StateMachine stateMachine(State::Idle::getInstance());
 
     // Init LFR Autonomous mode
     std::cout << "create lfr" << std::endl;
@@ -52,7 +53,7 @@ int main(void)
 
     //Start the permanent loop
     char input;
-    std::cout << "fress q to quit" << std::endl;
+    std::cout << "press q to quit" << std::endl;
     std::cin >> input;
     std::cout << "cinned" << std::endl;
     while (input != 'q')