From 395ded1c88b69aadbad820fa307dc236f62ad397 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 7 Mar 2022 16:39:51 +0000
Subject: [PATCH] Enable the verbosity in the time profiler machinery

---
 devices/wholeBodyDynamics/TimeProfiler.cpp    | 30 +++++++++++--------
 devices/wholeBodyDynamics/TimeProfiler.h      |  4 +--
 .../WholeBodyDynamicsDevice.cpp               |  2 +-
 3 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/devices/wholeBodyDynamics/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp
index 181de44..11997a0 100644
--- a/devices/wholeBodyDynamics/TimeProfiler.cpp
+++ b/devices/wholeBodyDynamics/TimeProfiler.cpp
@@ -14,6 +14,7 @@
 #include <numeric>
 #include <iomanip>
 
+#include <yarp/os/LogStream.h>
 
 #include "TimeProfiler.h"
 
@@ -28,7 +29,7 @@ std::string printTimerDescription(const std::string& name, const TimerHandler::T
        << "|" << std::setw(15) << std::setprecision(13) << description.averageDuration.count()
        << "|" << std::setw(10) << std::setprecision(8) << description.timer.getInfo().deadlineMiss
        << "|" << std::setw(15) << std::setprecision(13) << description.timer.getInfo().latestDeadlineMissDuration.count()
-       << "|";
+       << "|" << std::endl;
 
     return ss.str();
 };
@@ -66,6 +67,11 @@ void Timer::setExpectedDuration(const std::chrono::duration<double>& expectedDur
     m_exprectedDuration = expectedDuration;
 }
 
+const Timer::Info& Timer::getInfo() const
+{
+    return m_info;
+}
+
 void TimerHandler::setHorizon(unsigned int horizon)
 {
     m_horizon = horizon;
@@ -119,41 +125,41 @@ void TimerHandler::profiling()
         queue.push_back(duration);
 
         // keep the queue size equal to horizon
-        if (queue.size() >= m_horizon)
+        if (queue.size() > m_horizon)
         {
             queue.pop_front();
         }
 
         // this should never happen
-        assert(queue.size() == m_horizon);
+        assert(queue.size() <= m_horizon);
 
         timerDescription.averageDuration
-            = std::accumulate(std::next(queue.begin()), queue.end(), queue.front());
+	  = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) / double(queue.size());
     }
 
     if(m_verbosity)
     {
+      std::stringstream ss;
+      std::string output;
         if(m_verbosityCounter == 0)
         {
-            std::cout << "|" << std::setw(30) << "name            |"
+	    ss << "|" << std::setw(30) << "name            |"
                       << std::setw(15) << "tavarg (s)  |"
                       << std::setw(10) <<  "dm    |"
-                      << std::setw(15) << "tdm (s)   |" << std::endl;
-            std::cout << std::setfill('-') << "|" << std::setw(15) << "|"
+                      << std::setw(15) << "tdm (s)   |" << std::endl
+	       << "        " << std::setfill('-') << "|" << std::setw(30) << "|"
                       << std::setw(15) << "|"
                       << std::setw(10) <<  "|"
                       << std::setw(15) << "|" << std::endl;
 
+	    output = ss.str();
 
             for (const auto& [name, timerDescription] : m_timers)
             {
-                std::cout << printTimerDescription(name, timerDescription) << std::endl;
+                output += "        " + printTimerDescription(name, timerDescription);
             }
 
-            std::cout << std::setfill('-') << "|" << std::setw(30) << "|"
-                      << std::setw(15) << "|"
-                      << std::setw(10) <<  "|"
-                      << std::setw(15) << "|" << std::endl;
+	    yDebug() << output;
         }
 
         m_verbosityCounter++ ;
diff --git a/devices/wholeBodyDynamics/TimeProfiler.h b/devices/wholeBodyDynamics/TimeProfiler.h
index 60dbd73..4474c52 100644
--- a/devices/wholeBodyDynamics/TimeProfiler.h
+++ b/devices/wholeBodyDynamics/TimeProfiler.h
@@ -21,8 +21,8 @@ class Timer
         unsigned int deadlineMiss{0}; /**< Number of deadline miss */
         std::string name{"Timer"}; /**< Name associated to the timer */
         bool dealineMissDetected{false};
-        std::chrono::duration<double> latestDeadlineMissDuration; /**< Average duration. */
-        std::chrono::duration<double> duration; /**< Latest duration. */
+      std::chrono::duration<double> latestDeadlineMissDuration{0}; /**< Average duration. */
+      std::chrono::duration<double> duration{0}; /**< Latest duration. */
     };
 
     Timer(const std::string& name, const std::chrono::duration<double>& expectedDuration);
diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
index d3aeff2..6f0787a 100644
--- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
+++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -1639,7 +1639,7 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config)
     m_timerHandler.addTimer("all",
                             WholeBodyDynamics::Timer("all",
                                                      std::chrono::duration<double>(getPeriod())));
-
+    m_timerHandler.setVerbosity(true);
     yDebug() << "wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called.";
 
     return true;