//
// Created by cfif on 07.10.22.
//
#include "SystemDelayInterface.h"
#include "InternalFlashPage.h"

#define FLASH_PAGE_SIZE 0x800

uint32_t iInternalFlashPage_ReadWord(uint32_t addressOnFlash) {
    return (*(__IO uint32_t *) (addressOnFlash));
}

size_t sInternalFlashPage_Read(
        uint32_t pageAddress,
        size_t offset,
        uint8_t *data,
        size_t size
) {

    if ((offset & (uint32_t) 0x3) != 0) {
        /* The offset is not a multiple of 4 */
        return 0;
    }

    size_t left = size;

    while (left) {
        if (left >= 4) {
            *((uint32_t *) data) = iInternalFlashPage_ReadWord(pageAddress + offset);
            data += 4;
            offset += 4;
            left -= 4;
        } else {
            uint32_t word = iInternalFlashPage_ReadWord(pageAddress + offset);
            for (uint8_t sub_byte = 0; sub_byte < left; ++sub_byte) {
                *data = ((uint8_t *) &word)[sub_byte];
                ++data;
            }
            offset += 4;
            left = 0;
        }
    }

    return size;
}

bool bInternalFlashPage_Clear(
        uint32_t pageAddress
) {
    FLASH_Unlock();

    if (FLASH_COMPL != FLASH_EraseOnePage(pageAddress)) {
        FLASH_Lock();
        return false;
    }

    FLASH_Lock();

    return true;
}

bool bInternalFlashPage_ClearRange(
        uint32_t firstPageAddr,
        uint32_t totalSize
) {
    if (totalSize % FLASH_PAGE_SIZE) {
        return false;
    }

    uint32_t endAddr = firstPageAddr + totalSize;
    for (; firstPageAddr < endAddr; firstPageAddr += FLASH_PAGE_SIZE) {
        if (!bInternalFlashPage_Clear(firstPageAddr)) {
            return false;
        }
    }

    return true;
}

bool bInternalFlashPage_CopyRange(
        uint32_t targetPageAddr,
        uint32_t sourcePageAddr,
        uint32_t size
) {
    if (!bInternalFlashPage_ClearRange(targetPageAddr, size)) {
        return false;
    }

    FLASH_Unlock();
    uint32_t offset = 0;

    uint32_t word;
    while (offset < size) {
        word = iInternalFlashPage_ReadWord(sourcePageAddr + offset);

        if (FLASH_COMPL != FLASH_ProgramWord(targetPageAddr + offset, word)) {
            // whole isn't written
            FLASH_Lock();
            return offset;
        }
        offset += 4;
    }

    FLASH_Lock();
    return size;
}

bool FLASH_ProgramWordTry(uint32_t Address, uint32_t Data, uint8_t tries) {
    for (uint8_t try = 0; try < tries; ++try) {
        if (FLASH_COMPL == FLASH_ProgramWord(Address, Data)) {
            return false;
        }
    }
    return true;
}

size_t bInternalFlashPage_DumpFromRam(
        uint32_t beginPageAddr,
        void *sourceRamAddr,
        uint32_t size
) {
    size_t alignedSize = ((size / FLASH_PAGE_SIZE) + (size % FLASH_PAGE_SIZE ? 1 : 0)) * FLASH_PAGE_SIZE;

    if (!bInternalFlashPage_ClearRange(beginPageAddr, alignedSize)) {
        return 0;
    }

    FLASH_Unlock();
    uint32_t offset = 0;

    uint32_t word;
    while (offset < size) {
        word = *(uint32_t *) (sourceRamAddr + offset);

        if (size < 4) {
            word = word & (0xFFFFFFFF >> (8 * (4 - size)));
        }

        if (FLASH_ProgramWordTry(beginPageAddr + offset, word, 10)) {
            // whole isn't written
            FLASH_Lock();
            return offset;
        }
        offset += 4;
    }

    FLASH_Lock();
    return offset;
}

size_t sInternalFlashPage_Write(
        uint32_t pageAddress,
        size_t offset,
        uint8_t *data,
        size_t size
) {

    FLASH_Unlock();


    size_t left = size;

    while (left) {
        if (left >= 4) {
            if (FLASH_COMPL != FLASH_ProgramWord(pageAddress + offset, *((uint32_t *) data))) {
                // whole isn't written
                FLASH_Lock();
                return size - left;
            }
            data += 4;
            offset += 4;
            left -= 4;
        } else {
            uint32_t word;
            for (uint8_t sub_byte = 0; sub_byte < 4; ++sub_byte) {
                if (sub_byte < left) {
                    ((uint8_t *) &word)[sub_byte] = *data;
                    ++data;
                } else {
                    ++size;
                    ((uint8_t *) &word)[sub_byte] = 0;
                }
            }

            if (FLASH_COMPL != FLASH_ProgramWord(pageAddress + offset, word)) {
                // whole isn't written
                FLASH_Lock();
                return size - left;
            }
            offset += 4;
            left = 0;

        }

    }

    FLASH_Lock();
    return size;
}