diff --git a/Inc/HALAL/Services/Encoder/Encoder.hpp b/Inc/HALAL/Services/Encoder/Encoder.hpp
index b41370031..24f3fa30b 100644
--- a/Inc/HALAL/Services/Encoder/Encoder.hpp
+++ b/Inc/HALAL/Services/Encoder/Encoder.hpp
@@ -1,114 +1,117 @@
-#pragma once
-
-#include "HALAL/Models/TimerDomain/TimerDomain.hpp"
-
-#ifdef HAL_TIM_MODULE_ENABLED
-
-#define NANO_SECOND 1000000000.0
-#define CLOCK_MAX_VALUE 4294967295 // here goes the tim23 counter period
-
-namespace ST_LIB {
-
-template <const TimerDomain::Timer& dev> struct TimerWrapper;
-
-template <const TimerDomain::Timer& dev> class Encoder {
-    static_assert(
-        dev.e.pin_count == 2,
-        "Encoder must have exactly 2 encoder pins, as it uses the whole timer"
-    );
-    static_assert(dev.e.pins[0].af == TimerAF::Encoder, "Pin 0 must be declared as encoder");
-    static_assert(dev.e.pins[1].af == TimerAF::Encoder, "Pin 1 must be declared as encoder");
-    static_assert(
-        dev.e.pins[0].channel != dev.e.pins[1].channel,
-        "Pins must be of different channels"
-    );
-
-    inline static TimerWrapper<dev>* timer;
-    inline static bool is_on = false;
-
-public:
-    Encoder(TimerWrapper<dev>* tim) {
-        if (timer == nullptr) {
-            init(tim);
-        }
-    }
-
-    static void init(TimerWrapper<dev>* tim) {
-        timer = tim;
-        TIM_Encoder_InitTypeDef sConfig = {0};
-        TIM_MasterConfigTypeDef sMasterConfig = {0};
-
-        tim->instance->hal_tim->Init.Prescaler = 5;
-        tim->instance->hal_tim->Init.CounterMode = TIM_COUNTERMODE_UP;
-        tim->instance->hal_tim->Init.Period = 55000;
-        tim->instance->hal_tim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-        tim->instance->hal_tim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-
-        sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
-        sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
-        sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
-        sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
-        sConfig.IC1Filter = 0;
-        sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
-        sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
-        sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
-        sConfig.IC2Filter = 0;
-
-        if (HAL_TIM_Encoder_Init(tim->instance->hal_tim, &sConfig) != HAL_OK) {
-            ErrorHandler("Unable to init encoder");
-        }
-
-        sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-        sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-        if (HAL_TIMEx_MasterConfigSynchronization(tim->instance->hal_tim, &sMasterConfig) !=
-            HAL_OK) {
-            ErrorHandler("Unable to config master synchronization in encoder");
-        }
-    }
-
-    static void turn_on() {
-        if (is_on)
-            return;
-
-        if (HAL_TIM_Encoder_GetState(timer->instance->hal_tim) == HAL_TIM_STATE_RESET) {
-            ErrorHandler("Unable to get state from encoder");
-            return;
-        }
-        if (HAL_TIM_Encoder_Start(timer->instance->hal_tim, TIM_CHANNEL_ALL) != HAL_OK) {
-            ErrorHandler("Unable to start encoder");
-            return;
-        }
-        is_on = true;
-        reset();
-    }
-
-    static void turn_off() {
-        if (!is_on)
-            return;
-        if (HAL_TIM_Encoder_Stop(timer->instance->hal_tim, TIM_CHANNEL_ALL) != HAL_OK) {
-            ErrorHandler("Unable to stop encoder");
-        }
-        is_on = false;
-    }
-
-    static inline void reset() { timer->instance->tim->CNT = UINT32_MAX / 2; }
-
-    static inline uint32_t get_counter() { return timer->instance->tim->CNT; }
-
-    static inline bool get_direction() { return ((timer->instance->tim->CR1 & 0b10000) >> 4); }
-
-    static inline uint32_t get_initial_counter_value() { return timer->instance->tim->ARR / 2; }
-
-    static int64_t get_delta_clock(uint64_t clock_time, uint64_t last_clock_time) {
-        int64_t delta_clock = clock_time - last_clock_time;
-        if (clock_time < last_clock_time) { // overflow handle
-            delta_clock = clock_time +
-                          CLOCK_MAX_VALUE * NANO_SECOND / timer->get_clock_frequency() -
-                          last_clock_time;
-        }
-        return delta_clock;
-    }
-};
-
-} // namespace ST_LIB
-#endif
+#pragma once
+
+#include "HALAL/Models/TimerDomain/TimerDomain.hpp"
+
+#ifdef HAL_TIM_MODULE_ENABLED
+
+#define NANO_SECOND 1000000000.0
+#define CLOCK_MAX_VALUE 4294967295 // here goes the tim23 counter period
+
+namespace ST_LIB {
+
+template <const TimerDomain::Timer& dev> struct TimerWrapper;
+
+template <const TimerDomain::Timer& dev> class Encoder {
+    friend struct TimerWrapper<dev>;
+
+    static_assert(
+        dev.e.pin_count == 2,
+        "Encoder must have exactly 2 encoder pins, as it uses the whole timer"
+    );
+    static_assert(dev.e.pins[0].af == TimerAF::Encoder, "Pin 0 must be declared as encoder");
+    static_assert(dev.e.pins[1].af == TimerAF::Encoder, "Pin 1 must be declared as encoder");
+    static_assert(
+        dev.e.pins[0].channel != dev.e.pins[1].channel,
+        "Pins must be of different channels"
+    );
+
+    inline static TimerWrapper<dev>* timer;
+    inline static bool is_on = false;
+
+    Encoder(TimerWrapper<dev>* tim) {
+        if (timer == nullptr) {
+            init(tim);
+        }
+    }
+
+public:
+    static void init(TimerWrapper<dev>* tim) {
+        TIM_Encoder_InitTypeDef sConfig = {0};
+        TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+        sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
+        sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
+        sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
+        sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
+        sConfig.IC1Filter = 0;
+        sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
+        sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
+        sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
+        sConfig.IC2Filter = 0;
+
+        if (HAL_TIM_Encoder_Init(tim->instance->hal_tim, &sConfig) != HAL_OK) {
+            ErrorHandler("Unable to init encoder");
+            return;
+        }
+
+        sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+        sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+        if (HAL_TIMEx_MasterConfigSynchronization(tim->instance->hal_tim, &sMasterConfig) !=
+            HAL_OK) {
+            ErrorHandler("Unable to config master synchronization in encoder");
+            return;
+        }
+
+        tim->instance->tim->PSC = 5;
+        tim->instance->tim->ARR = 55000;
+        timer = tim;
+    }
+
+    static void turn_on() {
+        if (is_on)
+            return;
+
+        if (HAL_TIM_Encoder_GetState(timer->instance->hal_tim) == HAL_TIM_STATE_RESET) {
+            ErrorHandler("Unable to get state from encoder");
+            return;
+        }
+        if (HAL_TIM_Encoder_Start(timer->instance->hal_tim, TIM_CHANNEL_ALL) != HAL_OK) {
+            ErrorHandler("Unable to start encoder");
+            return;
+        }
+        is_on = true;
+        reset();
+    }
+
+    static void turn_off() {
+        if (!is_on)
+            return;
+
+        if (HAL_TIM_Encoder_Stop(timer->instance->hal_tim, TIM_CHANNEL_ALL) != HAL_OK) {
+            ErrorHandler("Unable to stop encoder");
+            return;
+        }
+        is_on = false;
+    }
+
+    static inline void reset() { timer->instance->tim->CNT = get_initial_counter_value(); }
+
+    static inline uint32_t get_counter() { return timer->instance->tim->CNT; }
+
+    static inline bool get_direction() { return ((timer->instance->tim->CR1 & 0b10000) >> 4); }
+
+    static inline uint32_t get_initial_counter_value() { return timer->instance->tim->ARR / 2; }
+
+    static int64_t get_delta_clock(uint64_t clock_time, uint64_t last_clock_time) {
+        int64_t delta_clock = clock_time - last_clock_time;
+        if (clock_time < last_clock_time) { // overflow handle
+            delta_clock = clock_time +
+                          CLOCK_MAX_VALUE * NANO_SECOND / timer->get_clock_frequency() -
+                          last_clock_time;
+        }
+        return delta_clock;
+    }
+};
+
+} // namespace ST_LIB
+#endif
diff --git a/Inc/HALAL/Services/PWM/DualPWM.hpp b/Inc/HALAL/Services/PWM/DualPWM.hpp
index 26c36d073..f4a315535 100644
--- a/Inc/HALAL/Services/PWM/DualPWM.hpp
+++ b/Inc/HALAL/Services/PWM/DualPWM.hpp
@@ -19,6 +19,8 @@ template <
     const ST_LIB::TimerPin pin,
     const ST_LIB::TimerPin negated_pin>
 class DualPWM {
+    friend TimerWrapper<dev>;
+
     static consteval uint8_t get_channel_state_idx(const ST_LIB::TimerChannel ch) {
         switch (ch) {
         case TimerChannel::CHANNEL_1:
@@ -70,7 +72,6 @@ class DualPWM {
     bool is_on_positive = false;
     bool is_on_negative = false;
 
-public:
     DualPWM(
         TimerWrapper<dev>* tim,
         uint32_t polarity,
@@ -98,6 +99,7 @@ class DualPWM {
         timer->template set_output_compare_preload_enable<pin.channel>();
     }
 
+public:
     inline void turn_on() {
         turn_on_positive();
         turn_on_negative();
diff --git a/Inc/HALAL/Services/PWM/PWM.hpp b/Inc/HALAL/Services/PWM/PWM.hpp
index b1d4cdb3a..9c3da2ff0 100644
--- a/Inc/HALAL/Services/PWM/PWM.hpp
+++ b/Inc/HALAL/Services/PWM/PWM.hpp
@@ -15,6 +15,8 @@ namespace ST_LIB {
 template <const TimerDomain::Timer& dev> struct TimerWrapper;
 
 template <const TimerDomain::Timer& dev, const ST_LIB::TimerPin pin> class PWM {
+    friend TimerWrapper<dev>;
+
     static consteval uint8_t get_channel_state_idx(const ST_LIB::TimerChannel ch) {
         switch (ch) {
         case TimerChannel::CHANNEL_1:
@@ -65,7 +67,6 @@ template <const TimerDomain::Timer& dev, const ST_LIB::TimerPin pin> class PWM {
     float* duty_cycle = nullptr;
     bool is_on = false;
 
-public:
     PWM(TimerWrapper<dev>* tim,
         uint32_t polarity,
         uint32_t negated_polarity,
@@ -90,6 +91,7 @@ template <const TimerDomain::Timer& dev, const ST_LIB::TimerPin pin> class PWM {
         timer->template set_output_compare_preload_enable<pin.channel>();
     }
 
+public:
     void turn_on() {
         if (this->is_on)
             return;
diff --git a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp
index f8d52c1bf..7bcf98eb7 100644
--- a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp
+++ b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp
@@ -6,8 +6,7 @@
  */
 #pragma once
 
-#include "HALAL/HALAL.hpp"
-// #include "HALAL/Services/Encoder/NewEncoder.hpp"
+#include "C++Utilities/CppUtils.hpp"
 
 namespace ST_LIB {
 
@@ -15,7 +14,8 @@ template <typename EncoderType, size_t SAMPLES> struct EncoderSensor {
     enum Direction : uint8_t { FORWARD = 0, BACKWARDS = 1 };
 
 private:
-    constexpr static int64_t START_COUNTER{UINT32_MAX / 2};
+    constexpr static size_t WINDOW_SIZE{(SAMPLES / 2) * 2};
+    static_assert(WINDOW_SIZE >= 2, "EncoderSensor requires at least two samples");
 
     const double counter_distance_m;
     const double sample_time_s;
@@ -41,7 +41,7 @@ template <typename EncoderType, size_t SAMPLES> struct EncoderSensor {
     )
         : counter_distance_m(counter_distance_m), sample_time_s(sample_time_s), encoder(enc),
           direction(direction), position(position), speed(speed), acceleration(acceleration) {
-        for (size_t i{0}; i < SAMPLES; i++)
+        for (size_t i{0}; i < WINDOW_SIZE; i++)
             past_delta_counters.push(0);
     }
 
@@ -50,7 +50,7 @@ template <typename EncoderType, size_t SAMPLES> struct EncoderSensor {
 
     void reset() {
         encoder.reset();
-        for (size_t i{0}; i < SAMPLES; ++i)
+        for (size_t i{0}; i < WINDOW_SIZE; ++i)
             past_delta_counters.push_pop(0);
     }
 
@@ -58,7 +58,7 @@ template <typename EncoderType, size_t SAMPLES> struct EncoderSensor {
     void read() {
         uint32_t counter{encoder.get_counter()};
 
-        int64_t delta_counter{(int64_t)counter - START_COUNTER};
+        int64_t delta_counter{(int64_t)counter - (int64_t)encoder.get_initial_counter_value()};
         const int64_t& previous_delta_counter{
             past_delta_counters[past_delta_counters.size() / 2 - 1]
         };
@@ -71,13 +71,12 @@ template <typename EncoderType, size_t SAMPLES> struct EncoderSensor {
         // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Backward_finite_difference
         *speed = ((3.0 * delta_counter / 2.0) - (2.0 * previous_delta_counter) +
                   (previous_previous_delta_counter / 2.0)) *
-                 counter_distance_m / (sample_time_s * past_delta_counters.size() / 2);
+                 counter_distance_m / (sample_time_s * WINDOW_SIZE / 2);
 
         *acceleration =
             (delta_counter - (2.0 * previous_delta_counter) + previous_previous_delta_counter) *
             counter_distance_m /
-            ((sample_time_s * past_delta_counters.size() / 2) *
-             (sample_time_s * past_delta_counters.size() / 2));
+            ((sample_time_s * WINDOW_SIZE / 2) * (sample_time_s * WINDOW_SIZE / 2));
 
         *direction = encoder.get_direction() ? FORWARD : BACKWARDS;
 
diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt
index 0c882b8a7..845f52058 100644
--- a/Tests/CMakeLists.txt
+++ b/Tests/CMakeLists.txt
@@ -19,6 +19,7 @@ message(STATUS "Generating test executable for ST-LIB")
 add_executable(${STLIB_TEST_EXECUTABLE}
     ${CMAKE_CURRENT_LIST_DIR}/../Src/HALAL/Models/SPI/SPI2.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../Src/HALAL/Models/DMA/DMA2.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/Time/encoder_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/Time/scheduler_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/Time/timer_wrapper_test.cpp
     ${CMAKE_CURRENT_LIST_DIR}/adc_test.cpp
diff --git a/Tests/Time/encoder_test.cpp b/Tests/Time/encoder_test.cpp
new file mode 100644
index 000000000..9d73cf27c
--- /dev/null
+++ b/Tests/Time/encoder_test.cpp
@@ -0,0 +1,139 @@
+#include <gtest/gtest.h>
+
+#include "HALAL/Services/Time/TimerWrapper.hpp"
+#include "ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp"
+
+namespace ST_LIB::TestErrorHandler {
+void reset();
+void set_fail_on_error(bool enabled);
+extern int call_count;
+} // namespace ST_LIB::TestErrorHandler
+
+namespace {
+
+constexpr ST_LIB::TimerDomain::Timer encoder_timer_decl{
+    ST_LIB::TimerRequest::GeneralPurpose32bit_2,
+    ST_LIB::TimerDomain::EMPTY_TIMER_NAME,
+    ST_LIB::TimerPin{
+        .af = ST_LIB::TimerAF::Encoder,
+        .pin = ST_LIB::PA0,
+        .channel = ST_LIB::TimerChannel::CHANNEL_1,
+    },
+    ST_LIB::TimerPin{
+        .af = ST_LIB::TimerAF::Encoder,
+        .pin = ST_LIB::PA1,
+        .channel = ST_LIB::TimerChannel::CHANNEL_2,
+    },
+};
+
+struct MockEncoder {
+    uint32_t counter = 1234;
+    uint32_t initial_counter = 1234;
+    bool direction = true;
+    int turn_on_calls = 0;
+    int turn_off_calls = 0;
+    int reset_calls = 0;
+
+    void turn_on() { ++turn_on_calls; }
+    void turn_off() { ++turn_off_calls; }
+    void reset() {
+        ++reset_calls;
+        counter = initial_counter;
+    }
+    uint32_t get_counter() { return counter; }
+    bool get_direction() { return direction; }
+    uint32_t get_initial_counter_value() { return initial_counter; }
+};
+
+using MockSensor = ST_LIB::EncoderSensor<MockEncoder, 4>;
+
+} // namespace
+
+class EncoderTest : public ::testing::Test {
+protected:
+    TIM_HandleTypeDef hal_tim{};
+    ST_LIB::TimerDomain::Instance instance{};
+    ST_LIB::TimerWrapper<encoder_timer_decl> wrapper{};
+
+    void SetUp() override {
+        ST_LIB::TestErrorHandler::reset();
+
+        TIM2_BASE->CNT = 0U;
+        TIM2_BASE->ARR = 0U;
+        TIM2_BASE->PSC = 0U;
+        TIM2_BASE->CR1 = 0U;
+
+        hal_tim = {};
+        hal_tim.Instance = TIM2_BASE;
+        hal_tim.State = HAL_TIM_STATE_READY;
+
+        instance.tim = TIM2_BASE;
+        instance.hal_tim = &hal_tim;
+        instance.timer_idx = 0U;
+
+        wrapper = ST_LIB::TimerWrapper<encoder_timer_decl>(&instance);
+        ST_LIB::Encoder<encoder_timer_decl>::init(&wrapper);
+        ST_LIB::Encoder<encoder_timer_decl>::turn_off();
+    }
+};
+
+TEST_F(EncoderTest, ResetUsesConfiguredInitialCounterValue) {
+    TIM2_BASE->CNT = 17U;
+
+    ST_LIB::Encoder<encoder_timer_decl>::reset();
+
+    EXPECT_EQ(TIM2_BASE->ARR, 55000U);
+    EXPECT_EQ(TIM2_BASE->CNT, ST_LIB::Encoder<encoder_timer_decl>::get_initial_counter_value());
+}
+
+TEST_F(EncoderTest, TurnOffKeepsTryingIfHALStopFails) {
+    ST_LIB::TestErrorHandler::set_fail_on_error(false);
+    ST_LIB::Encoder<encoder_timer_decl>::turn_on();
+
+    instance.hal_tim = nullptr;
+
+    ST_LIB::Encoder<encoder_timer_decl>::turn_off();
+    ST_LIB::Encoder<encoder_timer_decl>::turn_off();
+
+    EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 2);
+}
+
+TEST(EncoderSensorTest, ReadTreatsEncoderInitialCounterAsZeroPosition) {
+    MockEncoder encoder{};
+    double position = -1.0;
+    double speed = -1.0;
+    double acceleration = -1.0;
+    MockSensor::Direction direction = MockSensor::BACKWARDS;
+
+    MockSensor sensor(encoder, 0.5, 0.1, &direction, &position, &speed, &acceleration);
+
+    sensor.read();
+
+    EXPECT_DOUBLE_EQ(position, 0.0);
+    EXPECT_DOUBLE_EQ(speed, 0.0);
+    EXPECT_DOUBLE_EQ(acceleration, 0.0);
+    EXPECT_EQ(direction, MockSensor::FORWARD);
+}
+
+TEST(EncoderSensorTest, ResetForwardsToEncoderAndClearsHistory) {
+    MockEncoder encoder{};
+    encoder.counter = 1240U;
+
+    double position = 0.0;
+    double speed = 0.0;
+    double acceleration = 0.0;
+    MockSensor::Direction direction = MockSensor::BACKWARDS;
+
+    MockSensor sensor(encoder, 1.0, 1.0, &direction, &position, &speed, &acceleration);
+
+    sensor.read();
+    ASSERT_NE(position, 0.0);
+
+    sensor.reset();
+    sensor.read();
+
+    EXPECT_EQ(encoder.reset_calls, 1);
+    EXPECT_DOUBLE_EQ(position, 0.0);
+    EXPECT_DOUBLE_EQ(speed, 0.0);
+    EXPECT_DOUBLE_EQ(acceleration, 0.0);
+}