diff --git a/docs/img/FSM.drawio b/docs/img/FSM.drawio
index df1a5708254fc47a292d92cbb0f4ce97ecc0f44d..3ea73db2828a8c8d5a840e96b7400593011339c3 100644
--- a/docs/img/FSM.drawio
+++ b/docs/img/FSM.drawio
@@ -64,7 +64,7 @@
             <mxPoint x="340" y="140" as="targetPoint" />
           </mxGeometry>
         </mxCell>
-        <mxCell id="bw7OO0sNot4gaAuLXok9-17" value="&lt;font style=&quot;font-size: 15px;&quot; face=&quot;Ubuntu Mono&quot;&gt;Condition 1&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="bw7OO0sNot4gaAuLXok9-14" vertex="1" connectable="0">
+        <mxCell id="bw7OO0sNot4gaAuLXok9-17" value="&lt;font face=&quot;Ubuntu Mono&quot;&gt;VALID = 1&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="bw7OO0sNot4gaAuLXok9-14" vertex="1" connectable="0">
           <mxGeometry x="0.564" relative="1" as="geometry">
             <mxPoint x="10" as="offset" />
           </mxGeometry>
@@ -81,10 +81,10 @@
             <mxPoint x="340" y="620" as="targetPoint" />
           </mxGeometry>
         </mxCell>
-        <mxCell id="bw7OO0sNot4gaAuLXok9-18" value="&lt;font style=&quot;font-size: 15px;&quot; face=&quot;Ubuntu Mono&quot;&gt;Condition 2&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="1" vertex="1" connectable="0">
+        <mxCell id="bw7OO0sNot4gaAuLXok9-18" value="&lt;font face=&quot;Ubuntu Mono&quot;&gt;ProcessingDone = 1&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="1" vertex="1" connectable="0">
           <mxGeometry x="340.00279069767436" y="380" as="geometry" />
         </mxCell>
-        <mxCell id="bw7OO0sNot4gaAuLXok9-19" value="&lt;font style=&quot;font-size: 15px;&quot; face=&quot;Ubuntu Mono&quot;&gt;Condition 3&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="1" vertex="1" connectable="0">
+        <mxCell id="bw7OO0sNot4gaAuLXok9-19" value="&lt;font face=&quot;Ubuntu Mono&quot;&gt;VALID = 0 &lt;br&gt;&lt;/font&gt;" style="edgeLabel;html=1;align=left;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;labelBorderColor=none;fontStyle=2" parent="1" vertex="1" connectable="0">
           <mxGeometry x="340.00279069767436" y="620" as="geometry" />
         </mxCell>
         <mxCell id="bw7OO0sNot4gaAuLXok9-20" value="" style="curved=1;endArrow=block;html=1;rounded=0;endFill=1;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="1" target="bw7OO0sNot4gaAuLXok9-1" edge="1">
diff --git a/docs/img/FSM.png b/docs/img/FSM.png
index 0b03a46cbe76af366bdedf594bdf9a597d1ca636..c4a670c3e06669af1f275384c850903c0cfffa32 100644
Binary files a/docs/img/FSM.png and b/docs/img/FSM.png differ
diff --git a/src/hdl/controlUnit.vhd b/src/hdl/controlUnit.vhd
index 21da15f3e1bdfbfa00939c8508c01ee8a1caca2f..e7d1ba5ee7bd95223060751e597018c30e06dd77 100644
--- a/src/hdl/controlUnit.vhd
+++ b/src/hdl/controlUnit.vhd
@@ -54,23 +54,42 @@ begin
   -- Process to describe the state register
   -- Current state is provide at the output of the register
   -- and is updated with the next state at each rising edge of clock
-  process (_BLANK_) is
+  process (I_clock, I_reset) is
   begin
     if I_reset = '1' then               -- asynchronous reset (active high)
-      SR_currentState <= _BLANK_
+      SR_currentState <= WAIT_SAMPLE;
     elsif rising_edge(I_clock) then     -- rising clock edge
-      _BLANK_
+      SR_currentState <= SR_nextState;
     end if;
   end process;
 
   -- Combinatorial process computing the next state which depends on
   -- the current state and on the inputs
-  process (_BLANK_) is
+  process (SR_currentState, I_inputSampleValid, I_processingDone) is
   begin
-    case SR_currentState is
-
-      when WAIT_SAMPLE =>
-        _BLANK_
+    case SR_currentState is 
+      when WAIT_SAMPLE => 
+        if I_inputsampleValid = '1' then
+            SR_nextState <= STORE;
+         else
+            SR_nextState <= WAIT_SAMPLE;
+         end  if; 
+      when STORE =>
+        SR_nextState <= PROCESSING_LOOP;
+      when PROCESSING_LOOP => 
+        if I_processingDone= '1' then
+            SR_nextState <= OUTPUT;
+        else
+            SR_nextState <= PROCESSING_LOOP;
+        end  if;
+     when OUTPUT =>
+        SR_nextState <= WAIT_END_SAMPLE;
+     when WAIT_END_SAMPLE =>
+        if I_inputSampleValid = '0' then
+            SR_nextState <= WAIT_SAMPLE;
+        else 
+            SR_nextState <= WAIT_END_SAMPLE;
+        end if; 
 
       when others => null;
     end case;
@@ -78,13 +97,13 @@ begin
 
   -- Rules to compute the outputs depending on the current state
   -- (and on the inputs, if you want a Mealy machine).
-  O_loadShift           <= '1' when _BLANK_ else '0';
-  O_initAddress         <= '1' when _BLANK_ else '0';
-  O_incrAddress         <= '1' when _BLANK_ else '0';
-  O_initSum             <= '1' when _BLANK_ else '0';
-  O_loadSum             <= '1' when _BLANK_ else '0';
-  O_loadOutput          <= '1' when _BLANK_ else '0';
-  O_FilteredSampleValid <= '1' when _BLANK_ else '0';
+  O_loadShift           <= '1' when SR_currentState = STORE else '0';
+  O_initAddress         <= '1' when SR_currentState = STORE else '0';
+  O_incrAddress         <= '1' when SR_currentState = PROCESSING_LOOP else '0';
+  O_initSum             <= '1' when SR_currentState = STORE  else '0';
+  O_loadSum             <= '1' when SR_currentState = PROCESSING_LOOP else '0';
+  O_loadOutput          <= '1' when SR_currentState = OUTPUT else '0';
+  O_FilteredSampleValid <= '1' when SR_currentState = OUTPUT or SR_currentState = WAIT_END_SAMPLE else '0';