Add string to time function

include/ignition/math/Helpers.hh

@@ -17,16 +17,19 @@
 #ifndef IGNITION_MATH_FUNCTIONS_HH_
 #define IGNITION_MATH_FUNCTIONS_HH_
 
+#include <algorithm>
 #include <chrono>
 #include <cmath>
-#include <algorithm>
+#include <cstdint>
+#include <iomanip>
+#include <iostream>
 #include <limits>
+#include <regex>
+#include <sstream>
 #include <string>
-#include <iostream>
-#include <vector>
 #include <tuple>
 #include <utility>
-#include <cstdint>
+#include <vector>
 
 #include <ignition/math/config.hh>
 #include "ignition/math/Export.hh"
@@ -727,6 +730,43 @@ namespace ignition
       }
     }
 
+    /// \brief Convert a std::chrono::steady_clock::time_point to a seconds and
+    /// nanoseconds pair.
+    /// \param[in] _time The time point to convert.
+    /// \return A pair where the first element is the number of seconds and
+    /// the second is the number of nanoseconds.
+    inline std::pair<int64_t, int64_t> timePointToSecNsec(
+        const std::chrono::system_clock::time_point &_time)
+    {
+      auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        _time.time_since_epoch());
+      auto now_s = std::chrono::duration_cast<std::chrono::seconds>(
+        _time.time_since_epoch());
+      int64_t seconds = std::chrono::duration_cast<std::chrono::seconds>(
+        _time.time_since_epoch()).count();
+      int64_t nanoseconds = std::chrono::duration_cast
+        <std::chrono::nanoseconds>(now_ns - now_s).count();
+      return {seconds, nanoseconds};
+    }
+
+    /// \brief Convert to a seconds and nanoseconds to
+    /// std::chrono::steady_clock::time_point.
+    /// \param[in] _sec The seconds to convert.
+    /// \param[in] _nanosec The nanoseconds to convert.
+    /// \return A std::chrono::system_clock::time_poin based on the number of
+    /// seconds and
+    /// the number of nanoseconds.
+    inline std::chrono::system_clock::time_point secNsecToTimePoint(
+        const uint64_t &_sec, const uint64_t &_nanosec)
+    {
+      auto duration = std::chrono::seconds(_sec) + std::chrono::nanoseconds(
+        _nanosec);
+      auto result =
+        std::chrono::system_clock::from_time_t(0);
+      result += duration;
+      return result;
+    }
+
     /// \brief Convert a std::chrono::steady_clock::duration to a seconds and
     /// nanoseconds pair.
     /// \param[in] _dur The duration to convert.
@@ -740,6 +780,170 @@ namespace ignition
       return {s.count(), ns.count()};
     }
 
+    // TODO(anyone): Replace this with std::chrono::days.
+    /// This will exist in C++-20
+    typedef std::chrono::duration<uint64_t, std::ratio<86400>> days;
+
+    /// \brief break down durations
+    /// \param[in] d Duration to breaw down
+    /// \return A tuple based on the durations specified
+    template<class...Durations, class DurationIn>
+    std::tuple<Durations...> break_down_durations(DurationIn d) {
+      std::tuple<Durations...> retval;
+      using discard = int[];
+      (void)discard{0, (void((
+        (std::get<Durations>(retval) =
+          std::chrono::duration_cast<Durations>(d)),
+        (d -= std::chrono::duration_cast<DurationIn>(
+          std::get<Durations>(retval))))), 0)...};
+      return retval;
+    }
+
+    /// \brief Convert a std::chrono::steady_clock::duration to a string
+    /// \param[in] _point The std::chrono::system_clock::time_point to convert.
+    /// \return A string formatted with the time_point
+    inline std::string timePointToString(
+      const std::chrono::system_clock::time_point &_point)
+    {
+      auto duration = _point - std::chrono::system_clock::from_time_t(0);
+      auto clean_duration = break_down_durations<days,
+                                                 std::chrono::hours,
+                                                 std::chrono::minutes,
+                                                 std::chrono::seconds,
+                                                 std::chrono::milliseconds>(
+                                                   duration);
+      std::ostringstream output_string;
+      output_string << std::get<0>(clean_duration).count() << " "
+                    << std::setw(2) << std::setfill('0')
+                    << std::get<1>(clean_duration).count() << ":"
+                    << std::setw(2) << std::setfill('0')
+                    << std::get<2>(clean_duration).count() << ":"
+                    << std::setw(2) << std::setfill('0')
+                    << std::setprecision(5)
+                    << std::get<3>(clean_duration).count() +
+                       std::get<4>(clean_duration).count()/1000.0;
+      return output_string.str();
+    }
+
+    /// \brief Convert a string to a std::chrono::system_clock::time_point
+    /// \param[in] _timeString The string to convert in general format
+    /// "dd hh:mm:ss.nnn" where n is millisecond value
+    /// \return A std::chrono::system_clock::time_point containing the
+    /// string's time value
+    inline std::chrono::system_clock::time_point stringToTimePoint(
+        const std::string &_timeString)
+    {
+      std::chrono::system_clock::time_point timePoint =
+        std::chrono::system_clock::from_time_t(-1);
+
+      // The following regex takes a time string in the general format of
+      // "dd hh:mm:ss.nnn" where n is milliseconds, if just one number is
+      // provided, it is assumed to be seconds
+      std::regex time_regex(
+          "^([0-9]+ ){0,1}"                       // day:
+                                                  // Any positive integer
+
+          "(?:([1-9]:|[0-1][0-9]:|2[0-3]:){0,1}"  // hour:
+                                                  // 1-9:
+                                                  // 01-19:
+                                                  // 20-23:
+
+          "([0-9]:|[0-5][0-9]:)){0,1}"            // minute:
+                                                  // 0-9:
+                                                  // 00-59:
+
+          "(?:([0-9]|[0-5][0-9]){0,1}"            // second:
+                                                  // 0-9
+                                                  // 00-59
+
+          "(\\.[0-9]{1,3}){0,1})$");              // millisecond:
+                                                  // .0 - .9
+                                                  // .00 - .99
+                                                  // .000 - 0.999
+      std::smatch matches;
+
+      // `matches` should always be a size of 6 as there are 6 matching
+      // groups in the regex.
+      // 1. The whole regex
+      // 2. The days
+      // 3. The hours
+      // 4. The minutes
+      // 5. The seconds
+      // 6. The milliseconds
+      // We can also index them as such below.
+      // Note that the space will remain in the day match, the colon
+      // will remain in the hour and minute matches, and the period will
+      // remain in the millisecond match
+      if (!std::regex_search(_timeString, matches, time_regex) ||
+          matches.size() != 6)
+        return timePoint;
+
+      uint64_t numberDays = 0;
+      uint64_t numberHours = 0;
+      uint64_t numberMinutes = 0;
+      uint64_t numberSeconds = 0;
+      uint64_t numberMilliseconds = 0;
+      std::string dayString = matches[1];
+      std::string hourString = matches[2];
+      std::string minuteString = matches[3];
+      std::string secondString = matches[4];
+      std::string millisecondString = matches[5];
+
+      // Days are the only unbounded number, so check first to see if stoi
+      // runs successfully
+      if (!dayString.empty())
+      {
+        // Erase the space
+        dayString.erase(dayString.length() - 1);
+        try
+        {
+          numberDays = std::stoi(dayString);
+        }
+        catch (const std::out_of_range &oor)
+        {
+          return timePoint;
+        }
+      }
+
+      if (!hourString.empty())
+      {
+        // Erase the colon
+        hourString.erase(hourString.length() - 1);
+        numberHours = std::stoi(hourString);
+      }
+
+      if (!minuteString.empty())
+      {
+        // Erase the colon
+        minuteString.erase(minuteString.length() - 1);
+        numberMinutes = std::stoi(minuteString);
+      }
+
+      if (!secondString.empty())
+      {
+        numberSeconds = std::stoi(secondString);
+      }
+
+      if (!millisecondString.empty())
+      {
+        // Erase the period
+        millisecondString.erase(0, 1);
+        // Multiplier because "4" = 400 ms, "04" = 40 ms, and "004" = 4 ms
+        numberMilliseconds = std::stoi(millisecondString) *
+          (1000 / pow(10, millisecondString.length()));
+      }
+
+      timePoint = std::chrono::system_clock::from_time_t(0);
+      auto duration = std::chrono::milliseconds(numberMilliseconds) +
+        std::chrono::seconds(numberSeconds) +
+        std::chrono::minutes(numberMinutes) +
+        std::chrono::hours(numberHours) +
+        std::chrono::hours(24 * numberDays);
+      timePoint += duration;
+
+      return timePoint;
+    }
+
     // Degrade precision on Windows, which cannot handle 'long double'
     // values properly. See the implementation of Unpair.
     // 32 bit ARM processors also define 'long double' to be the same