text: // Copyright (c) Microsoft Corporation. All rights reserved.
  // Licensed under the MIT License.
  
  #ifndef common_utils_Utils_hpp
  #define common_utils_Utils_hpp
  
  #include "StrictMode.hpp"
  #include <chrono>
  #include <thread>
  #include <memory>
  #include <string>
  #include <cstdarg>
  #include <cstring>
  #include <array>
  #include <sstream>
  #include <fstream>
  #include <vector>
  #include <ctime>
  #include <random>
  #include <iomanip>
  #include <iostream>
  #include <limits>
  #include <queue>
  #include <bitset>
  #include "type_utils.hpp"
  
  #ifndef _WIN32
  #include <limits.h> // needed for CHAR_BIT used below
  #endif
  
  #ifndef _USE_MATH_DEFINES
  #define _USE_MATH_DEFINES
  #endif
  #include <math.h>
  //#include <cmath>
  
  #ifndef M_PIf
  #define M_PIf static_cast<float>(3.1415926535897932384626433832795028841972)
  #endif
  
  #ifndef M_PI
  #define M_PI static_cast<double>(3.1415926535897932384626433832795028841972)
  #endif
  
  #ifndef M_PIl
  #define M_PIl static_cast<long double>(3.1415926535897932384626433832795028841972)
  #endif
  
  #define EARTH_RADIUS (6378137.0f)
  
  /*
      This file is collection of routines that can be included in ANY project just
      by dropping in common_utils.hpp. Therefore there should not be any dependency 
      in the code below other than STL. The code should be able to compilable on
      all major platforms.
  */
  
  #ifndef _MSC_VER
  static int _vscprintf(const char * format, va_list pargs)
  {
      int retval;
      va_list argcopy;
      va_copy(argcopy, pargs);
      IGNORE_FORMAT_STRING_ON
      retval = vsnprintf(NULL, 0, format, argcopy);
      IGNORE_FORMAT_STRING_OFF
      va_end(argcopy);
      return retval;
  }
  #endif
  
  // Call this on a function parameter to suppress the unused paramter warning
  template <class T> inline 
  void unused(T const & result) { static_cast<void>(result); }
  
  namespace common_utils {
  
  class Utils {
  private:
      typedef std::chrono::system_clock system_clock;
      typedef std::chrono::steady_clock steady_clock;
      typedef std::string string;
      typedef std::stringstream stringstream;
      //this is not required for most compilers
      typedef unsigned int uint;
      template <typename T>
      using time_point = std::chrono::time_point<T>;    
  
  
  public:
      class Logger {
      public:
          virtual void log(int level, const std::string& message)
          {
              if (level >= 0)
                  std::cout << message << std::endl;
              else
                  std::cerr << message << std::endl;
          }
      };
      
      static void enableImmediateConsoleFlush() {
          //disable buffering
          setbuf(stdout, NULL);
      }
  
      template<typename T>
      static T getRandomFromGaussian(T stddev = 1, T mean = 0)
      {
          static std::default_random_engine random_gen;
          static std::normal_distribution<T> gaussian_dist(0.0f, 1.0f);
  
          return gaussian_dist(random_gen) * stddev + mean;
      }
      
      static constexpr double degreesToRadians(double degrees) {
          return static_cast<double>(M_PIl * degrees / 180.0);
      }
      static constexpr float degreesToRadians(float degrees) {
          return static_cast<float>(M_PI * degrees / 180.0f);
      }
      static constexpr double radiansToDegrees(double radians) {
          return static_cast<double>(radians * 180.0 / M_PIl);
      }
      static constexpr float radiansToDegrees(float radians) {
          return static_cast<float>(radians * 180.0f / M_PI);
      }
  
      static bool startsWith(const string& s, const string& prefix) {
          return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0;
      }
  
      template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
      static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal& default_val)
      {
          typename TContainer<TKey, TVal, Args...>::const_iterator it = m.find(key);
          if (it == m.end())
              return default_val;
          return it->second;
      }
  
      template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
      static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key)
      {
          static TVal default_val;
          return findOrDefault(m, key, default_val);
      }
  
      static Logger* getSetLogger(Logger* logger = nullptr)
      {
          static Logger logger_default_;
          static Logger* logger_;
  
          if (logger != nullptr)
              logger_ = logger;
          else if (logger_ == nullptr)
              logger_ = &logger_default_;
  
          return logger_;
      }
  
      static constexpr int kLogLevelInfo = 0;
      static constexpr int kLogLevelWarn = -1;
      static constexpr int kLogLevelError = -2;
      static void log(std::string message, int level = kLogLevelInfo)
      {
          if (level >= getSetMinLogLevel())
              getSetLogger()->log(level, message);
      }
      static int getSetMinLogLevel(bool set_or_get = false, 
          int set_min_log_level = std::numeric_limits<int>::min())
      {
          static int min_log_level = std::numeric_limits<int>::min();
          if (set_or_get)
              min_log_level = set_min_log_level;
          
          return min_log_level;
      }
  
      template <typename T>
      static int sign(T val) {
          return T(0) < val ? 1 : (T(0) > val ? -1 : 0);
      }   
  
      /// Limits absolute value whole preserving sign
      template <typename T> 
      static T limitAbsValue(T val, T min_value, T max_value) {
          T val_abs = std::abs(val);
          T val_limited = std::max(val_abs, min_value);
          val_limited = std::min(val_limited, max_value);
          return sign(val) * val_limited;
      }
      
      /// Limits absolute value whole preserving sign
      template <typename T> 
      static T clip(T val, T min_value, T max_value) {
          return std::max(min_value, std::min(val, max_value));
      }
  
      template<typename Range>
      static const string printRange(Range&& range, const string& delim = ", ",
          const string& prefix="(", const string& suffix=")")
      {
          return printRange(std::begin(range), std::end(range), delim, prefix, suffix);
      }
      template<typename Iterator>
      static const string printRange(Iterator start, Iterator last, const string& delim = ", ",
          const string& prefix="(", const string& suffix=")")
      {
          stringstream ss;
          ss << prefix;
  
          for (Iterator i = start; i != last; ++i)
          {
              if (i != start)
                  ss << delim;
              ss << *i;
          }
  
          ss << suffix;
          return ss.str();         
      }
  
      static std::string getFileExtension(const string& str)
      {
          int len = static_cast<int>(str.size());
          const char* ptr = str.c_str();
          int i = 0;
          for (i = len - 1; i >= 0; i--)
          {
              if (ptr[i] == '.')
                  break;
          }
          if (i < 0) return "";
          return str.substr(i, len - i);
      }
  
      static string formatNumber(double number, int digits_after_decimal = -1, int digits_before_decimal = -1, bool sign_always = false)
      {
          std::string format_string = "%";
          if (sign_always)
              format_string += "+";
          if (digits_before_decimal >= 0)
              format_string += "0" + std::to_string(digits_before_decimal + std::max(digits_after_decimal, 0) + 1);
          if (digits_after_decimal >= 0)
              format_string += "." + std::to_string(digits_after_decimal);
          format_string += "f";
  
          return stringf(format_string.c_str(), number);
      }
  
      static string stringf(const char* format, ...)
      {
          va_list args;
          va_start(args, format);
  
          IGNORE_FORMAT_STRING_ON
          auto size = _vscprintf(format, args) + 1U;
          IGNORE_FORMAT_STRING_OFF
          std::unique_ptr<char[]> buf(new char[size] ); 
  
          #ifndef _MSC_VER
              IGNORE_FORMAT_STRING_ON
              vsnprintf(buf.get(), size, format, args);
              IGNORE_FORMAT_STRING_OFF
          #else
              vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
          #endif
  
          va_end(args);            
  
          return string(buf.get());
      }
  
      static string trim(const string& str, char ch)
      {
          int len = static_cast<int>(str.size());
          const char* ptr = str.c_str();
          int i = 0;
          for (i = 0; i < len; i++)
          {
              if (ptr[i] != ch)
                  break;
          }
          int j = 0;
          for (j = len - 1; j >= i; j--)
          {
              if (ptr[j] != ch)
                  break;
          }
          if (i > j) return "";
          return str.substr(i, j - i + 1);
      }
      static std::vector<std::string> split(const string& s, const char* splitChars, int numSplitChars)
      {
          auto start = s.begin();
          std::vector<string> result;
          for (auto it = s.begin(); it != s.end(); it++)
          {
              char ch = *it;
              bool split = false;
              for (int i = 0; i < numSplitChars; i++)
              {
                  if (ch == splitChars[i]) {
                      split = true;
                      break;
                  }
              }
              if (split)
              {
                  if (start < it)
                  {
                      result.push_back(string(start, it));
                  }
                  start = it;
                  start++;
              }
          }
          if (start < s.end())
          {
              result.push_back(string(start, s.end()));
          }
          return result;
      }
  
      // split a line into tokens using any of the given separators as token separators.
      // this method also understands quoted string literals (either single or double quotes) and returns the
      // quoted value without the quotes, and this value can contain separators.
      static std::vector<std::string> tokenize(const std::string& line, const char* separators, int numSeparators)
      {
          auto start = line.begin();
          std::vector<std::string> result;
          auto end = line.end();
          for (auto it = line.begin(); it != end; )
          {
              bool split = false;
              char ch = *it;
              if (ch == '\'' || ch == '"') {
                  // skip quoted literal
                  if (start < it)
                  {
                      result.push_back(string(start, it));
                  }
                  it++;
                  start = it;
                  for (; it != end; it++) {					
                      if (*it == ch) {
                          break;
                      } 
                  }
                  split = true;
              } else {
                  for (int i = 0; i < numSeparators; i++) {
                      if (ch == separators[i]) {
                          split = true;
                          break;
                      }
                  }
              }
              if (split)
              {
                  if (start < it)
                  {
                      result.push_back(string(start, it));
                  }
                  start = it;
                  if (start < end) start++;
              }
              if (it != end) {
                  it++;
              }
          }
          if (start < end)
          {
              result.push_back(string(start, end));
          }
          return result;
      }
  
      static string toLower(const string& str)
      {
          auto len = str.size();
          std::unique_ptr<char[]> buf(new char[len + 1U]);
          str.copy(buf.get(), len, 0);
          buf[len] = '\0';
  #ifdef _WIN32
          _strlwr_s(buf.get(), len + 1U);
  #else
          char* p = buf.get();
          for (int i = len; i > 0; i--)
          {
              *p = tolower(*p);
              p++;
          }
          *p = '\0';
  #endif
          string lower = buf.get();
          return lower;
      }
      //http://stackoverflow.com/a/28703383/207661
      template <typename R>
      static constexpr R bitmask(unsigned int const onecount)
      {
          //  return (onecount != 0)
          //      ? (static_cast<R>(-1) >> ((sizeof(R) * CHAR_BIT) - onecount))
          //      : 0;
          return static_cast<R>(-(onecount != 0))
              & (static_cast<R>(-1) >> ((sizeof(R) * CHAR_BIT) - onecount));
      }
  
      static void cleanupThread(std::thread& th)
      {
          if (th.joinable()) {
              Utils::log("thread was cleaned up!", kLogLevelWarn);
              th.detach();
          }
      }
  
      static inline int floorToInt(float x)
      {
          return static_cast<int> (std::floor(x));
      }
  
      template<typename T>
      static constexpr T nan() {
          return std::numeric_limits<T>::quiet_NaN();
      }
      template<typename T>
      static constexpr T max() {
          return std::numeric_limits<T>::max();
      }
      template<typename T>
      static constexpr T min() {
          return std::numeric_limits<T>::min();
      }
  
      template<typename T>
      static void setValue(T arr[], size_t length, const T& val)
      {
          std::fill(arr, arr + length, val);
      }
  
      template<typename T, size_t N>
      static void setValue(T (&arr)[N], const T& val)
      {
          std::fill(arr, arr+N, val);
      }
  
      template< class T, size_t N>
      static std::size_t length(const T(&)[N])
      {
          return N;
      };
  
      static void saveToFile(string file_name, const char* data, uint size)
      {
          std::ofstream file(file_name, std::ios::binary);
          file.write(data, size);
      }
      template<typename Container>
      static typename std::enable_if<type_utils::is_container<Container>::value, void>::type
      append(Container& to, const Container& from)
      {
          using std::begin;
          using std::end;
          to.insert(end(to), begin(from), end(from));
      }
      template<typename Container>
      static typename std::enable_if<type_utils::is_container<Container>::value, void>::type
          copy(const Container& from, Container& to)
      {
          using std::begin;
          using std::end;
          std::copy(begin(from), end(from), begin(to));
      }
      template<typename T>
      static void copy(const T* from, T* to, uint count)
      {
          std::copy(from, from + count, to);
      }
  
      static const char* to_string(time_point<steady_clock> t)
      {
          time_t tt = system_clock::to_time_t(std::chrono::time_point_cast<system_clock::duration>(system_clock::now() + (t - steady_clock::now())));
          return ctime(&tt);
      }
  
      static time_point<system_clock> now()
      {
          return system_clock::now();
      }
  
      static std::time_t to_time_t(const std::string& str, bool is_dst = false, const std::string& format = "%Y-%m-%d %H:%M:%S")
      {
          std::tm t;
          t.tm_isdst = is_dst ? 1 : 0;
          std::istringstream ss(str);
          ss >> std::get_time(&t, format.c_str());
          return mktime(&t);
  
          /* GCC doesn't implement put_time yet
          stringstream ss;
          ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d-%H-%M-%S");
          return ss.str();
          */
      }
  
      static string to_string(time_t tt, const char* format = "%Y-%m-%d-%H-%M-%S")
      {
          char str[1024];
          if (std::strftime(str, sizeof(str), format, std::localtime(&tt)))
              return string(str);
          else return string();
      }
  
      static string to_string(time_point<system_clock> time, const char* format = "%Y-%m-%d-%H-%M-%S")
      {
          time_t tt = system_clock::to_time_t(time);
          char str[1024];
          if (std::strftime(str, sizeof(str), format, std::localtime(&tt)))
              return string(str);
          else return string();
      }
      static string getLogFileTimeStamp()
      {
          return to_string(now(), "%Y%m%d%H%M%S");
      }
      static int to_integer(std::string s)
      {
          return atoi(s.c_str());
      }
      static string getEnv(const string& var)
      {
          char* ptr = std::getenv(var.c_str());
          return ptr ? ptr : "";
      }
  
      static uint64_t getUnixTimeStamp(const std::time_t* t = nullptr)
      {
          //if specific time is not passed then get current time
          std::time_t st = t == nullptr ? std::time(nullptr) : *t;
          auto secs = static_cast<std::chrono::seconds>(st).count();
          return static_cast<uint64_t>(secs);
      }
  
      //high precision time in seconds since epoch
      static double getTimeSinceEpochSecs(std::chrono::system_clock::time_point* t = nullptr)
      {
          using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++
          return std::chrono::duration<double>((t != nullptr ? *t : Clock::now() ).time_since_epoch()).count();
      }
      static uint64_t getTimeSinceEpochNanos(std::chrono::system_clock::time_point* t = nullptr)
      {
          using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++
          return std::chrono::duration_cast<std::chrono::nanoseconds>(
              (t != nullptr ? *t : Clock::now())
                  .time_since_epoch()).
              count();  
      }
  
      template<typename T>
      static void clear(std::queue<T> &q, size_t max_elements = SIZE_MAX)
      {
          while(!q.empty() && max_elements > 0) {
              q.pop();
              --max_elements;
          }
      }
  
      template<typename T>
      static const std::vector<T>& emptyVector()
      {
          static const std::vector<T> empty_vector;
          return empty_vector;
      }
  
      static const std::string& emptyString()
      {
          static std::string empty = "";
          return empty;
      }
  
      static constexpr float kelvinToCelcius(float kelvin)
      {
          return kelvin - 273.15f;
      }
      static constexpr float celciusToKelvin(float celcius)
      {
          return celcius + 273.15f;
      }
  
  
      //implements relative method - do not use for comparing with zero
      //use this most of the time, tolerance needs to be meaningful in your context
      template<typename TReal>
      static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
      {
          TReal diff = std::fabs(a - b);
          if (diff <= tolerance)
              return true;
  
          if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
              return true;
  
          return false;
      }
  
      template<typename TReal>
      static constexpr TReal epsilon()
      {
          return std::numeric_limits<TReal>::epsilon();
      }
  
      //supply tolerance that is meaningful in your context
      //for example, default tolerance may not work if you are comparing double with float
      template<typename TReal>
      static bool isApproximatelyZero(TReal a, TReal tolerance = epsilon<TReal>())
      {
          if (std::fabs(a) <= tolerance)
              return true;
          return false;
      }
  
  
      //use this when you want to be on safe side
      //for example, don't start rover unless signal is above 1
      template<typename TReal>
      static bool isDefinitelyLessThan(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
      {
          TReal diff = a - b;
          if (diff < tolerance)
              return true;
  
          if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
              return true;
  
          return false;
      }
      template<typename TReal>
      static bool isDefinitelyGreaterThan(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
      {
          TReal diff = a - b;
          if (diff > tolerance)
              return true;
  
          if (diff > std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
              return true;
  
          return false;
      }
  
      //implements ULP method
      //use this when you are only concerned about floating point precision issue
      //for example, if you want to see if a is 1.0 by checking if its within
      //10 closest representable floating point numbers around 1.0.
      template<typename TReal>
      static bool isWithinPrecisionInterval(TReal a, TReal b, unsigned int interval_size = 1)
      {
          TReal min_a = a - (a - std::nextafter(a, std::numeric_limits<TReal>::lowest())) * interval_size;
          TReal max_a = a + (std::nextafter(a, std::numeric_limits<TReal>::max()) - a) * interval_size;
  
          return min_a <= b && max_a >= b;
      }
  
      static void DebugBreak()
      {
  #ifdef _MSC_VER
          __debugbreak();
  #else
          //TODO: Use GCC and Clang version from https://github.com/scottt/debugbreak
  #endif
      }
  
      //convert strongly typed enum to underlying scaler types
      template <typename E>
      static constexpr typename std::underlying_type<E>::type toNumeric(E e) {
          return static_cast<typename std::underlying_type<E>::type>(e);
      }
      template <typename E>
      static constexpr E toEnum(typename std::underlying_type<E>::type u) {
          return static_cast<E>(u);
      }
  
      // check whether machine is little endian
      static bool isLittleEndian()
      {
          int intval = 1;
          unsigned char *uval = reinterpret_cast<unsigned char *>(&intval);
          return uval[0] == 1;
      }
  
      static void writePfmFile(const float * const image_data, int width, int height, std::string path, float scalef=1)
      {
          std::fstream file(path.c_str(), std::ios::out | std::ios::binary);
  
          std::string bands;
          float fvalue;       // scale factor and temp value to hold pixel value
          bands = "Pf";       // grayscale
  
          // sign of scalefact indicates endianness, see pfm specs
          if(isLittleEndian())
              scalef = -scalef;
  
          // insert header information 
          file << bands   << "\n";
          file << width   << " ";
          file << height  << "\n";
          file << scalef  << "\n";
  
          if(bands == "Pf"){          // handle 1-band image 
              for (int i=0; i < height; i++) {
                  for(int j=0; j < width; ++j){
                      fvalue = image_data[i * width + j];
                      file.write(reinterpret_cast<char *>(&fvalue), sizeof(fvalue));
                  }
              }
          }
      }
  
      template<typename T>
      static std::string toBinaryString(const T& x)
      {
          std::stringstream ss;
          ss << std::bitset<sizeof(T) * 8>(x);
          return ss.str();
      }
  };
  
  } //namespace
  #endif
  