From 09a87b92821cdf2939e285dc4cdee557ec3b3029 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 7 Mar 2022 22:11:47 +0100
Subject: [PATCH] Print all the timers when a deadline miss is detected

---
 devices/wholeBodyDynamics/TimeProfiler.cpp | 72 +++++++++++++---------
 1 file changed, 44 insertions(+), 28 deletions(-)

diff --git a/devices/wholeBodyDynamics/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp
index 11997a0..7ede91b 100644
--- a/devices/wholeBodyDynamics/TimeProfiler.cpp
+++ b/devices/wholeBodyDynamics/TimeProfiler.cpp
@@ -8,11 +8,11 @@
 
 #include <cassert>
 #include <chrono>
+#include <iomanip>
 #include <iostream>
+#include <numeric>
 #include <sstream>
 #include <string>
-#include <numeric>
-#include <iomanip>
 
 #include <yarp/os/LogStream.h>
 
@@ -20,16 +20,14 @@
 
 using namespace WholeBodyDynamics;
 
-
-
-std::string printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description)
+std::string
+printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description)
 {
     std::stringstream ss;
-    ss << "|" << std::setw(30) << name
-       << "|" << 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;
+    ss << "|" << std::setw(30) << name << "|" << 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();
 };
@@ -104,7 +102,6 @@ bool TimerHandler::toc(const std::string& key)
         return false;
     }
 
-
     it->second.timer.toc();
     return true;
 }
@@ -116,10 +113,13 @@ void TimerHandler::setVerbosity(bool verbosity)
 
 void TimerHandler::profiling()
 {
+    bool deadlineMissDetected = false;
     for (auto& [name, timerDescription] : m_timers)
     {
         const auto& duration = timerDescription.timer.getInfo().duration;
 
+        deadlineMissDetected = deadlineMissDetected || timerDescription.timer.getInfo().dealineMissDetected;
+
         // this automatically create the element if does bot exist
         auto& queue = m_durations[name];
         queue.push_back(duration);
@@ -134,36 +134,52 @@ void TimerHandler::profiling()
         assert(queue.size() <= m_horizon);
 
         timerDescription.averageDuration
-	  = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) / double(queue.size());
+            = std::accumulate(std::next(queue.begin()), queue.end(), queue.front())
+              / double(queue.size());
     }
 
-    if(m_verbosity)
+    if (m_verbosity)
     {
-      std::stringstream ss;
-      std::string output;
-        if(m_verbosityCounter == 0)
+        if (m_verbosityCounter == 0)
         {
-	    ss << "|" << std::setw(30) << "name            |"
-                      << std::setw(15) << "tavarg (s)  |"
-                      << std::setw(10) <<  "dm    |"
-                      << std::setw(15) << "tdm (s)   |" << std::endl
-	       << "        " << std::setfill('-') << "|" << std::setw(30) << "|"
-                      << std::setw(15) << "|"
-                      << std::setw(10) <<  "|"
-                      << std::setw(15) << "|" << std::endl;
+            std::stringstream ss;
+            std::string output;
+
+            ss << "|" << std::setw(30) << "name            |" << std::setw(15) << "tavarg (s)  |"
+               << std::setw(10) << "dm    |" << 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();
+            output = ss.str();
 
             for (const auto& [name, timerDescription] : m_timers)
             {
                 output += "        " + printTimerDescription(name, timerDescription);
             }
 
-	    yDebug() << output;
+            yDebug() << output;
+        }
+
+        if (deadlineMissDetected)
+        {
+            std::stringstream ss;
+            std::string output;
+
+            ss << "---------------------------------- Deadline miss detected ----------------------------------" << std::endl;
+            ss << "|" << std::setw(30) << "name            |" << std::setw(15) << "t (s)  |" << std::endl
+               << "        " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) << std::endl;
+
+            for (const auto& [name, timerDescription] : m_timers)
+            {
+                ss << "|" << std::setw(30) << name << "|" << std::setw(15) << std::setprecision(13)
+                   << timerDescription.timer.getInfo().duration.count() << std::endl;
+            }
+
+            yDebug() << ss.str();
         }
 
-        m_verbosityCounter++ ;
-        if(m_verbosityCounter == m_horizon)
+        m_verbosityCounter++;
+        if (m_verbosityCounter == m_horizon)
             m_verbosityCounter = 0;
     }
 }