From 825b51eb55f3ecfb62b1b00eb2d5c8783e7bdd9c Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Wed, 30 Oct 2024 09:54:38 +0100
Subject: [PATCH] Set the minimum resolution for Windows clock

---
 .../BipedalLocomotion/System/StdClock.h       | 27 ++++++++++++++++++
 src/System/src/StdClock.cpp                   | 28 +++++++++++++++++++
 2 files changed, 55 insertions(+)

diff --git a/src/System/include/BipedalLocomotion/System/StdClock.h b/src/System/include/BipedalLocomotion/System/StdClock.h
index 78ea7af9b5..23a02f1559 100644
--- a/src/System/include/BipedalLocomotion/System/StdClock.h
+++ b/src/System/include/BipedalLocomotion/System/StdClock.h
@@ -60,6 +60,33 @@ class StdClock final : public IClock
      * threads to run.
      */
     void yield() final;
+
+private:
+
+    /**
+     * PrecisionScheduler is a class that allows to set the system timer resolution to the minimum
+     * value for higher precision. This class is used only on Windows systems.
+     */
+    struct PrecisionScheduler
+    {
+        /**
+         * Constructor.
+         * It sets the system timer resolution to the minimum value for higher precision.
+         * @note Only affects Windows systems.
+         */
+        PrecisionScheduler();
+
+        /**
+         * Destructor.
+         * It restores the system timer resolution to the default value.
+         * @note Only affects Windows systems.
+         */
+        ~PrecisionScheduler();
+    };
+
+    PrecisionScheduler m_precisionScheduler; /**< PrecisionScheduler object.
+                                                  It is used only on Windows systems. */
+
 };
 
 class StdClockFactory final : public ClockFactory
diff --git a/src/System/src/StdClock.cpp b/src/System/src/StdClock.cpp
index c2634850e4..7ead781f23 100644
--- a/src/System/src/StdClock.cpp
+++ b/src/System/src/StdClock.cpp
@@ -8,6 +8,10 @@
 #include <chrono>
 #include <thread>
 
+#if defined(_WIN32)
+#include <Windows.h>
+#endif
+
 #include <BipedalLocomotion/System/StdClock.h>
 
 using namespace BipedalLocomotion::System;
@@ -32,6 +36,30 @@ void StdClock::yield()
     std::this_thread::yield();
 }
 
+StdClock::PrecisionScheduler::PrecisionScheduler()
+{
+#if defined(_WIN32)
+    // Only affects Windows systems.
+    TIMECAPS tm; // Stores system timer capabilities.
+    // Get the minimum timer resolution supported by the system.
+    timeGetDevCaps(&tm, sizeof(TIMECAPS));
+    // Set the system timer resolution to the minimum value for higher precision.
+    timeBeginPeriod(tm.wPeriodMin);
+#endif
+}
+
+StdClock::PrecisionScheduler::~PrecisionScheduler()
+{
+#if defined(_WIN32)
+    // Only affects Windows systems.
+    TIMECAPS tm; // Stores system timer capabilities.
+    // Get the minimum timer resolution supported by the system.
+    timeGetDevCaps(&tm, sizeof(TIMECAPS));
+    // Restore the system timer resolution to the default value.
+    timeEndPeriod(tm.wPeriodMin);
+#endif
+}
+
 IClock& StdClockFactory::createClock()
 {
     // Create the singleton. Meyers' implementation. It is automatically threadsafe