logger.h

/*
 * Copyright 2015, 2019
 * LAAS-CNRS
 * Andrea Del Prete, François Bailly, Olivier Stasse
 *
 */

#ifndef __dynamic_graph_logger_H__
#define __dynamic_graph_logger_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(logger_EXPORTS)
#define LOGGER_EXPORT __declspec(dllexport)
#else
#define LOGGER_EXPORT __declspec(dllimport)
#endif
#else
#define LOGGER_EXPORT
#endif

namespace dynamicgraph {

enum MsgType {
  MSG_TYPE_TYPE_BITS = 1 << 0 | 1 << 1 | 1 << 2 | 1 << 3, // 15
  MSG_TYPE_STREAM_BIT = 1 << 4,                           // 16

  MSG_TYPE_DEBUG = 1 << 3,                                          // 1
  MSG_TYPE_INFO = 1 << 2,                                           // 2
  MSG_TYPE_WARNING = 1 << 1,                                        // 4
  MSG_TYPE_ERROR = 1 << 0,                                          // 8
  MSG_TYPE_DEBUG_STREAM = MSG_TYPE_DEBUG | MSG_TYPE_STREAM_BIT,     // 17
  MSG_TYPE_INFO_STREAM = MSG_TYPE_INFO | MSG_TYPE_STREAM_BIT,       // 18
  MSG_TYPE_WARNING_STREAM = MSG_TYPE_WARNING | MSG_TYPE_STREAM_BIT, // 20
  MSG_TYPE_ERROR_STREAM = MSG_TYPE_ERROR | MSG_TYPE_STREAM_BIT      // 24
};
} // namespace dynamicgraph

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <map>
#include <fstream>
#include <iomanip> // std::setprecision
#include <sstream>

#include <boost/assign.hpp>
#include <boost/preprocessor/stringize.hpp>

#include <dynamic-graph/deprecated.hh>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/real-time-logger-def.h>

namespace dynamicgraph {

//#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
#define LOGGER_VERBOSITY_ALL

#define SEND_MSG(msg, type)                                                    \
  sendMsg(msg, type, __FILE__ ":" BOOST_PP_STRINGIZE(__LINE__))

#define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_DEBUG_STREAM)
#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_INFO_STREAM)
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)

#define _DYNAMIC_GRAPH_ENTITY_MSG(entity, type)                                \
  (entity).logger().stream(type, __FILE__ BOOST_PP_STRINGIZE(__LINE__))

#define DYNAMIC_GRAPH_ENTITY_DEBUG(entity)                                     \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_DEBUG)
#define DYNAMIC_GRAPH_ENTITY_INFO(entity)                                      \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_INFO)
#define DYNAMIC_GRAPH_ENTITY_WARNING(entity)                                   \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_WARNING)
#define DYNAMIC_GRAPH_ENTITY_ERROR(entity)                                     \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_ERROR)

#define DYNAMIC_GRAPH_ENTITY_DEBUG_STREAM(entity)                              \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_DEBUG_STREAM)
#define DYNAMIC_GRAPH_ENTITY_INFO_STREAM(entity)                               \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_INFO_STREAM)
#define DYNAMIC_GRAPH_ENTITY_WARNING_STREAM(entity)                            \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_WARNING_STREAM)
#define DYNAMIC_GRAPH_ENTITY_ERROR_STREAM(entity)                              \
  _DYNAMIC_GRAPH_ENTITY_MSG(entity, MSG_TYPE_ERROR_STREAM)

template <typename T>
std::string toString(const T &v, const int precision = 3,
                     const int width = -1) {
  std::stringstream ss;
  if (width > precision)
    ss << std::fixed << std::setw(width) << std::setprecision(precision) << v;
  else
    ss << std::fixed << std::setprecision(precision) << v;
  return ss.str();
}

template <typename T>
std::string toString(const std::vector<T> &v, const int precision = 3,
                     const int width = -1, const std::string separator = ", ") {
  std::stringstream ss;
  if (width > precision) {
    for (unsigned int i = 0; i < v.size() - 1; i++)
      ss << std::fixed << std::setw(width) << std::setprecision(precision)
         << v[i] << separator;
    ss << std::fixed << std::setw(width) << std::setprecision(precision)
       << v[v.size() - 1];
  } else {
    for (unsigned int i = 0; i < v.size() - 1; i++)
      ss << std::fixed << std::setprecision(precision) << v[i] << separator;
    ss << std::fixed << std::setprecision(precision) << v[v.size() - 1];
  }

  return ss.str();
}

template <typename T>
std::string toString(const Eigen::MatrixBase<T> &v, const int precision = 3,
                     const int width = -1, const std::string separator = ", ") {
  std::stringstream ss;
  if (width > precision) {
    for (unsigned int i = 0; i < v.size() - 1; i++)
      ss << std::fixed << std::setw(width) << std::setprecision(precision)
         << v[i] << separator;
    ss << std::fixed << std::setw(width) << std::setprecision(precision)
       << v[v.size() - 1];
  } else {
    for (unsigned int i = 0; i < v.size() - 1; i++)
      ss << std::fixed << std::setprecision(precision) << v[i] << separator;
    ss << std::setprecision(precision) << v[v.size() - 1];
  }

  return ss.str();
}

enum LoggerVerbosity {
  VERBOSITY_ALL = MSG_TYPE_DEBUG,
  VERBOSITY_INFO_WARNING_ERROR = MSG_TYPE_INFO,
  VERBOSITY_WARNING_ERROR = MSG_TYPE_WARNING,
  VERBOSITY_ERROR = MSG_TYPE_ERROR,
  VERBOSITY_NONE = 0
};

class Logger {
public:
  Logger(double timeSample = 0.001, double streamPrintPeriod = 1.0);

  ~Logger();

  void countdown();

  RTLoggerStream stream() {
    return ::dynamicgraph::RealTimeLogger::instance().front();
  }

  RTLoggerStream stream(MsgType type, const std::string &lineId = "") {
    RealTimeLogger &rtlogger = ::dynamicgraph::RealTimeLogger::instance();
    if (acceptMsg(type, lineId))
      return rtlogger.front();
    return rtlogger.emptyStream();
  }

  void sendMsg(std::string msg, MsgType type, const std::string &lineId = "");

  void sendMsg(std::string msg, MsgType type, const std::string &file,
               int line) DYNAMIC_GRAPH_DEPRECATED;

  bool setTimeSample(double t);

  double getTimeSample();

  bool setStreamPrintPeriod(double s);

  double getStreamPrintPeriod();

  void setVerbosity(LoggerVerbosity lv);

  LoggerVerbosity getVerbosity();

protected:
  LoggerVerbosity m_lv; 
  double m_timeSample;
  double m_streamPrintPeriod; 
  double m_printCountdown;

  typedef std::map<std::string, double> StreamCounterMap_t;
  StreamCounterMap_t m_stream_msg_counters;

  inline bool isStreamMsg(MsgType m) { return (m & MSG_TYPE_STREAM_BIT); }

  bool acceptMsg(MsgType m, const std::string &lineId) {
    // If more verbose than the current verbosity level
    if ((m & MSG_TYPE_TYPE_BITS) > m_lv)
      return false;

    // if print is allowed by current verbosity level
    if (isStreamMsg(m))
      return checkStreamPeriod(lineId);
    return true;
  }

  bool checkStreamPeriod(const std::string &lineId);
};

} // namespace dynamicgraph

#endif // #ifndef __sot_torque_control_logger_H__