/*
 * swo_log.h
 *
 * Created on: 20.01.2025
 * Author: Kilian
 *
 * Use the SWO interface to log messages
 */
#pragma once
#ifndef __SWO_LOG_H
#define __SWO_LOG_H
#include "stm32h7xx_hal.h"
#include <stdarg.h>
#include <stdio.h>

#define MAX_MESSAGE_LENGTH 256
#define USE_MULTIPLE_CHANNELS false // if true, each log level has its own channel (FATAL : 0, ERROR : 1, etc.)
#define USE_ANSI_ESCAPE_CODES true  // if true, log messages will be colored according to their log level, and the console can be cleared
#define PRINT_TIMESTAMP false       // if true, timestamp (from HAL_GetTick) is printed before each log message

#if !USE_MULTIPLE_CHANNELS
#define DEBUG_CHANNEL 0 // channel to output messages on
#define USE_CHANNEL_MASK_VARIABLE true
#endif

#if USE_CHANNEL_MASK_VARIABLE
#define MASK_VARIABLE logging_mask // variable to store the channel mask, must be globally defined somewhere
extern volatile uint32_t MASK_VARIABLE;
#endif

enum log_level_t {
  LOG_LEVEL_FATAL,
  LOG_LEVEL_ERROR,
  LOG_LEVEL_WARNING,
  LOG_LEVEL_INFO,
  LOG_LEVEL_DEBUG,
  LOG_LEVEL_NOISY
};

#if USE_ANSI_ESCAPE_CODES
[[maybe_unused]] static const char *const log_level_names[] = {
    "\033[31m[FATAL]\033[0m ", "\033[31m[ERROR]\033[0m ",
    "\033[93m[WARN] \033[0m ", "\033[97m[INFO] \033[0m ",
    "\033[37m[DEBUG]\033[0m ", "\033[37m[NOISY]\033[0m "};
#else
[[maybe_unused]] static const char *const log_level_names[] = {
    "[FATAL] ", "[ERROR] ", "[WARN]  ", "[INFO]  ", "[DEBUG] ", "[NOISY] "};
#endif

static inline bool __ITM_channel_enabled(uint32_t channel) {
#if !USE_MULTIPLE_CHANNELS
#if USE_CHANNEL_MASK_VARIABLE
  return ((ITM->TER & (1UL << DEBUG_CHANNEL)) != 0UL) &&
         ((MASK_VARIABLE & (1UL << channel)) != 0UL);
#else
  channel = DEBUG_CHANNEL;
#endif
#endif
  return ((ITM->TER & (1UL << channel)) != 0UL);
}

// adapted from ITM_SendChar() in the CMSIS-Core
//
// channel should be between 0 and 31
static inline uint32_t __swo_putc(uint32_t c, unsigned int channel) {
#if !USE_MULTIPLE_CHANNELS
  channel = DEBUG_CHANNEL;
#endif
  if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) && /* ITM enabled */
      ((ITM->TER & (1UL << channel)) != 0UL))     /* ITM Port enabled */
  {
    while (ITM->PORT[channel].u32 == 0UL) {
      __NOP();
    }
    ITM->PORT[channel].u8 = (uint8_t)c;
  }
  return (c);
}

#define DEBUG_CHANNEL_ENABLED(channel)                                         \
  ({                                                                           \
    unsigned int ch = (channel);                                               \
    (ch < 32) ? __ITM_channel_enabled(ch) : false;                             \
  })

[[gnu::nonnull(2), gnu::null_terminated_string_arg(2)]]
static inline void __swo_print(unsigned int channel, const char *str) {
  if (!__ITM_channel_enabled(channel)) {
    return;
  }
  while (*str) {
    __swo_putc(*str++, channel);
  }
}

[[maybe_unused]]
static void debug_clear_console() {
  for (int i = 0; i < LOG_LEVEL_NOISY; i++) {
#if USE_ANSI_ESCAPE_CODES
    __swo_print(i, "\033[2J\033[;H"); // clear screen
#else
    __swo_print(i, "\n\n\n\n\n-------------------\n\n\n\n\n"); // clear screen
#endif
  }
}

#define debug_log(level, msg, ...)                                             \
  do {                                                                         \
    if (DEBUG_CHANNEL_ENABLED(level)) {                                        \
      char __swo_buffer[MAX_MESSAGE_LENGTH];                                   \
      size_t len =                                                             \
          snprintf(__swo_buffer, sizeof(__swo_buffer), msg, ##__VA_ARGS__);    \
      __swo_putc('\n', level);                                                 \
      /* Print timestamp if enabled */                                         \
      if (PRINT_TIMESTAMP) {                                                   \
        char __time_buffer[16];                                                \
        if (USE_ANSI_ESCAPE_CODES) {                                           \
          snprintf(__time_buffer, sizeof(__time_buffer),                       \
                   "\033[90m[%lu]\033[0m ", HAL_GetTick());                    \
        } else {                                                               \
          snprintf(__time_buffer, sizeof(__time_buffer), "[%lu] ",             \
                   HAL_GetTick());                                             \
        }                                                                      \
        __swo_print(level, __time_buffer);                                     \
      }                                                                        \
      __swo_print(level, log_level_names[level]);                              \
      __swo_print(level, __swo_buffer);                                        \
      if (len >= sizeof(__swo_buffer)) {                                       \
        __swo_print(level, " [message length exceeded] ");                     \
      }                                                                        \
    }                                                                          \
  } while (0)

#define debug_log_cont(level, msg, ...)                                        \
  do {                                                                         \
    if (DEBUG_CHANNEL_ENABLED(level)) {                                        \
      char __swo_buffer[MAX_MESSAGE_LENGTH];                                   \
      size_t len =                                                             \
          snprintf(__swo_buffer, sizeof(__swo_buffer), msg, ##__VA_ARGS__);    \
      __swo_print(level, __swo_buffer);                                        \
      if (len >= sizeof(__swo_buffer)) {                                       \
        __swo_print(level, " [message length exceeded] ");                     \
      }                                                                        \
    }                                                                          \
  } while (0)

#endif /* __SWO_LOG_H */
