#include "stm32f407xx.h"
#include "stm32f4xx_hal_flash.h"
#include "stm32f4xx_hal_flash_ex.h"
#include <stdint.h>
#include <string.h>
static uint32_t flash_get_sector(uint32_t address) {
    address -= FLASH_BASE;
    if (address < 0x4000) return FLASH_SECTOR_0;
    else if (address < 0x8000) return FLASH_SECTOR_1;
    else if (address < 0xC000) return FLASH_SECTOR_2;
    else if (address < 0x10000) return FLASH_SECTOR_3;
    else if (address < 0x20000) return FLASH_SECTOR_4;
    else if (address < 0x40000) return FLASH_SECTOR_5;
    else if (address < 0x60000) return FLASH_SECTOR_6;
    else if (address < 0x80000) return FLASH_SECTOR_7;
    else if (address < 0xA0000) return FLASH_SECTOR_8;
    else if (address < 0xC0000) return FLASH_SECTOR_9;
    else if (address < 0xE0000) return FLASH_SECTOR_10;
    else return FLASH_SECTOR_11;
}

int flash_port_erase(uint32_t address, uint32_t length) {
    HAL_FLASH_Unlock();
    uint32_t start_sector = flash_get_sector(address);
    uint32_t end_sector = flash_get_sector(address + length - 1);

    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
    FLASH_EraseInitTypeDef erase;
    erase.TypeErase = FLASH_TYPEERASE_SECTORS;
    erase.Sector = start_sector;
    erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
    erase.NbSectors = end_sector - start_sector + 1;
    uint32_t sector_error = 0;
    if (HAL_FLASHEx_Erase(&erase, &sector_error) != HAL_OK) {
        HAL_FLASH_Lock();
        return -1;
    }
    HAL_FLASH_Lock();
    return 0;
}

int flash_port_write(uint32_t address, const uint8_t *data, uint32_t length) {
    HAL_FLASH_Unlock();
    uint32_t written = 0;
    while (written < length) {

        uint32_t chunk_data;
        if (length - written >= 4) {
            chunk_data = *(uint32_t *)(data + written);
        } else {
            word_data = 0xFFFFFFFF;
            for (uint32_t i = 0; i < length - written; i++) {
                ((uint8_t *)&chunk_data)[i] = data[written + i];
            }
        }
        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address + written, chunk_data) != HAL_OK) {
            HAL_FLASH_Lock();
            return -1;
        }
        written += 4;
    }
    HAL_FLASH_Lock();
    return 0;
}

int flash_port_read(uint32_t address, uint8_t *buffer, uint32_t length) {
    memcpy(buffer, (const void *)address, length);
    return 0;
}

bool flash_port_verify(uint32_t address, const uint8_t *data, uint32_t length) {
    uint8_t read_data[length];
    flash_port_read(address, read_data, length);
    return memcmp(read_data, data, length) == 0;
}

int flash_port_init(void) {
    return 0;
}

static const bsp_flash_ops_t flash_ops = {
    .init = flash_port_init,
    .erase = flash_port_erase,
    .write = flash_port_write,
    .read = flash_port_read,
    .verify = flash_port_verify,
};

void flash_port_register(void) {
    bsp_flash_register(&flash_ops);
}