From 0ea5b8f2fc7bb46f3906914a0d9da4e7998ee12d Mon Sep 17 00:00:00 2001
From: CocoSimone <simococo03@gmail.com>
Date: Tue, 28 Jun 2022 15:29:25 +0200
Subject: [PATCH] Circular buffer impl

---
 src/core/common.hpp |  1 +
 src/core/util.hpp   | 86 +++++++++++++++++++++++++++++----------------
 2 files changed, 57 insertions(+), 30 deletions(-)

diff --git a/src/core/common.hpp b/src/core/common.hpp
index 408ad63b..589e7899 100644
--- a/src/core/common.hpp
+++ b/src/core/common.hpp
@@ -3,6 +3,7 @@
 #include <bitset>
 #include <emmintrin.h>
 #include <fmt/format.h>
+#include <fmt/color.h>
 
 using u8 = uint8_t;
 using u16 = uint16_t;
diff --git a/src/core/util.hpp b/src/core/util.hpp
index 67c1663d..f46a4cc6 100644
--- a/src/core/util.hpp
+++ b/src/core/util.hpp
@@ -5,52 +5,54 @@
 #include <portable_endian_bswap.h>
 
 namespace natsukashii::util {
+enum MessageType : u8 {
+  INFO, WARN, ERROR
+};
+
+template <MessageType messageType = INFO, typename ...Args>
+constexpr void print(const std::string& fmt, Args... args) {
+  if constexpr(messageType == ERROR) {
+    fmt::print(fmt::emphasis::bold | fg(fmt::color::red), fmt, args...);
+    exit(-1);
+  } else if constexpr(messageType == WARN) {
+    fmt::print(fg(fmt::color::yellow), fmt, args...);
+  } else if constexpr(messageType == INFO) {
+    fmt::print(fmt, args...);
+  }
+}
+
 template <typename ...Args>
 constexpr void panic(const std::string& fmt, Args... args) {
-  fmt::print(fmt, args...);
-  exit(-1);
+  print<ERROR>(fmt, args...);
 }
 
-template <u8 start, u8 end>
-using BitSliceType =
-typename std::conditional<(end - start) <= 7, u8,
-  typename std::conditional<(end - start) <= 15, u16,
-    typename std::conditional<(end - start) <= 31, u32,
-      typename std::conditional<(end - start) <= 63, u64,
-        typename std::conditional<(end - start) <= 127, u128, void>::type
-      >::type
-    >::type
-  >::type
->::type;
-
-template <u8 start, u8 end, typename T>
-BitSliceType<start, end> BitSlice(const T& num) {
-  static_assert(end < (sizeof(T) * 8) && start < (sizeof(T) * 8));
-  constexpr auto correctedEnd = end == (sizeof(T) * 8) - 1 ? end : end + 1;
-  return (num >> start) & ((1 << correctedEnd) - 1);
+template <typename ...Args>
+constexpr void warn(const std::string& fmt, Args... args) {
+  print<WARN>(fmt, args...);
 }
 
-template <typename T>
-T BitSlice(const T& num, int start, int end) {
-  assert(end < (sizeof(T) * 8) && start < (sizeof(T) * 8));
-  auto correctedEnd = end == (sizeof(T) * 8) - 1 ? end : end + 1;
-  return (num >> start) & ((1 << correctedEnd) - 1);
+template <typename ...Args>
+constexpr void info(const std::string& fmt, Args... args) {
+  print(fmt, args...);
 }
 
-template <typename T, bool FromHToBE = false>
+template <typename T, bool HToBE = false>
 auto GetSwapFunc(T num) -> T {
   if constexpr(sizeof(T) == 2) {
-    if constexpr(FromHToBE)
+    if constexpr(HToBE) {
       return htobe16(num);
+    }
     return be16toh(num);
   } else if constexpr(sizeof(T) == 4) {
-    if constexpr(FromHToBE)
+    if constexpr(HToBE) {
       return htobe32(num);
+    }
     return be32toh(num);
   } else if constexpr(sizeof(T) == 8) {
-    if constexpr(FromHToBE)
-      return htobe32(num);
-    return be32toh(num);
+    if constexpr(HToBE) {
+      return htobe64(num);
+    }
+    return be64toh(num);
   }
 }
 
@@ -96,6 +98,30 @@ inline void SwapN64Rom(size_t size, u8* data) {
   }
 }
 
+template <size_t size, typename T = u8>
+struct CircularBuffer {
+  CircularBuffer() : head(0) {
+    memset(raw, 0, size * sizeof(T));
+  }
+
+  void PushValue(T val) {
+    raw[head & mask] = val;
+    head &= mask;
+    head++;
+  }
+
+  T PopValue() {
+    head--;
+    head &= mask;
+    return raw[head & mask];
+  }
+  size_t GetHead() { return head; }
+private:
+  T raw[size];
+  size_t head;
+  static constexpr size_t mask = size - 1;
+};
+
 inline size_t NextPow2(size_t num) {
   // Taken from "Bit Twiddling Hacks" by Sean Anderson:
   // https://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2