Updated logging, binary search to find starting point.

include/sta/utils/logger.tpp

@@ -17,26 +17,52 @@ namespace sta
     {
         STA_ASSERT(flash != nullptr);
         STA_ASSERT(endAddr > startAddr);
+
+        // Jump to the last written page.
+        findLast();
     }
 
     template <typename T>
-    void Logger<T>::write(T data)
+    void Logger<T>::findLast()
     {
-        uint8_t * bytes = (uint8_t*)(&data);
+        address_ = this->flash_->findLast([](uint8_t * buffer) -> bool {
+            for (size_t i = 0; i < W25QXX_PAGE_SIZE; i++)
+            {
+                if (buffer[i] != 0xFF)
+                {
+                    return true;
+                }
+            }
+
+            return false;
+        }, sta::ChunkSize::PAGE, start_ * W25QXX_SECTOR_SIZE, end_ * W25QXX_SECTOR_SIZE);
+
+        STA_DEBUG_PRINTF("Starting at page with address %d\n", address_);
+    }
+
+    template <typename T>
+    bool Logger<T>::write(T data)
+    {
+        // If writing the data would exceed the segment length, return false and don't do anything.
+        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
+            return false;
+
+        // Convert the data to a byte array.
+        uint8_t * bytes = reinterpret_cast<uint8_t*>(&data);
         uint8_t length = sizeof(T);
 
         // Bytes remaining until the page is full.
         uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
 
-        STA_DEBUG_PRINTF("Writing at address %d\n", address_+ptr_);
+        STA_DEBUG_PRINTF("Writing at address %d\n", address_ + ptr_);
 
         // If the written data exceeds the remaining bytes in the page.
         if (ptr_ + length >= W25QXX_PAGE_SIZE)
         {
-            // If the segment is full, stop writing data to it.
-            if (address_ / W25QXX_SECTOR_SIZE == end_)
+            // If the page to written is in a new sector, erase the new sector before writing to it.
+            if (address_ % W25QXX_SECTOR_SIZE == 0)
             {
-                return;
+                flash_->sectorErase(address_);
             }
 
             std::memcpy(buffer_ + ptr_, bytes, remaining);
@@ -45,19 +71,17 @@ namespace sta
             bytes += remaining;
             length -= remaining;
             ptr_ = 0;
-            STA_DEBUG_PRINTLN("New PAGE!");
-
             address_ += W25QXX_PAGE_SIZE;
-
-            // If a new sector has to be started, erase the new sector.
-            if (address_ % W25QXX_SECTOR_SIZE == 0)
-            {
-                flash_->sectorErase(address_);
-            }
         }
 
         std::memcpy(buffer_ + ptr_, bytes, length);
         ptr_ += length;
+
+        // If the end of the segment was reached, flush the temporally stored data.
+        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
+            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
+
+        return true;
     }
 
     template <typename T>
@@ -73,19 +97,19 @@ namespace sta
     }
 
     template <typename T>
-    size_t Logger<T>::occupied()
+    size_t Logger<T>::count()
     {
-        return (address_ - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
+        return ((address_ + ptr_) - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
     }
 
     template <typename T>
     size_t Logger<T>::remaining()
     {
-        return (end_ * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
+        return (end_ * W25QXX_SECTOR_SIZE - (address_ + ptr_)) / sizeof(T);
     }
 
     template <typename T>
-    size_t Logger<T>::length()
+    size_t Logger<T>::capacity()
     {
         return (end_ - start_) * W25QXX_SECTOR_SIZE / sizeof(T);
     }
@@ -94,6 +118,7 @@ namespace sta
     T Logger<T>::operator[](std::size_t idx)
     {
         uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);
+        STA_DEBUG_PRINTF("Reading from address %d\n", address);
 
         // If the requested element is in the cache, read it from there.
         if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)