#include "flash_operation.h"

void Flash_InitInternal(void)
{
    // 解锁Flash
    HAL_FLASH_Unlock();

    // 设置Flash延迟
    __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_4);

    // 清除所有错误标志
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
}

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
    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;
    }

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

            // 组合4个int16_t到一个uint64_t
            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;
            }
        }
    }

    // 锁定Flash
    HAL_FLASH_Lock();

    // 恢复中断状态
    if (!primask_bit) {
        __enable_irq();
    }

    return status;
}

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

    // 内部初始化Flash（主要是解锁和配置）
    Flash_InitInternal();

    // 从Flash读取数据
    for (int i = 0; i < PARAM_COUNT; i++)
    {
        // 读取int16_t数据
        parameters[i] = *(int16_t*)(flash_address + (i * sizeof(int16_t)));
    }

    // 锁定Flash（虽然读取不需要锁定，但为了对称性）
    HAL_FLASH_Lock();
}