feat(logger): Enhance logging functionality with variadic templates for formatted messages

- Added variadic template methods to the Logger class for formatted logging at different log levels (debug, info, warn, error).
- Updated existing logging calls in CustomRobot to utilize the new formatted logging methods for improved readability and consistency.
- Removed the old log method implementation from logger.cpp to streamline the codebase.

include/logger.hpp

@@ -7,6 +7,8 @@
 #include <sstream>
 #include <chrono>
 #include <iomanip>
+#include <cstdio> // For snprintf
+#include <memory> // For std::unique_ptr
 
 namespace custom {
 
@@ -61,4 +63,64 @@ private:
 #define LOG_WARN(msg, ...) custom::Logger::getInstance().warn(msg, ##__VA_ARGS__)
 #define LOG_ERROR(msg, ...) custom::Logger::getInstance().error(msg, ##__VA_ARGS__)
 
+template<typename... Args>
+void Logger::log(LogLevel level, const char* format, Args... args)
+{
+    if (level < currentLevel_) {
+        return;
+    }
+
+    // A buffer to hold the formatted string.
+    // First, we try with a small buffer.
+    char buffer[256];
+    int size = snprintf(buffer, sizeof(buffer), format, args...);
+    if (size < 0) {
+        // Encoding error
+        error("Encoding error in log message.");
+        return;
+    }
+
+    std::string message;
+    if (size < sizeof(buffer)) {
+        // The message fit in the buffer.
+        message = buffer;
+    } else {
+        // The buffer was too small, we need to allocate a larger one.
+        std::unique_ptr<char[]> dynamic_buffer(new char[size + 1]);
+        int new_size = snprintf(dynamic_buffer.get(), size + 1, format, args...);
+        if (new_size >= 0 && new_size <= size) {
+            message = dynamic_buffer.get();
+        } else {
+            error("Encoding error in log message after buffer resize.");
+            return;
+        }
+    }
+
+    log(level, message);
+}
+
+template<typename... Args>
+void Logger::debug(const char* format, Args... args)
+{
+    log(LogLevel::DEBUG, format, args...);
+}
+
+template<typename... Args>
+void Logger::info(const char* format, Args... args)
+{
+    log(LogLevel::INFO, format, args...);
+}
+
+template<typename... Args>
+void Logger::warn(const char* format, Args... args)
+{
+    log(LogLevel::WARN, format, args...);
+}
+
+template<typename... Args>
+void Logger::error(const char* format, Args... args)
+{
+    log(LogLevel::ERROR, format, args...);
+}
+
 } // namespace custom