#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(endSec > startSec);

        // Jump to the last written page.
        findLast();
    }

    template <typename T>
    void Logger<T>::findLast()
    {
        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);
        uint32_t length = sizeof(T);

        // Bytes remaining until the page is full.
        uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;

        // If the written data exceeds the remaining bytes in the page.
        if (ptr_ + length >= W25QXX_PAGE_SIZE)
        {
            // If the page to written is in a new sector, erase the new sector before writing to it.
            if (address_ % W25QXX_SECTOR_SIZE == 0)
            {
                flash_->sectorErase(address_);
            }

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

            bytes += remaining;
            length -= remaining;
            ptr_ = 0;
            address_ += W25QXX_PAGE_SIZE;
        }

        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>
    void Logger<T>::clear()
    {
        address_ = start_ * W25QXX_SECTOR_SIZE; 
        ptr_ = 0;
    
        flash_->sectorErase(address_);
    }

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

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

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

    template <typename T>
    T Logger<T>::get(std::size_t i)
    {
        uint32_t address = start_ * W25QXX_SECTOR_SIZE + i * sizeof(T);

        // If the requested element is in the cache, read it from there.
        if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
        {
            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);
        }
    }

    template <typename T>
    T Logger<T>::operator[](std::size_t i)
    {
        return this->get(i);
    }
} // namespace sta

#endif // STA_UTILS_LOGGER_TPP