#include "flash_operation.h"

static void Flash_InitInternal(void)
{
    HAL_FLASH_Unlock();
    __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_4);
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
}

static uint32_t GetFlashPage(uint32_t address)
{
    return (address - FLASH_BASE_ADDRESS) / FLASH_PAGE_SIZE;
}

HAL_StatusTypeDef WriteParametersToFlash(int16_t *parameters)
{
    HAL_StatusTypeDef status = HAL_OK;
    uint32_t flash_address = DATA_FLASH_ADDRESS;
    uint32_t primask_bit;

    primask_bit = __get_PRIMASK();
    __disable_irq();

    Flash_InitInternal();

    /* 擦除Flash页 */
    FLASH_EraseInitTypeDef erase_init;
    uint32_t page_error = 0;

    erase_init.TypeErase = FLASH_TYPEERASE_PAGES;
    erase_init.Banks = FLASH_BANK_1;
    erase_init.Page = GetFlashPage(DATA_FLASH_ADDRESS);
    erase_init.NbPages = 1;

    if (HAL_FLASHEx_Erase(&erase_init, &page_error) != HAL_OK)
    {
        HAL_FLASH_Lock();
        if (!primask_bit) __enable_irq();
        return HAL_ERROR;
    }

    /* 每次写入64位数据（4个int16_t）*/
    for (int i = 0; i < PARAM_COUNT; i++)
    {
        if (i % 4 == 0)
        {
            uint64_t combined_data = 0;

            for (int j = 0; j < 4 && (i + j) < PARAM_COUNT; j++)
            {
                combined_data |= ((uint64_t)((uint16_t)parameters[i + j]) << (j * 16));
            }

            if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD,
                                  flash_address + (i * sizeof(int16_t)),
                                  combined_data) != HAL_OK)
            {
                status = HAL_ERROR;
                break;
            }
        }
    }

    HAL_FLASH_Lock();

    if (!primask_bit) __enable_irq();

    return status;
}

void ReadParametersFromFlash(int16_t *parameters)
{
    uint32_t flash_address = DATA_FLASH_ADDRESS;

    Flash_InitInternal();

    for (int i = 0; i < PARAM_COUNT; i++)
    {
        parameters[i] = *(int16_t *)(flash_address + (i * sizeof(int16_t)));
    }

    HAL_FLASH_Lock();
}