#include "simple_flash_storage.h"

#include <stdint.h>
#include <string.h>

#include "app_error.h"
#include "nrf_fstorage_sd.h"

//#include "nrf_log.h"
//#include "nrf_log_ctrl.h"
//#include "nrf_log_default_backends.h"
#include "log.h"

static uint32_t first_empty_addr;

nrf_fstorage_t fstorage;

static void fstorage_evt_handler(nrf_fstorage_evt_t * p_evt) {
    if (p_evt->result != NRF_SUCCESS)
    {
        __LOG(LOG_SRC_APP, LOG_LEVEL_ERROR, "--> Event received: ERROR while executing an fstorage operation: code %d \n", 
	    p_evt->result);
	//NRF_LOG_INFO("--> Event received: ERROR while executing an fstorage operation: code %d \n", p_evt->result);
        return;
    }

    switch (p_evt->id)
    {
        case NRF_FSTORAGE_EVT_WRITE_RESULT:
        {
            __LOG(LOG_SRC_APP, LOG_LEVEL_DBG1, "--> Success: wrote %d bytes at address 0x%x. \n",
                         p_evt->len, p_evt->addr);
	    //NRF_LOG_INFO("--> Success: wrote %d bytes at address 0x%x. \n", p_evt->len, p_evt->addr);
        } break;

        case NRF_FSTORAGE_EVT_ERASE_RESULT:
        {
            __LOG(LOG_SRC_APP, LOG_LEVEL_DBG1, "--> Success: erased %d page from address 0x%x. \n",
                         p_evt->len, p_evt->addr);
	    //NRF_LOG_INFO("--> Success: erased %d page from address 0x%x. \n", p_evt->len, p_evt->addr)
        } break;

        default:
            break;
    }
}

ret_code_t flash_store_data(const level_t * values) {
    ret_code_t rc;
    static flash_entry_t data_to_store;
    data_to_store.label = 0xFACE;
    memcpy(data_to_store.channel_vals, values, NUM_CHANNELS);
    
    if (first_empty_addr >= fstorage.end_addr) {
	flash_clean_data();
    }

    rc = nrf_fstorage_write(&fstorage, first_empty_addr, &data_to_store, ENTRY_SIZE, NULL);
    if (rc != NRF_SUCCESS) {
	__LOG(LOG_SRC_APP, LOG_LEVEL_ERROR, "Error occured when writing to persistent storage: code %d \n", rc);
	//NRF_LOG_INFO("Error occured when writing to persistent storage: code %d \n", rc);
    }

    first_empty_addr += ENTRY_SIZE;
    return rc;
}

ret_code_t flash_retrieve_data(level_t * values) {
    ret_code_t rc;
    flash_entry_t data_to_retrieve;
    
    uint32_t addr_to_read = first_empty_addr - ENTRY_SIZE;
    if (first_empty_addr == fstorage.start_addr) {
	addr_to_read = 1 + fstorage.end_addr - ENTRY_SIZE;
    }

    rc = nrf_fstorage_read(&fstorage, addr_to_read, &data_to_retrieve, ENTRY_SIZE);
    if (rc != NRF_SUCCESS) {
	__LOG(LOG_SRC_APP, LOG_LEVEL_ERROR, "Error occured when reading persistent storage: code %d \n", rc);
	//NRF_LOG_INFO("Error occured when reading persistent storage: code %d \n", rc);
    } else {
	memcpy(values, data_to_retrieve.channel_vals, NUM_CHANNELS);
    }

    return rc;
}

ret_code_t flash_clean_data() {
    ret_code_t rc;
    rc = nrf_fstorage_erase(&fstorage, fstorage.start_addr, NUM_PAGES, NULL);
    if (rc != NRF_SUCCESS) {
        __LOG(LOG_SRC_APP, LOG_LEVEL_ERROR, "Error occured when cleaning persistent storage: code %d \n", rc);
	//NRF_LOG_INFO("Error occured when cleaning persistent storage: code %d \n", rc);
    }
    first_empty_addr = fstorage.start_addr;

    return rc;
}

uint32_t flash_addr_pointer() {
    return first_empty_addr;
}

void flash_init() {
    nrf_fstorage_api_t * p_fs_api;
    p_fs_api = &nrf_fstorage_sd;
    
    fstorage.evt_handler = fstorage_evt_handler;
    fstorage.start_addr = START_ADDRESS;
    fstorage.end_addr = END_ADDRESS;

    APP_ERROR_CHECK(nrf_fstorage_init(&fstorage, p_fs_api, NULL));
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "Simple flash storage module enabled \n");
    //NRF_LOG_INFO("Simple flash storage module enabled \n");
    
    __LOG(LOG_SRC_APP, LOG_LEVEL_DBG1, "Read Address 0x%x \n", fstorage.start_addr);
    //NRF_LOG_INFO("Read Address 0x%x \n", fstorage.start_addr);

    flash_entry_t latest_data;
    APP_ERROR_CHECK(nrf_fstorage_read(&fstorage, fstorage.start_addr, &latest_data, ENTRY_SIZE));
    __LOG(LOG_SRC_APP, LOG_LEVEL_DBG1, "Read Address 0x%x \n", fstorage.start_addr);
    //NRF_LOG_INFO("Read Address 0x%x \n", fstorage.start_addr);
    first_empty_addr = fstorage.start_addr;
    while (latest_data.label != 0xffff) {
	first_empty_addr += ENTRY_SIZE;
	if (first_empty_addr >= fstorage.end_addr) {
	    break;
	} 
	else {
	    APP_ERROR_CHECK(nrf_fstorage_read(&fstorage, first_empty_addr, &latest_data, ENTRY_SIZE));
	    __LOG(LOG_SRC_APP, LOG_LEVEL_DBG1, "Read Address 0x%x \n", first_empty_addr);
	    //NRF_LOG_INFO("Read Address 0x%x \n", first_empty_addr);
	}

    }

}