#ifndef STA_UTILS_LOGGER_TPP
#define STA_UTILS_LOGGER_TPP

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

#define STA_LOGGER_VALID_DATA_BYTE      0x42

namespace sta
{
    template <typename T>
    Logger<T>::Logger(W25Qxx * flash, uint32_t startSec, uint32_t endSec)
        : flash_{flash},
          start_{startSec},
          end_{endSec},
          address_{end_ * 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   = 0;
        uint32_t right  = capacity()-1;
        uint32_t middle;

        while (left <= right)
        {
            middle = (left + right) / 2;
            

            // Check if we have to look in the upper half of the address space or the lower half.
            if (!isValid(middle))
            {
                // If we reached the first sector, we are finished.
            	if (middle == 0)
            	{
            		break;
            	}

            	if (isValid(middle-1))
            	{
            		break;
            	}
            
                right = middle; 
            }
            else
            {
                left = middle + 1;
            }
        }

        middle = (left + right) / 2;

        ptr_ = (middle * (sizeof(T)+1)) % W25QXX_PAGE_SIZE;
        address_ = (middle * (sizeof(T)+1)) - ptr_;
    }

    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 buffer[sizeof(T)+1];
        std::memcpy(buffer+1, data, sizeof(T));

        // Set the first byte to 0x42 to mark it as a valid data point.
        buffer[0] = STA_LOGGER_VALID_DATA_BYTE;
        uint8_t * buff_ptr = buffer;
        uint32_t length = sizeof(T)+1;

		// 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_, buff_ptr, remaining);
            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);

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

        std::memcpy(buffer_ + ptr_, buff_ptr, 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   = 0;
        uint32_t right  = capacity()-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;
            uint32_t address = middle * (sizeof(T)+1);
            uint32_t sector = (sector / W25QXX_SECTOR_SIZE);
            flash_->sectorErase(sector * W25QXX_SECTOR_SIZE, true);
            
            right = middle;
            
            if (middle == 0)
            	break;
            	     
            flash_->sectorErase((sector-1) * W25QXX_SECTOR_SIZE, true);
        }

        middle = (left + right) / 2;
        uint32_t sector = ((middle * (sizeof(T)+1)) / W25QXX_SECTOR_SIZE);
        flash_->sectorErase(sector * W25QXX_SECTOR_SIZE, true);

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

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

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

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

    template <typename T>
    bool Logger<T>::isValid(uint32_t i)
    {
    	uint32_t address = start_ * W25QXX_SECTOR_SIZE + i * (sizeof(T)+1);

        // If the requested element is in the cache, read it from there.
        if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
        {             
            return buffer_[address % W25QXX_PAGE_SIZE] == STA_LOGGER_VALID_DATA_BYTE;
        }
        else
        {
            uint8_t valid;
            flash_->readData(address, &valid, 1);
        	
            return valid == STA_LOGGER_VALID_DATA_BYTE;
        }
    }

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

        // If the requested element is in the cache, read it from there.
        if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
        {            
        	T data;
        	std::memcpy(&data, buffer_ + address % W25QXX_PAGE_SIZE + 1, sizeof(T)); 
            return data;
        }
        else
        {
            uint8_t buffer[sizeof(T)];
            flash_->readData(address+1, buffer, sizeof(T));
            
            T data;
        	std::memcpy(&data, buffer, sizeof(T)); 
        	
            return data;
        }
    }
} // namespace sta

#endif // STA_UTILS_LOGGER_TPP