#include "eeprom.h"

/*
 * Record format (4 bytes):
 *  [0] VirtAddress (uint16_t)
 *  [2] Data        (uint16_t)
 *
 * Page layout:
 *  [0]  PageStatus (uint16_t)
 *  [2..] Records...
 */

extern uint16_t m1pwmap;
extern uint16_t m1pwmch;
extern uint16_t m2pwmap;
extern uint16_t m2pwmch;
extern uint16_t m3pwmap;
extern uint16_t m3pwmch;
extern uint16_t m4pwmap;
extern uint16_t m4pwmch;
extern uint16_t t1ap;
extern uint16_t t2ap;
extern uint16_t t3ap;
extern uint16_t t4ap;
extern uint16_t twap;
extern uint16_t tramp;
extern uint16_t t1ch;
extern uint16_t t2ch;
extern uint16_t t3ch;
extern uint16_t t4ch;
extern uint16_t twch;
extern uint16_t tramp1;
extern uint16_t trampman;
extern uint16_t m1pwmMan;
extern uint16_t m2pwmMan;
extern uint16_t m3pwmMan;
extern uint16_t m4pwmMan;

const uint8_t deftab[EE_NUM_VIRTUAL_ADDR]={
	40,	//m1pwmap
	60, //m1pwmch
	70, //m2pwmap
	70, //m2pwmch
	35, //m3pwmap
	40, //m3pwmch
	40, //m4pwmap
	40,	//m4pwmch
	20, //m1rampstart
	20, //m2rampstart
	20, //m3rampstart
	20, //m4rampstart
	28,	//t1ap
	4,	//t2ap
	10,  //t3ap
	40, //t4ap
	1,  //twap
	27,  //t1ch
	10,  //t2ch
	30,  //t3ch
	1,  //t4ch
	1,  //twch
	0,
	0,
	0,
	100, //tramp1
	60,  //m1pwmMan
	70,  //m2pwmMan
	40,	 //m3pwmMan
	40,	 //m4pwmMan
	50,  //trampman
	10,  //tramp
	40,
	60,
	70,
	70,
	35,
	40,
	40,
	40,
	20,
	20,
	20,
	20,
	28,
	4,
	10,
	40,
	1,
	27,
	10,
	30,
	1,
	1,
	0,
	0,
	0,
	100,
	60,
	70,
	40,
	40,
	50,
	10,
};

typedef struct
{
    uint16_t VirtAddress;
    uint16_t Data;
} EE_Record_t;

/* Active page base address (runtime selected in EE_Init) */
static uint32_t EE_ActivePageBase = EE_PAGE0_BASE;

/* 32 sequential virtual addresses */
const uint16_t EE_VirtAddrs[EE_NUM_VIRTUAL_ADDR] =
{
	    0x0001, 0x0002, 0x0003, 0x0004,
	    0x0005, 0x0006, 0x0007, 0x0008,
	    0x0009, 0x000A, 0x000B, 0x000C,
	    0x000D, 0x000E, 0x000F, 0x0010,
	    0x0011, 0x0012, 0x0013, 0x0014,
	    0x0015, 0x0016, 0x0017, 0x0018,
	    0x0019, 0x001A, 0x001B, 0x001C,
	    0x001D, 0x001E, 0x001F, 0x0020,

	    0x0021, 0x0022, 0x0023, 0x0024,
	    0x0025, 0x0026, 0x0027, 0x0028,
	    0x0029, 0x002A, 0x002B, 0x002C,
	    0x002D, 0x002E, 0x002F, 0x0030,
	    0x0031, 0x0032, 0x0033, 0x0034,
	    0x0035, 0x0036, 0x0037, 0x0038,
	    0x0039, 0x003A, 0x003B, 0x003C,
	    0x003D, 0x003E, 0x003F, 0x0040
};
/* ========================================================================= */

/* --- Internal helpers ---------------------------------------------------- */

static uint16_t EE_GetPageStatus(uint32_t pageBase)
{
    return *(__IO uint16_t *)pageBase;
}

static HAL_StatusTypeDef EE_FlashProgramHalfWord(uint32_t Address, uint16_t Data)
{
    HAL_StatusTypeDef status;

    HAL_FLASH_Unlock();
    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, Address, Data);
    HAL_FLASH_Lock();

    return status;
}

static HAL_StatusTypeDef EE_FlashErasePage(uint32_t PageAddress)
{
    HAL_StatusTypeDef status;
    FLASH_EraseInitTypeDef EraseInit;
    uint32_t PageError = 0;

    HAL_FLASH_Unlock();

    EraseInit.TypeErase   = FLASH_TYPEERASE_PAGES;
    EraseInit.PageAddress = PageAddress;
    EraseInit.NbPages     = 1;

    status = HAL_FLASHEx_Erase(&EraseInit, &PageError);

    HAL_FLASH_Lock();
    return status;
}

/* Find first free record address in given page (returns 0 if full) */
static uint32_t EE_FindFreeAddress(uint32_t pageBase)
{
    uint32_t addr = pageBase + 2U;                /* Skip status word */
    uint32_t pageEnd = pageBase + EE_PAGE_SIZE;

    while (addr < (pageEnd - sizeof(EE_Record_t) + 1U))
    {
        uint16_t vaddr = *(__IO uint16_t *)addr;

        if (vaddr == 0xFFFFU)
        {
            /* Empty slot */
            return addr;
        }
        addr += sizeof(EE_Record_t);
    }

    return 0U; /* No space */
}

/* Find latest value of VirtAddress in a specific page (internal, uses EE_Status) */
/* Find latest value of VirtAddress in a specific page (scan forward) */
static EE_Status EE_FindInPage(uint32_t pageBase, uint16_t VirtAddress, uint16_t *Data)
{
    uint32_t addr     = pageBase + 2U;            // skip status halfword
    uint32_t pageEnd  = pageBase + EE_PAGE_SIZE;
    EE_Status result  = EE_NOT_FOUND;
    uint16_t lastVal  = 0;

    if (Data == NULL)
        return EE_ERROR;

    while (addr <= (pageEnd - sizeof(EE_Record_t)))
    {
        uint16_t vaddr = *(__IO uint16_t *)addr;

        if (vaddr == 0xFFFFU)
        {
            // First empty slot => no more records in this page
            break;
        }

        uint16_t value = *(__IO uint16_t *)(addr + 2U);

        if (vaddr == VirtAddress)
        {
            lastVal = value;    // keep most recent
            result  = EE_OK;
        }

        addr += sizeof(EE_Record_t);  // move 4 bytes forward
    }

    if (result == EE_OK)
        *Data = lastVal;

    return result;
}



/* Format both pages: erase and set PAGE0 as VALID */
static EE_Status EE_Format(void)
{
    if (EE_FlashErasePage(EE_PAGE0_BASE) != HAL_OK)
        return EE_STATUS_ERROR;

    if (EE_FlashErasePage(EE_PAGE1_BASE) != HAL_OK)
        return EE_STATUS_ERROR;

    if (EE_FlashProgramHalfWord(EE_PAGE0_BASE, EE_PAGE_STATUS_VALID) != HAL_OK)
        return EE_STATUS_ERROR;

    /* PAGE1 will remain erased (status = 0xFFFF) */
    EE_ActivePageBase = EE_PAGE0_BASE;

    return EE_STATUS_OK;
}

/* Get the base of the other page */
static uint32_t EE_GetOtherPageBase(uint32_t pageBase)
{
    return (pageBase == EE_PAGE0_BASE) ? EE_PAGE1_BASE : EE_PAGE0_BASE;
}

/* Page transfer (garbage collection + new write) */
static EE_Status EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
{
    uint32_t oldBase = EE_ActivePageBase;
    uint32_t newBase = EE_GetOtherPageBase(oldBase);
    uint32_t addr;
    uint16_t value;
    EE_Status st;

    /* Erase new page */
    if (EE_FlashErasePage(newBase) != HAL_OK)
        return EE_STATUS_ERROR;

    /* Mark new page as RECEIVE */
    if (EE_FlashProgramHalfWord(newBase, EE_PAGE_STATUS_RECEIVE) != HAL_OK)
        return EE_STATUS_ERROR;

    /* Start writing records just after status */
    addr = newBase + 2U;

    /* For each known virtual variable */
    for (uint16_t i = 0; i < EE_NUM_VIRTUAL_ADDR; i++)
    {
        uint16_t vaddr = EE_VirtAddrs[i];

        if (vaddr == VirtAddress)
        {
            /* Use the new data passed into PageTransfer */
            value = Data;
        }
        else
        {
            /* Read latest value from old active page */
            st = EE_FindInPage(oldBase, vaddr, &value);
            if (st != EE_STATUS_OK)
            {
                /* Variable never written -> skip */
                continue;
            }
        }

        /* Write record to new page */
        if (EE_FlashProgramHalfWord(addr, vaddr) != HAL_OK)
            return EE_STATUS_ERROR;
        if (EE_FlashProgramHalfWord(addr + 2U, value) != HAL_OK)
            return EE_STATUS_ERROR;

        addr += sizeof(EE_Record_t);
        if (addr >= (newBase + EE_PAGE_SIZE))
            return EE_STATUS_NO_SPACE;
    }

    /* Erase old page */
    if (EE_FlashErasePage(oldBase) != HAL_OK)
        return EE_STATUS_ERROR;

    /* Mark new page as VALID */
    if (EE_FlashProgramHalfWord(newBase, EE_PAGE_STATUS_VALID) != HAL_OK)
        return EE_STATUS_ERROR;

    /* Update active page */
    EE_ActivePageBase = newBase;

    return EE_STATUS_OK;
}

/* --- Public API ----------------------------------------------------------- */

/*
 * Initialize the EEPROM emulation.
 * - Checks page statuses and chooses the active page.
 * - If inconsistent or blank, formats pages.
 * PUBLIC RETURN TYPE: uint16_t (EE_OK / EE_ERROR / ...)
 */
uint16_t EE_Init(void)
{
    uint16_t status0 = EE_GetPageStatus(EE_PAGE0_BASE);
    uint16_t status1 = EE_GetPageStatus(EE_PAGE1_BASE);

    if ((status0 == EE_PAGE_STATUS_ERASED) && (status1 == EE_PAGE_STATUS_ERASED))
    {
        /* Fresh device -> format */
        return (uint16_t)EE_Format();
    }
    else if ((status0 == EE_PAGE_STATUS_VALID) && (status1 == EE_PAGE_STATUS_ERASED))
    {
        EE_ActivePageBase = EE_PAGE0_BASE;
        return EE_OK;
    }
    else if ((status1 == EE_PAGE_STATUS_VALID) && (status0 == EE_PAGE_STATUS_ERASED))
    {
        EE_ActivePageBase = EE_PAGE1_BASE;
        return EE_OK;
    }
    else
    {
        /* Any weird or inconsistent state -> reformat */
        return (uint16_t)EE_Format();
    }
}

/*
 * Read a 16-bit variable by its virtual address.
 * PUBLIC RETURN: EE_OK / EE_NOT_FOUND / EE_ERROR (as uint16_t)
 */
uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t *Data)
{
    EE_Status st;

    if (Data == NULL)
        return EE_ERROR;

    st = EE_FindInPage(EE_ActivePageBase, VirtAddress, Data);
    return (uint16_t)st;
}

/*
 * Write (append) a 16-bit variable.
 * - Writes a new record in the active page.
 * - If the page is full, triggers a page transfer (GC).
 * PUBLIC RETURN: EE_OK / EE_ERROR / EE_NO_SPACE (as uint16_t)
 */
uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data)
{
    uint32_t freeAddr;
    EE_Status st;

    /* Find free space in active page */
    freeAddr = EE_FindFreeAddress(EE_ActivePageBase);
    if (freeAddr != 0U)
    {
        /* Write new record */
        if (EE_FlashProgramHalfWord(freeAddr, VirtAddress) != HAL_OK)
            return EE_ERROR;
        if (EE_FlashProgramHalfWord(freeAddr + 2U, Data) != HAL_OK)
            return EE_ERROR;

        return EE_OK;
    }

    /* No space -> page transfer */
    st = EE_PageTransfer(VirtAddress, Data);
    return (uint16_t)st;
}

void loadEE(void){
	EEW_Read(M1PWMAP, &m1pwmap);
	EEW_Read(M1PWMCH, &m1pwmch);
	EEW_Read(M2PWMAP, &m2pwmap);
	EEW_Read(M2PWMCH, &m2pwmch);
	EEW_Read(M3PWMAP, &m3pwmap);
	EEW_Read(M3PWMCH, &m3pwmch);
	EEW_Read(M4PWMAP, &m4pwmap);
	EEW_Read(M4PWMCH, &m4pwmch);
	EEW_Read(T1AP, &t1ap);
	EEW_Read(T2AP, &t2ap);
	EEW_Read(T3AP, &t3ap);
	EEW_Read(T4AP, &t4ap);
	EEW_Read(TWAP, &twap);
	EEW_Read(T1CH, &t1ch);
	EEW_Read(T2CH, &t2ch);
	EEW_Read(T3CH, &t3ch);
	EEW_Read(T4CH, &t4ch);
	EEW_Read(TWCH, &twch);
	EEW_Read(TRAMP1, &tramp1);
	EEW_Read(M1PWMMAN, &m1pwmMan);
	EEW_Read(M2PWMMAN, &m2pwmMan);
	EEW_Read(M3PWMMAN, &m3pwmMan);
	EEW_Read(M4PWMMAN, &m4pwmMan);
	EEW_Read(TRAMPMAN, &trampman);
	EEW_Read(TRAMP, &tramp);
}