#include "app_timer.h"
#include "error.h"
#include "fds_flash.h"
#include "fstorage.h"
#include "nrf_delay.h"
#include "nrf_fstorage.h"
#include "nrf_fstorage_sd.h"
#include "nrf_fstorage_nvmc.h"
#include "nrf_log.h"
#include "nrf_soc.h"
#include "spi_flash.h"

#define FSTORAGE_PAGE_NUM    3
#define FSTORAGE_WAIT_CNT    1000



static bool _gFstorageInitialized = false;
static bool _gFstorageEraseDone   = true;
static bool _gFstorageWriteDone   = true;
static bool _gFstoragePageDirty   = true;
static fstorage_state_t _gFstorageState = FSTORAGE_STATE_UNKNOWN;

static void _fstorageEvtHandler(nrf_fstorage_evt_t * p_evt);

NRF_FSTORAGE_DEF(nrf_fstorage_t _gFstorage) =
{
    /* Set a handler for fstorage events. */
    .evt_handler = _fstorageEvtHandler,

    /* These below are the boundaries of the flash space assigned to this instance of fstorage.
     * You must set these manually, even at runtime, before nrf_fstorage_init() is called.
     * The function nrf5_flash_end_addr_get() can be used to retrieve the last address on the
     * last page of flash available to write data. */
    .start_addr = 0x7D000,
    .end_addr   = 0x80000,
};

static void _fstorageEvtHandler(nrf_fstorage_evt_t * pEvt) {
    NRF_LOG_DEBUG("Evt e\n");
    switch (pEvt->id) {
        case NRF_FSTORAGE_EVT_WRITE_RESULT:
        {
            NRF_LOG_DEBUG("Evt w\n");
            _gFstorageWriteDone = true;
            _gFstorageState     = FSTORAGE_STATE_WRITTEN;
            if (pEvt->result != NRF_SUCCESS) {
               NRF_LOG_WARNING("fstorage writing failed %d bytes at address 0x%x.",
                            pEvt->len, pEvt->addr);
            } else {
                NRF_LOG_DEBUG("NRF_FSTORAGE_EVT_WRITE_RESULT ok");
                _gFstoragePageDirty = true;
            }
        } break;

        case NRF_FSTORAGE_EVT_ERASE_RESULT:
        {
            NRF_LOG_DEBUG("Evt er\n");
            _gFstorageEraseDone = true;
            _gFstorageState     = FSTORAGE_STATE_ERASED;
            if (pEvt->result != NRF_SUCCESS) {
                NRF_LOG_WARNING("fstorage erased failed %d page from address 0x%x.",
                                 pEvt->len, pEvt->addr);
            } else {
                NRF_LOG_DEBUG("NRF_FSTORAGE_EVT_ERASE_RESULT ok");
                _gFstoragePageDirty = false;
            }
        } break;

        default:
            break;
    }
     NRF_LOG_DEBUG("Evt x\n");
}

static uint32_t _fstorageGetEndAddr(void) {
    const uint32_t bootloaderAddr = BOOTLOADER_ADDRESS;
    const uint32_t pageSz         = NRF_FICR->CODEPAGESIZE;
    const uint32_t codeSz         = NRF_FICR->CODESIZE;

    return (bootloaderAddr != 0xFFFFFFFF ?
            bootloaderAddr : (codeSz * pageSz));
}

static uint32_t _fstorageGetStartAddr(void) {
    const uint32_t eraseUnit = _gFstorage.p_flash_info->erase_unit;
    const uint32_t endAddr = _fstorageGetEndAddr();
    const uint32_t bootloaderAddr = BOOTLOADER_ADDRESS;
    uint32_t startAddr;
    if (bootloaderAddr == 0xFFFFFFFF) {
        startAddr = endAddr - FDS_VIRTUAL_PAGES * eraseUnit; 
    } else {
        startAddr = bootloaderAddr - FSTORAGE_PAGE_NUM * eraseUnit;
    }

    NRF_LOG_DEBUG("endAddr=0x%x", endAddr);
    NRF_LOG_DEBUG("startAddr=0x%x", startAddr);
    return startAddr;
}
static bool _fstorageIsEraseNeeded(void) {
    return _gFstoragePageDirty;
}    

static uint32_t _fstorageRoundUpU32(const uint32_t len) {
    if (len % sizeof(uint32_t)) {
        return (len + sizeof(uint32_t) - (len % sizeof(uint32_t)));
    }

    return len;
}

static void power_manage(void)
{
#ifdef SOFTDEVICE_PRESENT
    (void) sd_app_evt_wait();
#else
    __WFE();
#endif
}

int _fstorageWait(const uint32_t timeOutCnt) {
#if 0
    NRF_LOG_DEBUG("timeOutCnt=%u\n", timeOutCnt);
    const uint32_t startCnt   = app_timer_cnt_get();
    NRF_LOG_DEBUG("startCnt=%u\n", startCnt);
    uint32_t endCnt;
    uint32_t diffCnt;

    do {
        endCnt           = app_timer_cnt_get();
        diffCnt          = app_timer_cnt_diff_compute(endCnt, startCnt);
        if (diffCnt >= timeOutCnt) {
            NRF_LOG_WARNING("_fstorage wait timeout\n");
            return ERROR_FSTORAGE_WAIT_TIMEOUT;
        }

        if (_gFstorageEraseDone && _gFstorageWriteDone) {
            break;
        }
    } while (true);

    NRF_LOG_DEBUG("endCnt=%u\n", endCnt);

    //while (nrf_fstorage_is_busy(&_gFstorage))
    while (!_gFstorageEraseDone || !_gFstorageWriteDone) {
        NRF_LOG_DEBUG("pm e\n");
        power_manage();
        NRF_LOG_DEBUG("pm x\n");
    }
#endif
		return 0;
    
}

int _fstorageRead(const uint32_t addr, const uint32_t len, uint8_t* data) {
    if (!_gFstorageInitialized) {
        return ERROR_FSTORAGE_UNINITIALIZED;
    }   

    const ret_code_t errCode = nrf_fstorage_read(&_gFstorage, addr, data, len);
    if (errCode != NRF_SUCCESS) {
        NRF_LOG_WARNING("nrf_fstorage_read() failed 0x%x\n", errCode);
        return ERROR_FSTORAGE_READ_FAIL;
    }

    NRF_LOG_DEBUG("addr=0x%x, len=%u\n", addr, len);
    recordPrintData("data r", data);

    return 0;
}

int _fstorageWrite(const uint32_t addr, const uint32_t len, void const* pData) {
    if (!_gFstorageInitialized) {
        return ERROR_FSTORAGE_UNINITIALIZED;
    }

    if (!_gFstorageEraseDone) {
        return ERROR_FSTORAGE_ERASE_ON_GOING;
    }

    if (!_gFstorageWriteDone) {
        return ERROR_FSTORAGE_WRITE_ON_GOING;
    }

    _gFstorageWriteDone = false;
    _gFstorageState     = FSTORAGE_STATE_WRITING;
    const uint32_t wlen = _fstorageRoundUpU32(len);
    NRF_LOG_DEBUG("addr=0x%x, wlen=%u\n", addr, wlen);
    recordPrintData("pData w", pData);
    const ret_code_t retCode = nrf_fstorage_write(&_gFstorage, addr, pData, wlen, NULL);
    if (retCode != NRF_SUCCESS) {
        NRF_LOG_WARNING("nrf_fstorage_write() failed 0x%x", retCode);
        return ERROR_FSTORAGE_WRITE_FAIL;
    }

    _fstorageWait(FSTORAGE_WAIT_CNT);

    return 0;
}


int fstorageInit(void) {
    // initializing variables
    _gFstorageInitialized = false;
    _gFstorageEraseDone   = true;
    _gFstorageWriteDone   = true;
    _gFstoragePageDirty   = true;
    _gFstorageState = FSTORAGE_STATE_UNKNOWN;

    const ret_code_t errCode = nrf_fstorage_init(&_gFstorage, &nrf_fstorage_sd, NULL);
    if (errCode != NRF_SUCCESS) {
        NRF_LOG_WARNING("nrf_fstorage_init() failed 0x%x", errCode);
        return ERROR_FSTORAGE_INITIALIZE_FAIL;
    }

    _gFstorageInitialized = true;

    return 0;
}    

int fstorageErase(const uint32_t addr, const uint32_t pagesCnt) {
    if (!_gFstorageInitialized) {
        return ERROR_FSTORAGE_UNINITIALIZED;
    }

    if (!_gFstorageEraseDone) {
        return ERROR_FSTORAGE_ERASE_ON_GOING;
    }

    if (!_gFstorageWriteDone) {
        return ERROR_FSTORAGE_WRITE_ON_GOING;
    }    

    _gFstorageEraseDone = false;
    _gFstorageState     = FSTORAGE_STATE_ERASED;
    const ret_code_t errCode = nrf_fstorage_erase(&_gFstorage, addr, pagesCnt, NULL);
    if (errCode != NRF_SUCCESS) { 
        NRF_LOG_WARNING("nrf_fstorage_erase() failed 0x%x", errCode);
        return ERROR_FSTORAGE_ERASE_FAIL;
    }

    _fstorageWait(FSTORAGE_WAIT_CNT);
    
    return 0;
}

int fstorageReadRecord(const uint32_t addr, record_t* pRecord) {
    uint8_t data[RECORD_LEN] = {0};
    if (_fstorageRead(addr, RECORD_LEN, data) != 0) {
        return ERROR_FDS_READ_FAIL;
    }

    data2Record(data, pRecord);

    return 0;
}    

int fstorageUpdateRecord(const record_data_type_t type, const record_t* pRecord) {
    const uint32_t addr = _fstorageGetStartAddr();
    NRF_LOG_DEBUG("type=%d, addr=0x%x\n", type, addr);
    const fstorage_state_t state = fstorageGetState();
    NRF_LOG_DEBUG("state=%d\n", state);
    switch (type) {
        case RECORD_DATA_TYPE_START:
        {  
            NRF_LOG_DEBUG("START E\n");
            if ((state == FSTORAGE_STATE_UNKNOWN)) {
                fstorageErase(addr, 1);    
            } else if (state == FSTORAGE_STATE_ERASED) {    
                record_t record;
                record.offsetStart = 0;
                record.offsetEnd   = 0;
                fstorageWriteRecord(addr, &record);
            }    
            NRF_LOG_DEBUG("START x\n");
        }    
        break;

        case RECORD_DATA_TYPE_END:
        {
            NRF_LOG_DEBUG("END E\n");
            record_t record;

            if ((state == FSTORAGE_STATE_UNKNOWN) || (state == FSTORAGE_STATE_WRITTEN)) {
                if (fstorageReadRecord(addr, &record) != 0) {
                    NRF_LOG_WARNING("fstorageReadRecord(0x%x) failed\n", addr);
                    return ERROR_FSTORAGE_READ_FAIL;
                }
                NRF_LOG_DEBUG("record.offsetStart=0x%x", record.offsetStart);
                NRF_LOG_DEBUG("record.offsetEnd=0x%x",   record.offsetEnd);
                fstorageErase(addr, 1);
            } else if (state == FSTORAGE_STATE_ERASED) {
                record.offsetEnd = pRecord->offsetEnd;
                NRF_LOG_DEBUG("record.offsetEnd=0x%x",   record.offsetEnd);
                fstorageWriteRecord(addr, &record);
            }    
            NRF_LOG_DEBUG("END X");
        }   
        break;    
    }    

    return 0;
}

int fstorageWriteRecord(const uint32_t addr, record_t* pRecord) {
    uint8_t data[RECORD_LEN] = {0};
    record2Data(pRecord, data);
    if (_fstorageWrite(addr, RECORD_LEN, data) != 0) {
        return ERROR_FSTORAGE_WRITE_FAIL;
    }

    return 0;
}

bool fstorageGetInitialized(void) {
    return _gFstorageInitialized;
}

bool fstorageGetEraseDone(void) {
    return _gFstorageEraseDone;
}

bool fstorageGetWriteDone(void) {
    return _gFstorageWriteDone;
}

bool fstorageGetPageDirty(void) {
    return _gFstoragePageDirty;
}

fstorage_state_t fstorageGetState(void) {
    return _gFstorageState;
}