Add CompressionV3 and tests

src/app/gfx/compression.cc

@@ -10,12 +10,16 @@
 #include "app/core/constants.h"
 #include "app/rom.h"
 
+#define DEBUG_LOG(msg) std::cout << msg << std::endl
+
 namespace yaze {
 namespace app {
 namespace gfx {
 
 namespace lc_lz2 {
 
+// Compression commands
+
 void PrintCompressionPiece(const CompressionPiecePointer& piece) {
   std::cout << "Command: " << std::to_string(piece->command) << "\n";
   std::cout << "Command Length: " << piece->length << "\n";
@@ -48,16 +52,6 @@ void CheckByteRepeat(const uchar* rom_data, DataSizeArray& data_size_taken,
   cmd_args[kCommandByteFill][0] = byte_to_repeat;
 }
 
-void CheckByteRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
-                       CompressionCommand& cmd) {
-  uint i = 0;
-  while (src_pos + i < last_pos && data[src_pos] == data[src_pos + i]) {
-    ++i;
-  }
-  cmd.data_size[kCommandByteFill] = i;
-  cmd.arguments[kCommandByteFill][0] = data[src_pos];
-}
-
 void CheckWordRepeat(const uchar* rom_data, DataSizeArray& data_size_taken,
                      CommandArgumentArray& cmd_args, uint& src_data_pos,
                      const uint last_pos) {
@@ -80,26 +74,6 @@ void CheckWordRepeat(const uchar* rom_data, DataSizeArray& data_size_taken,
   }
 }
 
-void CheckWordRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
-                       CompressionCommand& cmd) {
-  if (src_pos + 2 <= last_pos && data[src_pos] != data[src_pos + 1]) {
-    uint pos = src_pos;
-    char byte1 = data[pos];
-    char byte2 = data[pos + 1];
-    pos += 2;
-    cmd.data_size[kCommandWordFill] = 2;
-    while (pos + 1 <= last_pos) {
-      if (data[pos] == byte1 && data[pos + 1] == byte2)
-        cmd.data_size[kCommandWordFill] += 2;
-      else
-        break;
-      pos += 2;
-    }
-    cmd.arguments[kCommandWordFill][0] = byte1;
-    cmd.arguments[kCommandWordFill][1] = byte2;
-  }
-}
-
 void CheckIncByte(const uchar* rom_data, DataSizeArray& data_size_taken,
                   CommandArgumentArray& cmd_args, uint& src_data_pos,
                   const uint last_pos) {
@@ -116,21 +90,6 @@ void CheckIncByte(const uchar* rom_data, DataSizeArray& data_size_taken,
   cmd_args[kCommandIncreasingFill][0] = rom_data[src_data_pos];
 }
 
-void CheckIncByteV2(const uchar* rom_data, uint& src_data_pos,
-                    const uint last_pos, CompressionCommand& cmd) {
-  uint pos = src_data_pos;
-  char byte = rom_data[pos];
-  pos++;
-  cmd.data_size[kCommandIncreasingFill] = 1;
-  byte++;
-  while (pos <= last_pos && byte == rom_data[pos]) {
-    cmd.data_size[kCommandIncreasingFill]++;
-    byte++;
-    pos++;
-  }
-  cmd.arguments[kCommandIncreasingFill][0] = rom_data[src_data_pos];
-}
-
 void CheckIntraCopy(const uchar* rom_data, DataSizeArray& data_size_taken,
                     CommandArgumentArray& cmd_args, uint& src_data_pos,
                     const uint last_pos, uint start) {
@@ -165,40 +124,6 @@ void CheckIntraCopy(const uchar* rom_data, DataSizeArray& data_size_taken,
   }
 }
 
-void CheckIntraCopyV2(const uchar* rom_data, uint& src_data_pos,
-                      const uint last_pos, uint start,
-                      CompressionCommand& cmd) {
-  if (src_data_pos != start) {
-    uint searching_pos = start;
-    uint current_pos_u = src_data_pos;
-    uint copied_size = 0;
-    uint search_start = start;
-
-    while (searching_pos < src_data_pos && current_pos_u <= last_pos) {
-      while (rom_data[current_pos_u] != rom_data[searching_pos] &&
-             searching_pos < src_data_pos)
-        searching_pos++;
-      search_start = searching_pos;
-      while (current_pos_u <= last_pos &&
-             rom_data[current_pos_u] == rom_data[searching_pos] &&
-             searching_pos < src_data_pos) {
-        copied_size++;
-        current_pos_u++;
-        searching_pos++;
-      }
-      if (copied_size > cmd.data_size[kCommandRepeatingBytes]) {
-        search_start -= start;
-        printf("- Found repeat of %d at %d\n", copied_size, search_start);
-        cmd.data_size[kCommandRepeatingBytes] = copied_size;
-        cmd.arguments[kCommandRepeatingBytes][0] = search_start & kSnesByteMax;
-        cmd.arguments[kCommandRepeatingBytes][1] = search_start >> 8;
-      }
-      current_pos_u = src_data_pos;
-      copied_size = 0;
-    }
-  }
-}
-
 // Check if a command managed to pick up `max_win` or more bytes
 // Avoids being even with copy command, since it's possible to merge copy
 void ValidateForByteGain(const DataSizeArray& data_size_taken,
@@ -219,26 +144,6 @@ void ValidateForByteGain(const DataSizeArray& data_size_taken,
   }
 }
 
-// Table indicating command sizes, in bytes
-const std::array<int, 5> kCommandSizes = {1, 2, 2, 2, 3};
-
-// TODO(@scawful): TEST ME
-void ValidateForByteGainV2(const CompressionCommand& cmd, uint& max_win,
-                           uint& cmd_with_max) {
-  for (uint cmd_i = 1; cmd_i < 5; cmd_i++) {
-    uint cmd_size_taken = cmd.data_size[cmd_i];
-    // Check if the command size exceeds the maximum win and the size in the
-    // command sizes table, except for the repeating bytes command when the size
-    // taken is 3
-    if (cmd_size_taken > max_win && cmd_size_taken > kCommandSizes[cmd_i] &&
-        !(cmd_i == kCommandRepeatingBytes && cmd_size_taken == 3)) {
-      printf("==> C:%d / S:%d\n", cmd_i, cmd_size_taken);
-      cmd_with_max = cmd_i;
-      max_win = cmd_size_taken;
-    }
-  }
-}
-
 void CompressionCommandAlternative(const uchar* rom_data,
                                    CompressionPiecePointer& compressed_chain,
                                    const CommandSizeArray& cmd_size,
@@ -274,6 +179,108 @@ void CompressionCommandAlternative(const uchar* rom_data,
   comp_accumulator = 0;
 }
 
+// ============================================================================
+// Compression V2
+
+void CheckByteRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
+                       CompressionCommand& cmd) {
+  uint i = 0;
+  while (src_pos + i < last_pos && data[src_pos] == data[src_pos + i]) {
+    ++i;
+  }
+  cmd.data_size[kCommandByteFill] = i;
+  cmd.arguments[kCommandByteFill][0] = data[src_pos];
+}
+
+void CheckWordRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
+                       CompressionCommand& cmd) {
+  if (src_pos + 2 <= last_pos && data[src_pos] != data[src_pos + 1]) {
+    uint pos = src_pos;
+    char byte1 = data[pos];
+    char byte2 = data[pos + 1];
+    pos += 2;
+    cmd.data_size[kCommandWordFill] = 2;
+    while (pos + 1 <= last_pos) {
+      if (data[pos] == byte1 && data[pos + 1] == byte2)
+        cmd.data_size[kCommandWordFill] += 2;
+      else
+        break;
+      pos += 2;
+    }
+    cmd.arguments[kCommandWordFill][0] = byte1;
+    cmd.arguments[kCommandWordFill][1] = byte2;
+  }
+}
+
+void CheckIncByteV2(const uchar* rom_data, uint& src_data_pos,
+                    const uint last_pos, CompressionCommand& cmd) {
+  uint pos = src_data_pos;
+  char byte = rom_data[pos];
+  pos++;
+  cmd.data_size[kCommandIncreasingFill] = 1;
+  byte++;
+  while (pos <= last_pos && byte == rom_data[pos]) {
+    cmd.data_size[kCommandIncreasingFill]++;
+    byte++;
+    pos++;
+  }
+  cmd.arguments[kCommandIncreasingFill][0] = rom_data[src_data_pos];
+}
+
+void CheckIntraCopyV2(const uchar* rom_data, uint& src_data_pos,
+                      const uint last_pos, uint start,
+                      CompressionCommand& cmd) {
+  if (src_data_pos != start) {
+    uint searching_pos = start;
+    uint current_pos_u = src_data_pos;
+    uint copied_size = 0;
+    uint search_start = start;
+
+    while (searching_pos < src_data_pos && current_pos_u <= last_pos) {
+      while (rom_data[current_pos_u] != rom_data[searching_pos] &&
+             searching_pos < src_data_pos)
+        searching_pos++;
+      search_start = searching_pos;
+      while (current_pos_u <= last_pos &&
+             rom_data[current_pos_u] == rom_data[searching_pos] &&
+             searching_pos < src_data_pos) {
+        copied_size++;
+        current_pos_u++;
+        searching_pos++;
+      }
+      if (copied_size > cmd.data_size[kCommandRepeatingBytes]) {
+        search_start -= start;
+        printf("- Found repeat of %d at %d\n", copied_size, search_start);
+        cmd.data_size[kCommandRepeatingBytes] = copied_size;
+        cmd.arguments[kCommandRepeatingBytes][0] = search_start & kSnesByteMax;
+        cmd.arguments[kCommandRepeatingBytes][1] = search_start >> 8;
+      }
+      current_pos_u = src_data_pos;
+      copied_size = 0;
+    }
+  }
+}
+
+// Table indicating command sizes, in bytes
+const std::array<int, 5> kCommandSizes = {1, 2, 2, 2, 3};
+
+// TODO(@scawful): TEST ME
+void ValidateForByteGainV2(const CompressionCommand& cmd, uint& max_win,
+                           uint& cmd_with_max) {
+  for (uint cmd_i = 1; cmd_i < 5; cmd_i++) {
+    uint cmd_size_taken = cmd.data_size[cmd_i];
+    // Check if the command size exceeds the maximum win and the size in the
+    // command sizes table, except for the repeating bytes command when the size
+    // taken is 3
+    if (cmd_size_taken > max_win && cmd_size_taken > kCommandSizes[cmd_i] &&
+        !(cmd_i == kCommandRepeatingBytes && cmd_size_taken == 3)) {
+      printf("==> C:%d / S:%d\n", cmd_i, cmd_size_taken);
+      cmd_with_max = cmd_i;
+      max_win = cmd_size_taken;
+    }
+  }
+}
+
 void CompressionCommandAlternativeV2(const uchar* rom_data,
                                      const CompressionCommand& cmd,
                                      CompressionPiecePointer& compressed_chain,
@@ -591,6 +598,460 @@ absl::StatusOr<Bytes> CompressOverworld(const uchar* data, const int pos,
   return CompressV2(data, pos, length, kNintendoMode1);
 }
 
+// ============================================================================
+// Compression V3
+
+void CheckByteRepeatV3(CompressionContext& context) {
+  uint pos = context.src_pos;
+
+  // Ensure the sequence does not start with an uncompressable byte
+  if (pos == 0 || context.data[pos - 1] != context.data[pos]) {
+    char byte_to_repeat = context.data[pos];
+    while (pos <= context.last_pos && context.data[pos] == byte_to_repeat) {
+      context.current_cmd.data_size[kCommandByteFill]++;
+      pos++;
+    }
+
+    context.current_cmd.arguments[kCommandByteFill][0] = byte_to_repeat;
+
+    // Added debug log
+    DEBUG_LOG("CheckByteRepeatV3: byte_to_repeat = "
+              << (int)byte_to_repeat << ", size = "
+              << context.current_cmd.data_size[kCommandByteFill]);
+  }
+}
+
+void CheckWordRepeatV3(CompressionContext& context) {
+  if (context.src_pos + 1 <= context.last_pos) {  // Changed the condition here
+    uint pos = context.src_pos;
+    char byte1 = context.data[pos];
+    char byte2 = context.data[pos + 1];
+    pos += 2;
+    context.current_cmd.data_size[kCommandWordFill] = 2;
+    while (pos + 1 <= context.last_pos) {
+      if (context.data[pos] == byte1 && context.data[pos + 1] == byte2)
+        context.current_cmd.data_size[kCommandWordFill] += 2;
+      else
+        break;
+      pos += 2;
+    }
+
+    context.current_cmd.arguments[kCommandWordFill][0] = byte1;
+    context.current_cmd.arguments[kCommandWordFill][1] = byte2;
+  }
+
+  DEBUG_LOG("CheckWordRepeatV3: byte1 = "
+            << (int)context.current_cmd.arguments[kCommandWordFill][0]
+            << ", byte2 = "
+            << (int)context.current_cmd.arguments[kCommandWordFill][1]
+            << ", size = " << context.current_cmd.data_size[kCommandWordFill]);
+}
+
+void CheckIncByteV3(CompressionContext& context) {
+  uint pos = context.src_pos;
+  uint8_t byte = context.data[pos];
+  pos++;
+  context.current_cmd.data_size[kCommandIncreasingFill] = 1;
+  byte++;
+
+  while (pos <= context.last_pos && byte == context.data[pos]) {
+    context.current_cmd.data_size[kCommandIncreasingFill]++;
+    byte++;
+    pos++;
+  }
+
+  // Let's see if the sequence is surrounded by identical bytes and if so,
+  // consider if a direct copy is better.
+  if (context.current_cmd.data_size[kCommandIncreasingFill] == 3 &&
+      context.src_pos > 0 && pos < context.data.size() &&
+      context.data[context.src_pos - 1] == context.data[pos]) {
+    context.current_cmd.data_size[kCommandIncreasingFill] =
+        0;  // Reset the size to 0 to prioritize direct copy
+    return;
+  }
+
+  context.current_cmd.arguments[kCommandIncreasingFill][0] =
+      context.data[context.src_pos];
+
+  DEBUG_LOG("CheckIncByteV3: byte = "
+            << (int)context.current_cmd.arguments[kCommandIncreasingFill][0]
+            << ", size = "
+            << context.current_cmd.data_size[kCommandIncreasingFill]);
+}
+
+void CheckIntraCopyV3(CompressionContext& context) {
+  const int window_size =
+      32;  // This can be adjusted for optimal performance and results
+
+  // We'll only search for repeating sequences if we're not at the very
+  // beginning
+  if (context.src_pos > 0 &&
+      context.src_pos + window_size <= context.data.size()) {
+    uint max_copied_size = 0;
+    uint best_search_start = 0;
+
+    // Slide the window over the source data
+    for (int win_pos = 1; win_pos < window_size && win_pos < context.src_pos;
+         ++win_pos) {
+      auto start_search_from = context.data.begin() + context.src_pos - win_pos;
+      auto search_end = context.data.begin() + context.src_pos;
+
+      // Use std::search to find the sequence in the window in the previous
+      // source data
+      auto found_pos = std::search(
+          start_search_from, search_end, context.data.begin() + context.src_pos,
+          context.data.begin() + context.src_pos + win_pos);
+
+      if (found_pos != search_end) {
+        // Check the entire length of the match
+        uint len = 0;
+        while (context.src_pos + len < context.data.size() &&
+               context.data[context.src_pos + len] == *(found_pos + len)) {
+          len++;
+        }
+
+        if (len > max_copied_size) {
+          max_copied_size = len;
+          best_search_start = found_pos - context.data.begin();
+        }
+      }
+    }
+
+    if (max_copied_size >
+        context.current_cmd.data_size[kCommandRepeatingBytes]) {
+      DEBUG_LOG("CheckIntraCopyV3: Detected repeating sequence of length "
+                << max_copied_size << " starting from " << best_search_start);
+      context.current_cmd.data_size[kCommandRepeatingBytes] = max_copied_size;
+      context.current_cmd.arguments[kCommandRepeatingBytes][0] =
+          best_search_start & kSnesByteMax;
+      context.current_cmd.arguments[kCommandRepeatingBytes][1] =
+          best_search_start >> 8;
+    }
+
+    DEBUG_LOG("CheckIntraCopyV3: max_copied_size = " << max_copied_size
+                                                     << ", best_search_start = "
+                                                     << best_search_start);
+  }
+}
+
+void InitializeCompression(CompressionContext& context) {
+  // Initialize the current_cmd with default values.
+  context.current_cmd = {/*argument*/ {{}},
+                         /*cmd_size*/ {0, 1, 2, 1, 2},
+                         /*data_size*/ {0, 0, 0, 0, 0}};
+}
+
+void CheckAvailableCompressionCommands(CompressionContext& context) {
+  // Reset the data_size and arguments for a fresh check.
+  context.current_cmd.data_size.fill({});
+  context.current_cmd.arguments.fill({{}});
+
+  CheckByteRepeatV3(context);
+  CheckWordRepeatV3(context);
+  CheckIncByteV3(context);
+  CheckIntraCopyV3(context);
+
+  DEBUG_LOG("CheckAvailableCompressionCommands: src_pos = " << context.src_pos);
+}
+
+void DetermineBestCompression(CompressionContext& context) {
+  int max_net_savings = -1;  // Adjusted the bias to consider any savings
+
+  // Start with the default scenario.
+  context.cmd_with_max = kCommandDirectCopy;
+
+  for (uint cmd_i = 1; cmd_i < 5; cmd_i++) {
+    uint cmd_size_taken = context.current_cmd.data_size[cmd_i];
+    int net_savings = cmd_size_taken - context.current_cmd.cmd_size[cmd_i];
+
+    // Skip commands that aren't efficient.
+    if (cmd_size_taken <= 2 && cmd_i != kCommandDirectCopy) {
+      continue;
+    }
+
+    // Check surrounding data for optimization.
+    if (context.src_pos > 0 &&
+        context.src_pos + cmd_size_taken < context.data.size()) {
+      char prev_byte = context.data[context.src_pos - 1];
+      char next_byte = context.data[context.src_pos + cmd_size_taken];
+      if (prev_byte != next_byte && cmd_size_taken == 3) {
+        continue;
+      }
+    }
+
+    // Check if the current command offers more net savings.
+    if (net_savings > max_net_savings) {
+      context.cmd_with_max = cmd_i;
+      max_net_savings = net_savings;
+    }
+  }
+
+  DEBUG_LOG("DetermineBestCompression: cmd_with_max = "
+            << context.cmd_with_max << ", data_size = "
+            << context.current_cmd.data_size[context.cmd_with_max]);
+}
+
+void HandleDirectCopy(CompressionContext& context) {
+  // If the next best compression method isn't direct copy and we have bytes
+  // accumulated for direct copy, flush them out.
+  if (context.cmd_with_max != kCommandDirectCopy &&
+      context.comp_accumulator > 0) {
+    uint8_t header = BUILD_HEADER(kCommandDirectCopy, context.comp_accumulator);
+    context.compressed_data.push_back(header);
+    std::vector<uint8_t> uncompressed_data(
+        context.data.begin() + context.src_pos - context.comp_accumulator,
+        context.data.begin() + context.src_pos);
+    context.compressed_data.insert(context.compressed_data.end(),
+                                   uncompressed_data.begin(),
+                                   uncompressed_data.end());
+    context.comp_accumulator = 0;
+    return;
+  }
+
+  // If the next best compression method is not direct copy and we haven't
+  // accumulated any bytes, treat it as a single byte direct copy.
+  if (context.cmd_with_max != kCommandDirectCopy &&
+      context.comp_accumulator == 0) {
+    context.compressed_data.push_back(
+        0x00);  // Command for a single byte direct copy
+    context.compressed_data.push_back(
+        context.data[context.src_pos]);  // The single byte
+    context.src_pos++;
+    return;
+  }
+
+  // If we reach here, accumulate bytes for a direct copy.
+  context.src_pos++;
+  context.comp_accumulator++;
+
+  // If we've accumulated the maximum bytes for a direct copy command or
+  // reached the end, flush them.
+  if (context.comp_accumulator >= 32 || context.src_pos > context.last_pos) {
+    uint8_t header = BUILD_HEADER(kCommandDirectCopy, context.comp_accumulator);
+    context.compressed_data.push_back(header);
+    std::vector<uint8_t> uncompressed_data(
+        context.data.begin() + context.src_pos - context.comp_accumulator,
+        context.data.begin() + context.src_pos);
+    context.compressed_data.insert(context.compressed_data.end(),
+                                   uncompressed_data.begin(),
+                                   uncompressed_data.end());
+    context.comp_accumulator = 0;
+  }
+
+  DEBUG_LOG("HandleDirectCopy: src_pos = " << context.src_pos
+                                           << ", compressed_data size = "
+                                           << context.compressed_data.size());
+}
+
+void AddCompressionToChain(CompressionContext& context) {
+  DEBUG_LOG("AddCompressionToChain: Adding command arguments: ");
+
+  // If there's uncompressed data, add a copy chunk before the compression
+  // command
+  if (context.comp_accumulator != 0) {
+    uint8_t header = BUILD_HEADER(kCommandDirectCopy, context.comp_accumulator);
+    context.compressed_data.push_back(header);
+    std::vector<uint8_t> uncompressed_data(
+        context.data.begin() + context.src_pos - context.comp_accumulator,
+        context.data.begin() + context.src_pos);
+    context.compressed_data.insert(context.compressed_data.end(),
+                                   uncompressed_data.begin(),
+                                   uncompressed_data.end());
+    context.comp_accumulator = 0;
+  }
+
+  // Now, add the compression command
+  uint8_t header =
+      BUILD_HEADER(context.cmd_with_max,
+                   context.current_cmd.data_size[context.cmd_with_max]);
+  context.compressed_data.push_back(header);
+
+  DEBUG_LOG("AddCompressionToChain: (Before) src_pos = "
+            << context.src_pos
+            << ", compressed_data size = " << context.compressed_data.size());
+
+  // Add the command arguments to the compressed_data vector
+  context.compressed_data.push_back(
+      context.current_cmd.arguments[context.cmd_with_max][0]);
+  if (context.current_cmd.cmd_size[context.cmd_with_max] == 2) {
+    context.compressed_data.push_back(
+        context.current_cmd.arguments[context.cmd_with_max][1]);
+  }
+
+  context.src_pos += context.current_cmd.data_size[context.cmd_with_max];
+  context.comp_accumulator = 0;
+
+  DEBUG_LOG("AddCompressionToChain: (After) src_pos = "
+            << context.src_pos
+            << ", compressed_data size = " << context.compressed_data.size());
+}
+
+absl::Status ValidateCompressionResultV3(const CompressionContext& context) {
+  if (!context.compressed_data.empty()) {
+    ROM temp_rom;
+    RETURN_IF_ERROR(temp_rom.LoadFromBytes(context.compressed_data));
+    ASSIGN_OR_RETURN(auto decomp_data,
+                     DecompressV2(temp_rom.data(), 0, temp_rom.size()))
+
+    if (!std::equal(decomp_data.begin() + context.start, decomp_data.end(),
+                    temp_rom.begin())) {
+      return absl::InternalError(absl::StrFormat(
+          "Compressed data does not match uncompressed data at %d\n",
+          (context.src_pos - context.start)));
+    }
+  }
+  return absl::OkStatus();
+}
+
+absl::StatusOr<CompressionPiece> SplitCompressionPieceV3(
+    CompressionPiece& piece, int mode) {
+  CompressionPiece new_piece;
+  uint length_left = piece.length - kMaxLengthCompression;
+  piece.length = kMaxLengthCompression;
+
+  switch (piece.command) {
+    case kCommandByteFill:
+    case kCommandWordFill:
+      new_piece = CompressionPiece(piece.command, length_left, piece.argument,
+                                   piece.argument_length);
+      break;
+    case kCommandIncreasingFill:
+      new_piece = CompressionPiece(piece.command, length_left, piece.argument,
+                                   piece.argument_length);
+      new_piece.argument[0] = (char)(piece.argument[0] + kMaxLengthCompression);
+      break;
+    case kCommandDirectCopy:
+      piece.argument_length = kMaxLengthCompression;
+      new_piece =
+          CompressionPiece(piece.command, length_left, nullptr, length_left);
+      // MEMCPY
+      for (int i = 0; i < length_left; ++i) {
+        new_piece.argument[i] = piece.argument[i + kMaxLengthCompression];
+      }
+      break;
+    case kCommandRepeatingBytes: {
+      piece.argument_length = kMaxLengthCompression;
+      uint offset = piece.argument[0] + (piece.argument[1] << 8);
+      new_piece = CompressionPiece(piece.command, length_left, piece.argument,
+                                   piece.argument_length);
+      if (mode == kNintendoMode2) {
+        new_piece.argument[0] = (offset + kMaxLengthCompression) & kSnesByteMax;
+        new_piece.argument[1] = (offset + kMaxLengthCompression) >> 8;
+      }
+      if (mode == kNintendoMode1) {
+        new_piece.argument[1] = (offset + kMaxLengthCompression) & kSnesByteMax;
+        new_piece.argument[0] = (offset + kMaxLengthCompression) >> 8;
+      }
+    } break;
+    default: {
+      return absl::InvalidArgumentError(
+          "SplitCompressionCommand: Invalid Command");
+    }
+  }
+
+  return new_piece;
+}
+
+void FinalizeCompression(CompressionContext& context) {
+  uint pos = 0;
+
+  for (CompressionPiece& piece : context.compression_pieces) {
+    if (piece.length <= kMaxLengthNormalHeader) {  // Normal Header
+      context.compression_string.push_back(
+          BUILD_HEADER(piece.command, piece.length));
+      pos++;
+    } else {
+      if (piece.length <= kMaxLengthCompression) {
+        context.compression_string.push_back(
+            kCompressionStringMod | ((uchar)piece.command << 2) |
+            (((piece.length - 1) & 0xFF00) >> 8));
+        pos++;
+        std::cout << "Building extended header : cmd: " << piece.command
+                  << ", length: " << piece.length << " - "
+                  << (int)context.compression_string[pos - 1] << std::endl;
+        context.compression_string.push_back(
+            ((piece.length - 1) & 0x00FF));  // (char)
+      } else {
+        // We need to split the command
+        auto new_piece = SplitCompressionPieceV3(piece, context.mode);
+        if (!new_piece.ok()) {
+          std::cout << new_piece.status().ToString() << std::endl;
+        }
+        context.compression_pieces.insert(
+            context.compression_pieces.begin() + pos + 1, new_piece.value());
+        continue;
+      }
+    }
+
+    if (piece.command == kCommandRepeatingBytes) {
+      char tmp[2];
+      tmp[0] = piece.argument[0];
+      tmp[1] = piece.argument[1];
+      if (context.mode == kNintendoMode1) {
+        tmp[0] = piece.argument[1];
+        tmp[1] = piece.argument[0];
+      }
+      for (const auto& each : tmp) {
+        context.compression_string.push_back(each);
+        pos++;
+      }
+    } else {
+      for (int i = 0; i < piece.argument_length; ++i) {
+        context.compression_string.push_back(piece.argument[i]);
+        pos++;
+      }
+    }
+    pos += piece.argument_length;
+  }
+
+  // Add any remaining uncompressed data
+  if (context.comp_accumulator > 0) {
+    context.compressed_data.insert(
+        context.compressed_data.end(),
+        context.data.begin() + context.src_pos - context.comp_accumulator,
+        context.data.begin() + context.src_pos);
+    context.comp_accumulator = 0;
+  }
+
+  // Add the end marker to the compressed data
+  context.compressed_data.push_back(kSnesByteMax);
+  DEBUG_LOG("FinalizeCompression: compressed_data size = "
+            << context.compressed_data.size());
+}
+
+absl::StatusOr<Bytes> CompressV3(const std::vector<uint8_t> data,
+                                 const int start, const int length, int mode,
+                                 bool check) {
+  if (length == 0) {
+    return Bytes();
+  }
+
+  CompressionContext context(data, start, length, mode);
+  InitializeCompression(context);
+
+  while (context.src_pos <= context.last_pos) {
+    CheckAvailableCompressionCommands(context);
+    DetermineBestCompression(context);
+
+    DEBUG_LOG("CompressV3 Loop: cmd_with_max = " << context.cmd_with_max);
+
+    if (context.cmd_with_max == kCommandDirectCopy) {
+      HandleDirectCopy(context);
+    } else {
+      AddCompressionToChain(context);
+    }
+
+    if (check) {
+      RETURN_IF_ERROR(ValidateCompressionResultV3(context))
+    }
+  }
+
+  FinalizeCompression(context);
+  return Bytes(context.compressed_data.begin(), context.compressed_data.end());
+}
+
+// Decompression
+
 std::string SetBuffer(const uchar* data, int src_pos, int comp_accumulator) {
   std::string buffer;
   for (int i = 0; i < comp_accumulator; ++i) {
@@ -599,6 +1060,15 @@ std::string SetBuffer(const uchar* data, int src_pos, int comp_accumulator) {
   return buffer;
 }
 
+std::string SetBuffer(const std::vector<uint8_t>& data, int src_pos,
+                      int comp_accumulator) {
+  std::string buffer;
+  for (int i = 0; i < comp_accumulator; ++i) {
+    buffer.push_back(data[i + src_pos - comp_accumulator]);
+  }
+  return buffer;
+}
+
 void memfill(const uchar* data, Bytes& buffer, int buffer_pos, int offset,
              int length) {
   auto a = data[offset];
@@ -669,10 +1139,11 @@ absl::StatusOr<Bytes> DecompressV2(const uchar* data, int offset, int size,
                  ((data[offset] & kSnesByteMax) << 8);
         }
         if (addr > offset) {
-          return absl::InternalError(absl::StrFormat(
+          return absl::InternalError(
-              "Decompress: Offset for command copy exceeds current position "
+              absl::StrFormat("Decompress: Offset for command copy exceeds "
-              "(Offset : %#04x | Pos : %#06x)\n",
+                              "current position "
-              addr, offset));
+                              "(Offset : %#04x | Pos : %#06x)\n",
+                              addr, offset));
         }
         if (buffer_pos + length >= size) {
           size *= 2;

src/app/gfx/compression.h

@@ -68,47 +68,6 @@ void PrintCompressionPiece(const CompressionPiecePointer& piece);
 
 void PrintCompressionChain(const CompressionPiecePointer& chain_head);
 
-// Compression V2
-
-void CheckByteRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
-                       CompressionCommand& cmd);
-
-void CheckWordRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
-                       CompressionCommand& cmd);
-
-void CheckIncByteV2(const uchar* data, uint& src_pos, const uint last_pos,
-                    CompressionCommand& cmd);
-
-void CheckIntraCopyV2(const uchar* data, uint& src_pos, const uint last_pos,
-                      uint start, CompressionCommand& cmd);
-
-void ValidateForByteGainV2(const CompressionCommand& cmd, uint& max_win,
-                           uint& cmd_with_max);
-
-void CompressionCommandAlternativeV2(const uchar* data,
-                                     const CompressionCommand& cmd,
-                                     CompressionPiecePointer& compressed_chain,
-                                     uint& src_pos, uint& comp_accumulator,
-                                     uint& cmd_with_max, uint& max_win);
-
-absl::StatusOr<Bytes> CompressV2(const uchar* data, const int start,
-                                 const int length, int mode = 1,
-                                 bool check = false);
-absl::StatusOr<Bytes> CompressGraphics(const uchar* data, const int pos,
-                                       const int length);
-absl::StatusOr<Bytes> CompressOverworld(const uchar* data, const int pos,
-                                        const int length);
-
-std::string SetBuffer(const uchar* data, int src_pos, int comp_accumulator);
-void memfill(const uchar* data, Bytes& buffer, int buffer_pos, int offset,
-             int length);
-
-absl::StatusOr<Bytes> DecompressV2(const uchar* data, int offset,
-                                   int size = 0x800, int mode = 1);
-absl::StatusOr<Bytes> DecompressGraphics(const uchar* data, int pos, int size);
-absl::StatusOr<Bytes> DecompressOverworld(const uchar* data, int pos, int size);
-absl::StatusOr<Bytes> DecompressOverworld(const std::vector<uint8_t> data, int pos, int size);
-
 // Compression V1
 
 void CheckByteRepeat(const uchar* rom_data, DataSizeArray& data_size_taken,
@@ -138,6 +97,38 @@ void CompressionCommandAlternative(const uchar* rom_data,
                                    uint& src_data_pos, uint& comp_accumulator,
                                    uint& cmd_with_max, uint& max_win);
 
+// Compression V2
+
+void CheckByteRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
+                       CompressionCommand& cmd);
+
+void CheckWordRepeatV2(const uchar* data, uint& src_pos, const uint last_pos,
+                       CompressionCommand& cmd);
+
+void CheckIncByteV2(const uchar* data, uint& src_pos, const uint last_pos,
+                    CompressionCommand& cmd);
+
+void CheckIntraCopyV2(const uchar* data, uint& src_pos, const uint last_pos,
+                      uint start, CompressionCommand& cmd);
+
+void ValidateForByteGainV2(const CompressionCommand& cmd, uint& max_win,
+                           uint& cmd_with_max);
+
+void CompressionCommandAlternativeV2(const uchar* data,
+                                     const CompressionCommand& cmd,
+                                     CompressionPiecePointer& compressed_chain,
+                                     uint& src_pos, uint& comp_accumulator,
+                                     uint& cmd_with_max, uint& max_win);
+
+absl::StatusOr<Bytes> CompressV2(const uchar* data, const int start,
+                                 const int length, int mode = 1,
+                                 bool check = false);
+
+absl::StatusOr<Bytes> CompressGraphics(const uchar* data, const int pos,
+                                       const int length);
+absl::StatusOr<Bytes> CompressOverworld(const uchar* data, const int pos,
+                                        const int length);
+
 absl::StatusOr<CompressionPiecePointer> SplitCompressionPiece(
     CompressionPiecePointer& piece, int mode);
 
@@ -148,6 +139,71 @@ absl::Status ValidateCompressionResult(CompressionPiecePointer& chain_head,
 
 CompressionPiecePointer MergeCopy(CompressionPiecePointer& start);
 
+// Compression V3
+
+struct CompressionContext {
+  std::vector<uint8_t> data;
+  std::vector<uint8_t> compressed_data;
+  std::vector<CompressionPiece> compression_pieces;
+  std::vector<uint8_t> compression_string;
+  uint src_pos;
+  uint last_pos;
+  uint start;
+  uint comp_accumulator = 0;
+  uint cmd_with_max = kCommandDirectCopy;
+  uint max_win = 0;
+  CompressionCommand current_cmd = {};
+  int mode;
+
+  // Constructor to initialize the context
+  CompressionContext(const std::vector<uint8_t>& data_, const int start,
+                     const int length)
+      : data(data_), src_pos(start), last_pos(start + length - 1), mode(0) {}
+
+  // Constructor to initialize the context
+  CompressionContext(const std::vector<uint8_t>& data_, const int start,
+                     const int length, int mode_)
+      : data(data_),
+        src_pos(start),
+        last_pos(start + length - 1),
+        mode(mode_) {}
+};
+
+void CheckByteRepeatV3(CompressionContext& context);
+void CheckWordRepeatV3(CompressionContext& context);
+void CheckIncByteV3(CompressionContext& context);
+void CheckIntraCopyV3(CompressionContext& context);
+
+void InitializeCompression(CompressionContext& context);
+void CheckAvailableCompressionCommands(CompressionContext& context);
+void DetermineBestCompression(CompressionContext& context);
+void HandleDirectCopy(CompressionContext& context);
+void AddCompressionToChain(CompressionContext& context);
+absl::Status ValidateCompressionResultV3(const CompressionContext& context);
+
+absl::StatusOr<CompressionPiece> SplitCompressionPieceV3(
+    CompressionPiece& piece, int mode);
+void FinalizeCompression(CompressionContext& context);
+
+absl::StatusOr<Bytes> CompressV3(const std::vector<uint8_t> data,
+                                 const int start, const int length,
+                                 int mode = 1, bool check = false);
+
+// Decompression
+
+std::string SetBuffer(const std::vector<uint8_t>& data, int src_pos,
+                      int comp_accumulator);
+std::string SetBuffer(const uchar* data, int src_pos, int comp_accumulator);
+void memfill(const uchar* data, Bytes& buffer, int buffer_pos, int offset,
+             int length);
+
+absl::StatusOr<Bytes> DecompressV2(const uchar* data, int offset,
+                                   int size = 0x800, int mode = 1);
+absl::StatusOr<Bytes> DecompressGraphics(const uchar* data, int pos, int size);
+absl::StatusOr<Bytes> DecompressOverworld(const uchar* data, int pos, int size);
+absl::StatusOr<Bytes> DecompressOverworld(const std::vector<uint8_t> data,
+                                          int pos, int size);
+
 }  // namespace lc_lz2
 
 }  // namespace gfx

test/compression_test.cc

@@ -14,10 +14,19 @@ namespace yaze_test {
 namespace gfx_test {
 
 using yaze::app::ROM;
+using yaze::app::gfx::lc_lz2::CompressionContext;
 using yaze::app::gfx::lc_lz2::CompressionPiece;
 using yaze::app::gfx::lc_lz2::CompressV2;
+using yaze::app::gfx::lc_lz2::CompressV3;
 using yaze::app::gfx::lc_lz2::DecompressV2;
+using yaze::app::gfx::lc_lz2::kCommandByteFill;
+using yaze::app::gfx::lc_lz2::kCommandDirectCopy;
+using yaze::app::gfx::lc_lz2::kCommandIncreasingFill;
+using yaze::app::gfx::lc_lz2::kCommandLongLength;
+using yaze::app::gfx::lc_lz2::kCommandRepeatingBytes;
+using yaze::app::gfx::lc_lz2::kCommandWordFill;
 
+using ::testing::ElementsAre;
 using ::testing::ElementsAreArray;
 using ::testing::TypedEq;
 
@@ -26,7 +35,7 @@ namespace {
 Bytes ExpectCompressOk(ROM& rom, uchar* in, int in_size) {
   auto load_status = rom.LoadFromPointer(in, in_size);
   EXPECT_TRUE(load_status.ok());
-  auto compression_status = CompressV2(rom.data(), 0, in_size);
+  auto compression_status = CompressV3(rom.vector(), 0, in_size);
   EXPECT_TRUE(compression_status.ok());
   auto compressed_bytes = std::move(*compression_status);
   return compressed_bytes;
@@ -59,8 +68,77 @@ std::shared_ptr<CompressionPiece> ExpectNewCompressionPieceOk(
   return new_piece;
 }
 
+// Helper function to assert compression quality.
+void AssertCompressionQuality(
+    const std::vector<uint8_t>& uncompressed_data,
+    const std::vector<uint8_t>& expected_compressed_data) {
+  absl::StatusOr<Bytes> result =
+      CompressV3(uncompressed_data, 0, uncompressed_data.size(), 0, false);
+  ASSERT_TRUE(result.ok());
+  auto compressed_data = std::move(*result);
+  EXPECT_THAT(compressed_data, ElementsAreArray(expected_compressed_data));
+}
+
+Bytes ExpectCompressV3Ok(const std::vector<uint8_t>& uncompressed_data,
+                         const std::vector<uint8_t>& expected_compressed_data) {
+  absl::StatusOr<Bytes> result =
+      CompressV3(uncompressed_data, 0, uncompressed_data.size(), 0, false);
+  EXPECT_TRUE(result.ok());
+  auto compressed_data = std::move(*result);
+  return compressed_data;
+}
+
+std::vector<uint8_t> CreateRepeatedBetweenUncompressable(
+    int leftUncompressedSize, int repeatedByteSize, int rightUncompressedSize) {
+  std::vector<uint8_t> result(
+      leftUncompressedSize + repeatedByteSize + rightUncompressedSize, 0);
+  std::fill_n(result.begin() + leftUncompressedSize, repeatedByteSize, 0x00);
+  return result;
+}
+
 }  // namespace
 
+TEST(LC_LZ2_CompressionTest, TrivialRepeatedBytes) {
+  AssertCompressionQuality({0x00, 0x00, 0x00}, {0x22, 0x00, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, RepeatedBytesBetweenUncompressable) {
+  AssertCompressionQuality({0x01, 0x00, 0x00, 0x00, 0x10},
+                           {0x04, 0x01, 0x00, 0x00, 0x00, 0x10, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, RepeatedBytesBeforeUncompressable) {
+  AssertCompressionQuality({0x00, 0x00, 0x00, 0x10},
+                           {0x22, 0x00, 0x00, 0x10, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, RepeatedBytesAfterUncompressable) {
+  AssertCompressionQuality({0x01, 0x00, 0x00, 0x00},
+                           {0x00, 0x01, 0x22, 0x00, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, RepeatedBytesAfterUncompressableRepeated) {
+  AssertCompressionQuality(
+      {0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02},
+      {0x22, 0x00, 0x04, 0x01, 0x00, 0x00, 0x00, 0x02, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, RepeatedBytesBeforeUncompressableRepeated) {
+  AssertCompressionQuality(
+      {0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00},
+      {0x04, 0x01, 0x00, 0x00, 0x00, 0x02, 0x22, 0x00, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionDecompressionEmptyData) {
+  ROM rom;
+  uchar empty_input[0] = {};
+  auto comp_result = ExpectCompressOk(rom, empty_input, 0);
+  EXPECT_EQ(0, comp_result.size());
+
+  auto decomp_result = ExpectDecompressOk(rom, empty_input, 0);
+  EXPECT_EQ(0, decomp_result.size());
+}
+
 TEST(LC_LZ2_CompressionTest, NewDecompressionPieceOk) {
   char command = 1;
   int length = 1;
@@ -83,6 +161,248 @@ TEST(LC_LZ2_CompressionTest, NewDecompressionPieceOk) {
   }
 }
 
+// TODO: Check why header built is off by one
+// 0x25 instead of 0x24
+TEST(LC_LZ2_CompressionTest, CompressionSingleSet) {
+  ROM rom;
+  uchar single_set[5] = {0x2A, 0x2A, 0x2A, 0x2A, 0x2A};
+  uchar single_set_expected[3] = {BUILD_HEADER(1, 5), 0x2A, 0xFF};
+
+  auto comp_result = ExpectCompressOk(rom, single_set, 5);
+  EXPECT_THAT(single_set_expected, ElementsAreArray(comp_result.data(), 3));
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionSingleWord) {
+  ROM rom;
+  uchar single_word[6] = {0x2A, 0x01, 0x2A, 0x01, 0x2A, 0x01};
+  uchar single_word_expected[4] = {BUILD_HEADER(0x02, 0x06), 0x2A, 0x01, 0xFF};
+
+  auto comp_result = ExpectCompressOk(rom, single_word, 6);
+  EXPECT_THAT(single_word_expected, ElementsAreArray(comp_result.data(), 4));
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionSingleIncrement) {
+  ROM rom;
+  uchar single_inc[3] = {0x01, 0x02, 0x03};
+  uchar single_inc_expected[3] = {BUILD_HEADER(0x03, 0x03), 0x01, 0xFF};
+  auto comp_result = ExpectCompressOk(rom, single_inc, 3);
+  EXPECT_THAT(single_inc_expected, ElementsAreArray(comp_result.data(), 3));
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionSingleCopy) {
+  ROM rom;
+  uchar single_copy[4] = {0x03, 0x0A, 0x07, 0x14};
+  uchar single_copy_expected[6] = {
+      BUILD_HEADER(0x00, 0x04), 0x03, 0x0A, 0x07, 0x14, 0xFF};
+  auto comp_result = ExpectCompressOk(rom, single_copy, 4);
+  EXPECT_THAT(single_copy_expected, ElementsAreArray(comp_result.data(), 6));
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionSingleOverflowIncrement) {
+  AssertCompressionQuality({0xFE, 0xFF, 0x00, 0x01},
+                           {BUILD_HEADER(0x03, 0x04), 0xFE, 0xFF});
+}
+/**
+
+TEST(LC_LZ2_CompressionTest, CompressionSingleCopyRepeat) {
+  std::vector<uint8_t> single_copy_expected = {0x03, 0x0A, 0x07, 0x14,
+                                               0x03, 0x0A, 0x07, 0x14};
+
+  auto comp_result = ExpectCompressV3Ok(
+      single_copy_expected, {BUILD_HEADER(0x00, 0x04), 0x03, 0x0A, 0x07, 0x14,
+                             BUILD_HEADER(0x04, 0x04), 0x00, 0x00, 0xFF});
+  EXPECT_THAT(single_copy_expected, ElementsAreArray(comp_result.data(), 6));
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionMixedRepeatIncrement) {
+  AssertCompressionQuality(
+      {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+       0x05, 0x02, 0x05, 0x02, 0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02,
+       0x05, 0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05},
+      {BUILD_HEADER(0x01, 0x04), 0x05, BUILD_HEADER(0x03, 0x06), 0x06,
+       BUILD_HEADER(0x00, 0x01), 0x05, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionMixedIncrementIntraCopyOffset) {
+  // "Mixing, inc, alternate, intra copy"
+  // compress start: 3, length: 21
+  // compressed length: 9
+  AssertCompressionQuality(
+      {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+       0x05, 0x02, 0x05, 0x02, 0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02,
+       0x05, 0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05},
+      {BUILD_HEADER(0x03, 0x07), 0x05, BUILD_HEADER(0x02, 0x06), 0x05, 0x02,
+       BUILD_HEADER(0x04, 0x08), 0x05, 0x00, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionMixedIncrementIntraCopySource) {
+  // "Mixing, inc, alternate, intra copy"
+  // 0, 28
+  // 16
+  AssertCompressionQuality(
+      {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+       0x05, 0x02, 0x05, 0x02, 0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02,
+       0x05, 0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05},
+      {BUILD_HEADER(0x01, 0x04), 0x05, BUILD_HEADER(0x03, 0x06), 0x06,
+       BUILD_HEADER(0x02, 0x06), 0x05, 0x02, BUILD_HEADER(0x04, 0x08), 0x08,
+       0x00, BUILD_HEADER(0x00, 0x04), 0x08, 0x0A, 0x00, 0x05, 0xFF});
+}
+
+// Extended Header
+// 111CCCLL LLLLLLLL
+// CCC:        Real command
+// LLLLLLLLLL: Length
+
+// Normally you have 5 bits for the length, so the maximum value you can
+// represent is 31 (which outputs 32 bytes). With the long length, you get 5
+// more bits for the length, so the maximum value you can represent becomes
+// 1023, outputting 1024 bytes at a time.
+
+void build_extended_header(uint8_t command, uint8_t length, uint8_t& byte1,
+                           uint8_t& byte2) {
+  byte1 = command << 3;
+  byte1 += (length - 1);
+  byte1 += 0b11100000;
+  byte2 = length >> 3;
+}
+
+std::vector<uint8_t> CreateRepeatedBetweenUncompressable(
+    int leftUncompressedSize, int repeatedByteSize, int rightUncompressedSize) {
+  std::vector<uint8_t> result(
+      leftUncompressedSize + repeatedByteSize + rightUncompressedSize, 0);
+  std::fill_n(result.begin() + leftUncompressedSize, repeatedByteSize, 0x00);
+  return result;
+}
+
+TEST(LC_LZ2_CompressionTest, LengthBorderCompression) {
+  // "Length border compression"
+  std::vector<uint8_t> result(42, 0);
+  std::fill_n(result.begin(), 42, 0x05);
+  AssertCompressionQuality(result, {BUILD_HEADER(0x04, 42), 0x05, 0x05, 0xFF});
+
+  // "Extended length, 400 repeat of 5"
+  std::vector<uint8_t> result2(400, 0);
+  std::fill_n(result2.begin(), 400, 0x05);
+  uint8_t byte1;
+  uint8_t byte2;
+  build_extended_header(0x01, 42, byte1, byte2);
+  AssertCompressionQuality(result2, {byte1, byte2, 0x05, 0x05, 0xFF});
+
+  // "Extended length, 1050 repeat of 5"
+  std::vector<uint8_t> result3(1050, 0);
+  std::fill_n(result3.begin(), 1050, 0x05);
+  uint8_t byte3;
+  uint8_t byte4;
+  build_extended_header(0x04, 1050, byte3, byte4);
+  AssertCompressionQuality(result3, {byte3, byte4, 0x05, 0x05, 0xFF});
+
+  // // "Extended length, 2050 repeat of 5"
+  std::vector<uint8_t> result4(2050, 0);
+  std::fill_n(result4.begin(), 2050, 0x05);
+  uint8_t byte5;
+  uint8_t byte6;
+  build_extended_header(0x04, 2050, byte5, byte6);
+  AssertCompressionQuality(result4, {byte5, byte6, 0x05, 0x05, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionExtendedWordCopy) {
+  // ROM rom;
+  // uchar buffer[3000];
+  // for (unsigned int i = 0; i < 3000; i += 2) {
+  //   buffer[i] = 0x05;
+  //   buffer[i + 1] = 0x06;
+  // }
+  // uchar hightlength_word_1050[] = {
+  //     0b11101011, 0xFF, 0x05, 0x06, BUILD_HEADER(0x02, 0x1A), 0x05, 0x06,
+  //     0xFF};
+
+  // // "Extended word copy"
+  // auto comp_result = ExpectCompressOk(rom, buffer, 1050);
+  // EXPECT_THAT(hightlength_word_1050, ElementsAreArray(comp_result.data(),
+  // 8));
+
+  std::vector<uint8_t> buffer(3000, 0);
+  std::fill_n(buffer.begin(), 3000, 0x05);
+  for (unsigned int i = 0; i < 3000; i += 2) {
+    buffer[i] = 0x05;
+    buffer[i + 1] = 0x06;
+  }
+
+  uint8_t byte1;
+  uint8_t byte2;
+  build_extended_header(0x02, 0x1A, byte1, byte2);
+  AssertCompressionQuality(
+      buffer, {0b11101011, 0xFF, 0x05, 0x06, byte1, byte2, 0x05, 0x06, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionMixedPatterns) {
+  AssertCompressionQuality(
+      {0x05, 0x05, 0x05, 0x06, 0x07, 0x06, 0x07, 0x08, 0x09, 0x0A},
+      {BUILD_HEADER(0x01, 0x03), 0x05, BUILD_HEADER(0x02, 0x04), 0x06, 0x07,
+       BUILD_HEADER(0x03, 0x03), 0x08, 0xFF});
+}
+
+TEST(LC_LZ2_CompressionTest, CompressionLongIntraCopy) {
+  ROM rom;
+  uchar long_data[15] = {0x05, 0x06, 0x07, 0x08, 0x05, 0x06, 0x07, 0x08,
+                         0x05, 0x06, 0x07, 0x08, 0x05, 0x06, 0x07};
+  uchar long_expected[] = {BUILD_HEADER(0x00, 0x04), 0x05, 0x06, 0x07, 0x08,
+                           BUILD_HEADER(0x04, 0x0C), 0x00, 0x00, 0xFF};
+
+  auto comp_result = ExpectCompressOk(rom, long_data, 15);
+  EXPECT_THAT(long_expected,
+              ElementsAreArray(comp_result.data(), sizeof(long_expected)));
+}
+
+*/
+// Tests for HandleDirectCopy
+
+TEST(HandleDirectCopyTest, NotDirectCopyWithAccumulatedBytes) {
+  CompressionContext context({0x01, 0x02, 0x03}, 0, 3);
+  context.cmd_with_max = kCommandByteFill;
+  context.comp_accumulator = 2;
+  HandleDirectCopy(context);
+  EXPECT_EQ(context.compressed_data.size(), 3);
+}
+
+TEST(HandleDirectCopyTest, NotDirectCopyWithoutAccumulatedBytes) {
+  CompressionContext context({0x01, 0x02, 0x03}, 0, 3);
+  context.cmd_with_max = kCommandByteFill;
+  HandleDirectCopy(context);
+  EXPECT_EQ(context.compressed_data.size(), 2);  // Header + 1 byte
+}
+
+TEST(HandleDirectCopyTest, AccumulateBytesWithoutMax) {
+  CompressionContext context({0x01, 0x02, 0x03}, 0, 3);
+  context.cmd_with_max = kCommandDirectCopy;
+  HandleDirectCopy(context);
+  EXPECT_EQ(context.comp_accumulator, 1);
+  EXPECT_EQ(context.compressed_data.size(), 0);  // No data added yet
+}
+
+// Tests for CheckIncByteV3
+
+TEST(CheckIncByteV3Test, IncreasingSequence) {
+  CompressionContext context({0x01, 0x02, 0x03}, 0, 3);
+  CheckIncByteV3(context);
+  EXPECT_EQ(context.current_cmd.data_size[kCommandIncreasingFill], 3);
+}
+
+TEST(CheckIncByteV3Test, IncreasingSequenceSurroundedByIdenticalBytes) {
+  CompressionContext context({0x01, 0x02, 0x03, 0x04, 0x01}, 1,
+                             3);  // Start from index 1
+  CheckIncByteV3(context);
+  EXPECT_EQ(context.current_cmd.data_size[kCommandIncreasingFill],
+            0);  // Reset to prioritize direct copy
+}
+
+TEST(CheckIncByteV3Test, NotAnIncreasingSequence) {
+  CompressionContext context({0x01, 0x01, 0x03}, 0, 3);
+  CheckIncByteV3(context);
+  EXPECT_EQ(context.current_cmd.data_size[kCommandIncreasingFill],
+            1);  // Only one byte is detected
+}
+
 TEST(LC_LZ2_CompressionTest, DecompressionValidCommand) {
   ROM rom;
   Bytes simple_copy_input = {BUILD_HEADER(0x00, 0x02), 0x2A, 0x45, 0xFF};
@@ -109,205 +429,5 @@ TEST(LC_LZ2_CompressionTest, DecompressionMixingCommand) {
   EXPECT_THAT(random1_o, ElementsAreArray(decomp_result.data(), 9));
 }
 
-// TODO: Check why header built is off by one 
-// 0x25 instead of 0x24
-// TEST(LC_LZ2_CompressionTest, CompressionSingleSet) {
-//   ROM rom;
-//   uchar single_set[5] = {0x2A, 0x2A, 0x2A, 0x2A, 0x2A};
-//   uchar single_set_expected[3] = {BUILD_HEADER(1, 5), 0x2A, 0xFF};
-
-//   auto comp_result = ExpectCompressOk(rom, single_set, 5);
-//   EXPECT_THAT(single_set_expected, ElementsAreArray(comp_result.data(), 3));
-// }
-
-TEST(LC_LZ2_CompressionTest, CompressionSingleWord) {
-  ROM rom;
-  uchar single_word[6] = {0x2A, 0x01, 0x2A, 0x01, 0x2A, 0x01};
-  uchar single_word_expected[4] = {BUILD_HEADER(0x02, 0x06), 0x2A, 0x01, 0xFF};
-
-  auto comp_result = ExpectCompressOk(rom, single_word, 6);
-  EXPECT_THAT(single_word_expected, ElementsAreArray(comp_result.data(), 4));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionSingleIncrement) {
-  ROM rom;
-  uchar single_inc[3] = {0x01, 0x02, 0x03};
-  uchar single_inc_expected[3] = {BUILD_HEADER(0x03, 0x03), 0x01, 0xFF};
-  auto comp_result = ExpectCompressOk(rom, single_inc, 3);
-  EXPECT_THAT(single_inc_expected, ElementsAreArray(comp_result.data(), 3));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionSingleCopy) {
-  ROM rom;
-  uchar single_copy[4] = {0x03, 0x0A, 0x07, 0x14};
-  uchar single_copy_expected[6] = {
-      BUILD_HEADER(0x00, 0x04), 0x03, 0x0A, 0x07, 0x14, 0xFF};
-  auto comp_result = ExpectCompressOk(rom, single_copy, 4);
-  EXPECT_THAT(single_copy_expected, ElementsAreArray(comp_result.data(), 6));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionSingleCopyRepeat) {
-  ROM rom;
-  uchar single_copy_repeat[8] = {0x03, 0x0A, 0x07, 0x14, 0x03, 10, 0x07, 0x14};
-  uchar single_copy_repeat_expected[9] = {
-      BUILD_HEADER(0x00, 0x04), 0x03, 0x0A, 0x07, 0x14,
-      BUILD_HEADER(0x04, 0x04), 0x00, 0x00, 0xFF};
-  auto comp_result = ExpectCompressOk(rom, single_copy_repeat, 8);
-  EXPECT_THAT(single_copy_repeat_expected,
-              ElementsAreArray(comp_result.data(), 9));
-}
-
-/* Hiding tests until I figure out a better PR to address the bug
-TEST(LC_LZ2_CompressionTest, CompressionSingleOverflowIncrement) {
-  ROM rom;
-  uchar overflow_inc[4] = {0xFE, 0xFF, 0x00, 0x01};
-  uchar overflow_inc_expected[3] = {BUILD_HEADER(0x03, 0x04), 0xFE, 0xFF};
-
-  auto comp_result = ExpectCompressOk(rom, overflow_inc, 4);
-  EXPECT_THAT(overflow_inc_expected, ElementsAreArray(comp_result.data(), 3));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionMixedRepeatIncrement) {
-  ROM rom;
-  uchar to_compress_string[28] = {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08,
-                                  0x09, 0x0A, 0x0B, 0x05, 0x02, 0x05, 0x02,
-                                  0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02, 0x05,
-                                  0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05};
-  uchar repeat_and_inc_copy_expected[7] = {BUILD_HEADER(0x01, 0x04),
-                                           0x05,
-                                           BUILD_HEADER(0x03, 0x06),
-                                           0x06,
-                                           BUILD_HEADER(0x00, 0x01),
-                                           0x05,
-                                           0xFF};
-  // Mixing, repeat, inc, trailing copy
-  auto comp_result = ExpectCompressOk(rom, to_compress_string, 28);
-  EXPECT_THAT(repeat_and_inc_copy_expected,
-              ElementsAreArray(comp_result.data(), 7));
-}
- */
-
-TEST(LC_LZ2_CompressionTest, CompressionMixedIncrementIntraCopyOffset) {
-  ROM rom;
-  uchar to_compress_string[] = {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08,
-                                0x09, 0x0A, 0x0B, 0x05, 0x02, 0x05, 0x02,
-                                0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02, 0x05,
-                                0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05};
-  uchar inc_word_intra_copy_expected[] = {BUILD_HEADER(0x03, 0x07),
-                                          0x05,
-                                          BUILD_HEADER(0x02, 0x06),
-                                          0x05,
-                                          0x02,
-                                          BUILD_HEADER(0x04, 0x08),
-                                          0x05,
-                                          0x00,
-                                          0xFF};
-
-  // "Mixing, inc, alternate, intra copy"
-  // compress start: 3, length: 21
-  // compressed length: 9
-  auto comp_result = ExpectCompressOk(rom, to_compress_string + 3, 21);
-  EXPECT_THAT(inc_word_intra_copy_expected,
-              ElementsAreArray(comp_result.data(), 9));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionMixedIncrementIntraCopySource) {
-  ROM rom;
-  uchar to_compress_string[] = {0x05, 0x05, 0x05, 0x05, 0x06, 0x07, 0x08,
-                                0x09, 0x0A, 0x0B, 0x05, 0x02, 0x05, 0x02,
-                                0x05, 0x02, 0x0A, 0x0B, 0x05, 0x02, 0x05,
-                                0x02, 0x05, 0x02, 0x08, 0x0A, 0x00, 0x05};
-  uchar all_expected[] = {BUILD_HEADER(0x01, 0x04),
-                          0x05,
-                          BUILD_HEADER(0x03, 0x06),
-                          0x06,
-                          BUILD_HEADER(0x02, 0x06),
-                          0x05,
-                          0x02,
-                          BUILD_HEADER(0x04, 0x08),
-                          0x08,
-                          0x00,
-                          BUILD_HEADER(0x00, 0x04),
-                          0x08,
-                          0x0A,
-                          0x00,
-                          0x05,
-                          0xFF};
-  // "Mixing, inc, alternate, intra copy"
-  // 0, 28
-  // 16
-  auto comp_result = ExpectCompressOk(rom, to_compress_string, 28);
-  EXPECT_THAT(all_expected, ElementsAreArray(comp_result.data(), 16));
-}
-
-// TEST(LC_LZ2_CompressionTest, LengthBorderCompression) {
-//   ROM rom;
-//   uchar buffer[3000];
-
-//   for (unsigned int i = 0; i < 3000; i++) buffer[i] = 0x05;
-//   uchar ext_length_expected_42[] = {0b11100100, 0x29, 0x05, 0xFF};
-//   uchar ext_length_expected_400[] = {0b11100101, 0x8F, 0x05, 0xFF};
-//   uchar ext_length_expected_1050[] = {
-//       0b11100111, 0xFF, 0x05, BUILD_HEADER(0x01, 0x1A), 0x05, 0xFF};
-//   uchar ext_length_expected_2050[] = {
-//       0b11100111, 0xFF, 0x05, 0b11100111, 0xFF, 0x05, BUILD_HEADER(0x01, 0x02),
-//       0x05,       0xFF};
-
-//   // "Extended length, 42 repeat of 5"
-//   auto comp_result = ExpectCompressOk(rom, buffer, 42);
-//   EXPECT_THAT(ext_length_expected_42, ElementsAreArray(comp_result.data(), 4));
-
-//   // "Extended length, 400 repeat of 5"
-//   comp_result = ExpectCompressOk(rom, buffer, 400);
-//   EXPECT_THAT(ext_length_expected_400, ElementsAreArray(comp_result.data(), 4));
-
-//   // "Extended length, 1050 repeat of 5"
-//   comp_result = ExpectCompressOk(rom, buffer, 1050);
-//   EXPECT_THAT(ext_length_expected_1050,
-//               ElementsAreArray(comp_result.data(), 6));
-
-//   // "Extended length, 2050 repeat of 5"
-//   comp_result = ExpectCompressOk(rom, buffer, 2050);
-//   EXPECT_THAT(ext_length_expected_2050,
-//               ElementsAreArray(comp_result.data(), 9));
-// }
-
-TEST(LC_LZ2_CompressionTest, CompressionExtendedWordCopy) {
-  ROM rom;
-  uchar buffer[3000];
-  for (unsigned int i = 0; i < 3000; i += 2) {
-    buffer[i] = 0x05;
-    buffer[i + 1] = 0x06;
-  }
-  uchar hightlength_word_1050[] = {
-      0b11101011, 0xFF, 0x05, 0x06, BUILD_HEADER(0x02, 0x1A), 0x05, 0x06, 0xFF};
-
-  // "Extended word copy"
-  auto comp_result = ExpectCompressOk(rom, buffer, 1050);
-  EXPECT_THAT(hightlength_word_1050, ElementsAreArray(comp_result.data(), 8));
-}
-
-TEST(LC_LZ2_CompressionTest, CompressionDecompressionEmptyData) {
-  ROM rom;
-  uchar empty_input[0] = {};
-  auto comp_result = ExpectCompressOk(rom, empty_input, 0);
-  EXPECT_EQ(0, comp_result.size());
-
-  auto decomp_result = ExpectDecompressOk(rom, empty_input, 0);
-  EXPECT_EQ(0, decomp_result.size());
-}
-
-// TEST(LC_LZ2_CompressionTest, CompressionDecompressionAllBitsSet) {
-//   ROM rom;
-//   uchar all_bits_set[5] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
-//   uchar all_bits_set_expected[3] = {BUILD_HEADER(0x01, 0x05), 0xFF, 0xFF};
-
-//   auto comp_result = ExpectCompressOk(rom, all_bits_set, 5);
-//   EXPECT_THAT(all_bits_set_expected, ElementsAreArray(comp_result.data(), 3));
-
-//   auto decomp_result = ExpectDecompressOk(rom, all_bits_set_expected, 3);
-//   EXPECT_THAT(all_bits_set, ElementsAreArray(decomp_result.data(), 5));
-// }
-
 }  // namespace gfx_test
 }  // namespace yaze_test