From f27c87a5b57355d22f5ba38128b6974fbb8b7ac8 Mon Sep 17 00:00:00 2001
From: Phillip Stephens <antidote.crk@gmail.com>
Date: Tue, 11 Oct 2022 11:38:08 -0700
Subject: [PATCH] More COutputStream

---
 include/Kyoto/Streams/COutputStream.hpp |  3 ++
 src/Kyoto/Streams/COutputStream.cpp     | 55 ++++++++++++++++++-------
 2 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/include/Kyoto/Streams/COutputStream.hpp b/include/Kyoto/Streams/COutputStream.hpp
index 2a63137a..241961bd 100644
--- a/include/Kyoto/Streams/COutputStream.hpp
+++ b/include/Kyoto/Streams/COutputStream.hpp
@@ -3,6 +3,7 @@
 
 #include "types.h"
 
+
 class COutputStream;
 
 template < typename T >
@@ -16,6 +17,7 @@ class COutputStream {
 public:
   COutputStream(int len);
   virtual ~COutputStream();
+  virtual void Write(const void* ptr, u32 len);
   void WriteBits(int val, int bitCount);
 
   void FlushShiftRegister();
@@ -46,6 +48,7 @@ public:
     ++mPosition;
   }
 
+
 private:
   uint mPosition;
   uint mBufLen;
diff --git a/src/Kyoto/Streams/COutputStream.cpp b/src/Kyoto/Streams/COutputStream.cpp
index 5be8ec84..f291efeb 100644
--- a/src/Kyoto/Streams/COutputStream.cpp
+++ b/src/Kyoto/Streams/COutputStream.cpp
@@ -22,29 +22,52 @@ void COutputStream::DoPut(const void* ptr, size_t len) {
   if (len == 0) {
     return;
   }
+  uint curLen = len;
 
   mNumWrites += len;
-  uint curLen = len;
   if (mBufLen <= len + mPosition) {
     memcpy((uchar*)mBufPtr + mPosition, ptr, len);
     mPosition += len;
-  } else {
-    while (curLen != 0) {
-      uint count = mBufLen - mPosition;
-      if (curLen < count) {
-        count = curLen;
-      }
-      if (count == 0) {
-        DoFlush();
-      } else {
-        memcpy((uchar*)mBufPtr + mPosition, (uchar*)ptr + (len - curLen), count);
-        mPosition += count;
-        curLen -= count;
-      }
+    return;
+  }
+
+  while (curLen != 0) {
+    uint count = mBufLen - mPosition;
+    uint offset = curLen;
+    if (curLen < count) {
+      count = curLen;
+    }
+    if (count != 0) {
+      memcpy((uchar*)mBufPtr + mPosition, (uchar*)ptr + offset, count);
+      mPosition += count;
+      curLen -= count;
+    } else {
+      DoFlush();
     }
   }
 }
 
-void COutputStream::Flush() {}
+void COutputStream::Flush() {
+  FlushShiftRegister();
+  DoFlush();
+}
 
-void COutputStream::DoFlush() {}
+void COutputStream::DoFlush() {
+  if (mPosition != 0) {
+    Write(mBufPtr, mPosition);
+    mPosition = 0;
+  }
+}
+
+static inline u32 min_containing_bytes(u32 v) {
+  v = 32 - v;
+  return (v / 8) + ((v % 8) != 0);
+}
+
+void COutputStream::FlushShiftRegister() {
+  if (mShiftRegisterOffset < 32) {
+    DoPut(&mShiftRegister, min_containing_bytes(mShiftRegisterOffset));
+    mShiftRegister = 0;
+    mShiftRegisterOffset = 32;
+  }
+}