#include "flash.h"
#include "main.h"
#include <stdint.h>

#define FLASH_BASE_ADDR     0x08000000UL
// #define FLASH_PAGE_SIZE     0x800       // 2KB
#define FLASH_PAGE_NUM      128
#define FLASH_USER_PAGE     (FLASH_PAGE_NUM - 1)
#define FLASH_USER_ADDR     (FLASH_BASE_ADDR + FLASH_USER_PAGE * FLASH_PAGE_SIZE)
#define FLASH_EMPTY_DWORD   0xFFFFFFFFFFFFFFFFULL

static uint16_t Flash_CalcCrc(const FlashConfig_t *cfg)
{
    uint16_t crc = 0xA55A;
    crc ^= cfg->can_id;
    crc ^= ((uint16_t)cfg->sensorDir << 8) | cfg->debugFlag;
    return crc;
}

static void Flash_LoadDefaultConfig(FlashConfig_t *cfg)
{
    cfg->can_id = 0x01;
    cfg->sensorDir = 0;
    cfg->debugFlag = 0;
    cfg->crc = Flash_CalcCrc(cfg);
}

static uint64_t Flash_PackConfig(const FlashConfig_t *cfg)
{
    uint64_t data = 0;

    data |= (uint64_t)(cfg->can_id & 0xFFFFU);
    data |= ((uint64_t)cfg->sensorDir << 16);
    data |= ((uint64_t)cfg->debugFlag << 24);
    data |= ((uint64_t)cfg->crc << 32);

    return data;
}

static void Flash_UnpackConfig(uint64_t data, FlashConfig_t *cfg)
{
    cfg->can_id = (uint16_t)(data & 0xFFFFU);
    cfg->sensorDir = (uint8_t)((data >> 16) & 0xFFU);
    cfg->debugFlag = (uint8_t)((data >> 24) & 0xFFU);
    cfg->crc = (uint16_t)((data >> 32) & 0xFFFFU);
}

FlashConfig_t currentCfg;
FlashConfig_t *Flash_ReadConfig(void)
{
    uint64_t rawData = *(volatile uint64_t *)FLASH_USER_ADDR;

    if (rawData == FLASH_EMPTY_DWORD)
    {
        Flash_LoadDefaultConfig(&currentCfg);
        return &currentCfg;
    }

    Flash_UnpackConfig(rawData, &currentCfg);

    if ((currentCfg.can_id > 0x7FFU) || (currentCfg.crc != Flash_CalcCrc(&currentCfg)))
    {
        Flash_LoadDefaultConfig(&currentCfg);
    }

    return &currentCfg;
}

void Flash_WriteConfig(FlashConfig_t *cfg)
{
    uint64_t data;

    cfg->crc = Flash_CalcCrc(cfg);
    data = Flash_PackConfig(cfg);

    HAL_FLASH_Unlock();

    FLASH_EraseInitTypeDef erase;
    uint32_t page_error;

    erase.TypeErase = FLASH_TYPEERASE_PAGES;
    erase.Page      = FLASH_USER_PAGE;
    erase.NbPages   = 1;

    HAL_FLASHEx_Erase(&erase, &page_error);

    uint32_t addr = FLASH_USER_ADDR;
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, addr, data);

    HAL_FLASH_Lock();
}