#ifndef STA_UTILS_LOGGER_TPP
#define STA_UTILS_LOGGER_TPP

#include <sta/debug/debug.hpp>
#include <sta/drivers/w25qxx.hpp>

namespace sta
{
    template <typename T>
    Logger<T>::Logger(W25Qxx * flash, uint32_t startSec, uint32_t endSec)
        : flash_{flash},
          start_{startSec},
          end_{endSec},
          address_{start_ * W25QXX_SECTOR_SIZE},
          buffer_{0x00, },
          ptr_ {0}
    {
        STA_ASSERT(flash != nullptr);
        STA_ASSERT(endAddr > startAddr);
    }

    template <typename T>
    void Logger<T>::write(T data)
    {
        uint8_t * bytes = (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_);

        // 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_)
            {
                return;
            }

            std::memcpy(buffer_ + ptr_, bytes, remaining);
            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);

            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;
    }

    template <typename T>
    void Logger<T>::clear()
    {
        address_ = start_ * W25QXX_SECTOR_SIZE; 
        ptr_ = 0;

        for (uint32_t i = start_; i < end_; i++)
        {
            flash_->sectorErase(i * W25QXX_SECTOR_SIZE);
        }
    }

    template <typename T>
    size_t Logger<T>::occupied()
    {
        return (address_ - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
    }

    template <typename T>
    size_t Logger<T>::remaining()
    {
        return (end_ * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
    }

    template <typename T>
    size_t Logger<T>::length()
    {
        return (end_ - start_) * W25QXX_SECTOR_SIZE / sizeof(T);
    }

    template <typename T>
    T Logger<T>::operator[](std::size_t idx)
    {
        uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);

        // If the requested element is in the cache, read it from there.
        if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
        {
            STA_DEBUG_PRINTLN("Reading data from buffer!");
            uint8_t * ptr = buffer_ + address % W25QXX_PAGE_SIZE;
            return *reinterpret_cast<T*>(ptr);
        }
        else
        {
            uint8_t buffer[sizeof(T)];
            flash_->readData(address, buffer, sizeof(T));
            return *reinterpret_cast<T*>(buffer);
        }
    }
} // namespace sta

#endif // STA_UTILS_LOGGER_TPP