#include "stm32f0xx_hal.h"
#include "ssd1306.h"
#include "stm32f0xx_hal_flash.h"
#include "flash.h"

void flash_init(therm_settings_t* tosave)
{
    uint16_t size = sizeof(therm_settings_t)-1;
    uint32_t flash_adr = END_ADDR - size;
    flash_adr -= 2;
    uint8_t* flash_ptr = (uint8_t *)flash_adr;

    // Check if flash is blank
    uint16_t i = 0;
    uint16_t count = 0;

    char tempstr[10];
    itoa(flash_adr, tempstr, 10);
    ssd1306_DrawString(tempstr, 1, 0);

    uint16_t test;
    for(i=0;i<size;i++)
    {
        test = *flash_ptr;
        if(test==0xFF) count++;
    }

    ssd1306_DrawString("END LOOP ", 0, 0);

    // If blank, do nothing and just use values from RAM

    // If not blank, check the checksums
    if(count != size) 
    {
        ssd1306_DrawString("FLASH NOT BLANK", 1, 0);
        // Calculate Checksums
    uint8_t cksum0=0,cksum1=0;
    uint8_t rdSum0=0,rdSum1=0;
        flash_adr = END_ADDR - size;
        flash_ptr = (uint8_t *)flash_adr;
        for(i=1; i < size; i++)
        {
            cksum0 += *flash_ptr++;
            cksum1 += cksum0;
        }

        // Read flash checksums
        flash_adr -= 2;
        flash_ptr = (uint8_t *)flash_adr;
        rdSum0 = *flash_ptr++;
        rdSum1 = *flash_ptr;

        // Compare Checksums values
        if((rdSum1==cksum1)&&(rdSum0==cksum0)) {
	    ssd1306_DrawString("CHECKSUM OK", 2, 0);
	    flash_read(tosave);
	}
        else {
	    ssd1306_DrawString("CHECKSUM BAD", 2, 0);
	    return; // If the checksum is bad, just use vals from RAM
	}

    }
    else {
        ssd1306_DrawString("FLASH BLANK", 1, 0);
    }
}


void flash_save(therm_settings_t* tosave)
{

	    ssd1306_DrawString("BEGIN SAVE", 2, 0);
            HAL_Delay(1500);
        flash_erase(tosave);
        flash_write(tosave);
        flash_checksum(tosave);
	    ssd1306_DrawString("END SAVE", 2, 0);
            HAL_Delay(1500);
}


void flash_read(therm_settings_t *tosave)
{
    ssd1306_DrawString("READING SAVE", 1, 0);
    char tempstr[10];
    itoa(sizeof(therm_settings_t), tempstr, 10);
    ssd1306_DrawString(tempstr, 2, 0);

    uint16_t size = sizeof(therm_settings_t)-1; // in Bytes
    uint32_t flash_adr = END_ADDR - size;
    uint8_t *flash_ptr = (uint8_t *)flash_adr;
    uint8_t *struct_ptr = (uint8_t*)tosave;

    uint16_t i;
    for(i=0;i<size;i++)
        *struct_ptr++ = *flash_ptr++;
    ssd1306_DrawString("READ COMPLETE", 3, 0);

}


void flash_write(therm_settings_t* tosave)
{

    HAL_FLASH_Unlock();

    uint16_t size = sizeof(therm_settings_t)-1; // in Bytes
    uint32_t start_address = END_ADDR-size; // write to end of page
    uint32_t struct_ptr = (uint32_t*) tosave;

    uint16_t  length;
    if(size%2==0)
        length = size/2;
    else
        length = size/2+1;

    uint16_t i;
    for(i=0;i<length;i++)
    {
        HAL_FLASH_Program(TYPEPROGRAM_HALFWORD, start_address, struct_ptr);
        struct_ptr++;
        start_address +=2;
    }

    HAL_FLASH_Lock();
}


void flash_checksum(therm_settings_t* tosave)
{
    uint8_t cksum0=0,cksum1=0;
    uint16_t  i,size,checksum;
    uint32_t flash_adr;
    uint8_t  *flash_ptr;

    HAL_FLASH_Unlock();

    size = sizeof(*tosave)-1; // in Bytes
    flash_adr = END_ADDR-size;
    flash_ptr = (uint8_t *)flash_adr;

    for(i=1; i < size; i++)
    {
        cksum0 += *flash_ptr++;
        cksum1 += cksum0;
    }
    checksum = (cksum1<<8) | cksum0;
    flash_adr -= 2;
    HAL_FLASH_Program(TYPEPROGRAM_HALFWORD, flash_adr, checksum);

    HAL_FLASH_Lock();
}


void flash_erase(therm_settings_t* tosave)
{
    uint8_t FLASHStatus = 1; // FLASH_COMPLETE=1
    uint32_t end_addr = END_ADDR;
    uint32_t NbrOfPage = abs( (sizeof(*tosave)-1)/0x400 )+1;   // Number of pages to be erased, most definitely 1 but hey, we might as well try to calculate it.
    uint32_t StartAddr = (end_addr+1) - (0x400*NbrOfPage);   // Starting address to be erased

    HAL_FLASH_Unlock();

    // Clear All pending flags
    //FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);

    // Erase the FLASH pages
    FLASH_EraseInitTypeDef erase;
    erase.TypeErase = TYPEERASE_PAGES; 
    erase.PageAddress = StartAddr;
    erase.NbPages = NbrOfPage;
    uint32_t SectorError = 0;
    FLASHStatus = HAL_FLASHEx_Erase(&erase, &SectorError);

    HAL_FLASH_Lock();
}

// vim:softtabstop=4 shiftwidth=4 expandtab