From c091e41805693a91ed7c4990354e9e7cf7c7c775 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Thu, 23 Jun 2022 06:33:15 +0200
Subject: [PATCH] Enable conditional formatting of timestamp. This fixes #5.

---
 src/Arduino_DebugUtils.cpp | 44 ++++++++++++++++++++++++++++++++++++--
 src/Arduino_DebugUtils.h   |  4 ++++
 2 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/src/Arduino_DebugUtils.cpp b/src/Arduino_DebugUtils.cpp
index e2818e5..3ffde7b 100644
--- a/src/Arduino_DebugUtils.cpp
+++ b/src/Arduino_DebugUtils.cpp
@@ -36,6 +36,7 @@ Arduino_DebugUtils::Arduino_DebugUtils() {
   timestampOff();
   newlineOn();
   debugLabelOff();
+  formatTimestampOff();
   setDebugLevel(DEFAULT_DEBUG_LEVEL);
   setDebugOutputStream(DEFAULT_OUTPUT_STREAM);
 }
@@ -72,6 +73,14 @@ void Arduino_DebugUtils::debugLabelOff() {
   _print_debug_label = false;
 }
 
+void Arduino_DebugUtils::formatTimestampOn() {
+  _format_timestamp_on = true;
+}
+
+void Arduino_DebugUtils::formatTimestampOff() {
+  _format_timestamp_on = false;
+}
+
 void Arduino_DebugUtils::timestampOn() {
   _timestamp_on = true;
 }
@@ -146,8 +155,39 @@ void Arduino_DebugUtils::vPrint(char const * fmt, va_list args) {
 
 void Arduino_DebugUtils::printTimestamp()
 {
-  char timestamp[20];
-  snprintf(timestamp, 20, "[ %lu ] ", millis());
+  char timestamp[32];
+
+  if (_format_timestamp_on)
+  {
+    auto    const  msCount      = millis();
+
+    uint16_t const milliseconds = msCount % 1000;           // ms remaining when converted to seconds
+    uint16_t const allSeconds   = msCount / 1000;           // total number of seconds to calculate remaining values
+
+    uint16_t const hours            = allSeconds / 3600;    // convert seconds to hours
+    uint16_t const secondsRemaining = allSeconds % 3600;    // seconds left over
+
+    uint16_t const minutes  = secondsRemaining / 60 ;       // convert seconds left over to minutes
+    uint16_t const seconds  = secondsRemaining % 60;        // seconds left over
+
+    snprintf(timestamp, sizeof(timestamp),                  // "prints" formatted output to a char array (string)
+                "[ "
+                "%02d:"   //HH:
+                "%02d:"   //MM:
+                "%02d."   //SS.
+                "%03d"    //MMM
+                " ] ",
+                hours,
+                minutes,
+                seconds,
+                milliseconds
+            );
+  }
+  else
+  {
+    snprintf(timestamp, sizeof(timestamp), "[ %lu ] ", millis());
+  }
+
   _debug_output_stream->print(timestamp);
 }
 
diff --git a/src/Arduino_DebugUtils.h b/src/Arduino_DebugUtils.h
index 2fe7392..335f1c5 100644
--- a/src/Arduino_DebugUtils.h
+++ b/src/Arduino_DebugUtils.h
@@ -64,6 +64,9 @@ class Arduino_DebugUtils {
     void debugLabelOn();
     void debugLabelOff();
 
+    void formatTimestampOn();
+    void formatTimestampOff();
+
     void print(int const debug_level, const char * fmt, ...);
     void print(int const debug_level, const __FlashStringHelper * fmt, ...);
 
@@ -73,6 +76,7 @@ class Arduino_DebugUtils {
     bool      _timestamp_on;
     bool      _newline_on;
     bool      _print_debug_label;
+    bool      _format_timestamp_on;
     int       _debug_level;
     Stream *  _debug_output_stream;