#include <stdio.h>
#include <math.h>
#include <stm32l432xx.h>
#include "ee14lib.h"

#define FLASH_BASE_ADDRESS 0x08000000

// This function is called by printf() to handle the text string
// We want it to be sent over the serial terminal, so we just delegate to that function
int _write(int file, char *data, int len) {
    serial_write(USART2, data, len);
    return len;
}

void print_flash_page(void* page_base)
{
    uint8_t* p = (uint8_t*)page_base;

    // Pages are 2kB each
    for(unsigned int i = 0; i < 128; i++){
        if(i % 16 == 0){
            printf("\n%08x: ", p);
        }
        printf("%02x ", *p);
        p++; // Next *byte*
    }
    printf("\n");
}

void erase_flash_page(void* page_base)
{
    // Wait until flash is ready
    while(FLASH->SR & FLASH_SR_BSY){}

    // TODO: check any previous error flags
    uint32_t page_num = ((uint32_t)page_base >> 11) & 0x7F; // 2048-byte pages, 128 pages total on L432KC

    // Set the page number and control bit to erase the page
    FLASH->CR |= (page_num << FLASH_CR_PNB_Pos) | FLASH_CR_PER;

    // Perform the erase
    FLASH->CR |= FLASH_CR_STRT;

    // Wait until the operation completes
    while(FLASH->SR & FLASH_SR_BSY){}

    // Unset the erase bit
    FLASH->CR &= ~FLASH_CR_PER;
}

void write_flash_dword(uint32_t* dword_base, uint32_t words[2])
{
    // If the address isn't 8-byte aligned, don't even try
    if((uint32_t)dword_base & 0x7){
        return;
    }

    // Wait until flash is ready
    while(FLASH->SR & FLASH_SR_BSY){}

    // Set the control bit to perform a write
    FLASH->CR |= FLASH_CR_PG;

    while(!(FLASH->CR & FLASH_CR_PG)){}

    // Write the data like any other memory
    dword_base[0] = words[0];
    dword_base[1] = words[1];

    // Wait until the operation completes
    while(FLASH->SR & FLASH_SR_BSY){}

    // Check that the write succeeded
    if(FLASH->SR & FLASH_SR_EOP){
        FLASH->SR = FLASH_SR_EOP; // Clear the flag
    }
    else{
        // Crash and burn
    }

    // Unset the control bit to stop writing
    FLASH->CR &= ~FLASH_CR_PG;
}

int main()
{
    host_serial_init(9600);

    // Enable the Flash controller
    RCC->AHB1ENR |= RCC_AHB1ENR_FLASHEN;

    // Unlock the Flash for modification
    FLASH->KEYR = 0x45670123;
    FLASH->KEYR = 0xCDEF89AB;

    // Based on current code size (~5400 bytes)
    // 0x 800 = 2048
    // 0x1000 = 4096
    // 0x2000 = 8192

    uint32_t page_address = FLASH_BASE_ADDRESS + 0x2000;

    printf("Initial value of flash memory:\n");
    print_flash_page((void*)page_address);

    // Erase a page
    erase_flash_page((void*)page_address);
    printf("Page after being erased:\n");
    print_flash_page((void*)page_address);

    // Write some data to the page
    uint32_t data[] = {0xDEADBEEF, 0xF00DFACE};
    write_flash_dword((uint32_t*)page_address, data);
    printf("Page after writing:\n");
    print_flash_page((void*)page_address);

    printf("All done!\n");
    while(1){ }

    return 0;
}