#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, },
          flushed_{false},
          ptr_ {0}
    {
        STA_ASSERT(flash != nullptr);
        STA_ASSERT(endSec > startSec);

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

    template <typename T>
    bool Logger<T>::searchCriterion(uint8_t * buffer)
    {
        for (size_t i = 0; i < W25QXX_PAGE_SIZE; i++)
        {
            if (buffer[i] != 0xFF)
            {
                return false;
            }
        }
        
        return true;
    }

    template <typename T>
    void Logger<T>::findLast()
    {
        uint32_t left   = start_;
        uint32_t right  = end_+1;
        uint32_t middle;

        uint8_t * buffer = new uint8_t[256];

        while (left <= right)
        {
            middle = (left + right) / 2;
            flash_->readData(middle * W25QXX_SECTOR_SIZE, buffer, W25QXX_PAGE_SIZE);
            
            if (middle == 0 && searchCriterion(buffer))
            {
            	break;
            }

            if (searchCriterion(buffer))
            {
            	if (middle == 0)
            	{
            		break;
            	}
            	
            	flash_->readData((middle-1) * W25QXX_SECTOR_SIZE, buffer, W25QXX_PAGE_SIZE);
            	if (!searchCriterion(buffer))
            	{
            		break;
            	}
            
                right = middle; 
            }
            else
            {
                left = middle + 1;
            }
        }

        middle = (left + right) / 2;
        delete[] buffer;

        address_ = middle * W25QXX_SECTOR_SIZE;
        ptr_ = 0;
    }

    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_)
        {
            // If the current page hasn't been uploaded yet, do it now.
            if (!flushed_)
            {
                flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
                flushed_ = true;
            }
            return false;
        }

        // Convert the data to a byte array.
        uint8_t * bytes = reinterpret_cast<uint8_t*>(data);
        uint32_t length = sizeof(T);

		// If the written data exceeds the remaining bytes in the page.
		while (ptr_ + length >= W25QXX_PAGE_SIZE)
		{
	        // Bytes remaining until the page is full.
			uint32_t remaining = W25QXX_PAGE_SIZE - ptr_;
		
			// 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;

        return true;
    }

    template <typename T>
    bool Logger<T>::write(T data)
    {
        return write(&data);
    }

    template <typename T>
    void Logger<T>::clear()
    {
        uint32_t left   = start_;
        uint32_t right  = end_+1;
        uint32_t middle;

        // Erase all sectors the binary search would check when trying to find the last written page.
        while (left <= right)
        {
            middle = (left + right) / 2;
            flash_->sectorErase(middle * W25QXX_SECTOR_SIZE, true);
            right = middle;
            
            if (middle == 0)
            	break;
            	
            flash_->sectorErase((middle-1) * W25QXX_SECTOR_SIZE, true);
        }

        middle = (left + right) / 2;
        flash_->sectorErase(middle * W25QXX_SECTOR_SIZE, true);

        address_ = start_ * W25QXX_SECTOR_SIZE; 
        ptr_ = 0;
        flushed_ = false;
    }

    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