#include "flash.h"
#include <stdbool.h>
#include "hal_conf_extra.h"

static uint32_t write_ptr = SECTOR_6;

void flash_unlock(){

    // Check if flash is locked
    if(!(FLASH->CR & FLASH_CR_LOCK)) {
        return; // Already unlocked
    }

    // Write flash key sequence to unlock
    FLASH->KEYR = 0x45670123;  // First key
    FLASH->KEYR = 0xCDEF89AB;  // Second key

}

void flash_lock() {
    if(FLASH->CR & FLASH_CR_LOCK) {
        return; // Already locked
    }
    FLASH->CR |= FLASH_CR_LOCK; // Lock flash memory
}

void erase_sector(uint8_t sector){
    
    // Wait if flash is busy
    while(FLASH_BUSY);

    // Check if flash is locked and unlock if needed
    if(FLASH->CR & FLASH_CR_LOCK) {
        flash_unlock();
    }

    // Set sector erase bit and sector number
    FLASH->CR |= FLASH_CR_SER;
    FLASH->CR &= ~FLASH_CR_SNB;
    FLASH->CR |= (sector << FLASH_CR_SNB_Pos) & FLASH_CR_SNB_Msk;

    // Start erase
    FLASH->CR |= FLASH_CR_STRT;

    // Wait for erase to complete
    while(FLASH_BUSY);
    
    // Clear sector erase bit
    FLASH->CR &= ~FLASH_CR_SER;

}

void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){

    // Wait if flash is busy
    while(FLASH_BUSY);
    // Check if flash is locked and unlock if needed
    if(FLASH->CR & FLASH_CR_LOCK) {
        flash_unlock();
    }

    // Set program bit 32bit programm size and  Write data to address
    if(byte_len == 1) {
        FLASH_8BYTE;
        FLASH->CR |= FLASH_CR_PG;
        *(volatile uint8_t*)address = (uint8_t)data;
    } else {
        FLASH_32BYTE;
        FLASH->CR |= FLASH_CR_PG;
        *(volatile uint32_t*)address = data;
    }

    // Wait for programming to complete
    while(FLASH_BUSY);

    // Clear program bit
    FLASH->CR &= ~FLASH_CR_PG;

}
void flash_write(uint32_t addr, FLASH_RECORD* record){

    uint32_t* data = (uint32_t*)record;
    uint32_t size = FLASH_RECORD_SIZE / 4;    //count words in struct
    // Wait if flash is busy
    while(FLASH_BUSY);

    // Check if flash is locked and unlock if needed
    if(FLASH->CR & FLASH_CR_LOCK) {
        flash_unlock();
    }

    // Set program bit and write data to flash
    FLASH_32BYTE;
    FLASH->CR |= FLASH_CR_PG;
    
    for(int i = 0;i < size;i++){
        *(volatile uint32_t*)(addr + i) = data[i];
        write_ptr++;
    }
    
    // Clear program bit
    FLASH->CR &= ~FLASH_CR_PG;
    flash_lock();
}

uint8_t flash_read_word(uint32_t address){
   
    // Check if address is valid
    if(address < FLASH_BASE || address > FLASH_END) {
        return 0;
    }

    // Read byte from flash memory
    return *((volatile uint8_t*)address);

}
    // Wait if flash
bool validata_crc(FLASH_RECORD* crc){
    return crc->crc == 0x6933? true : false;
}

uint16_t validate_crc16(uint8_t *data, uint32_t length) {
    uint16_t crc = 0xFFFF; // start value for CRC MODBUS
    while (length--) {
        crc ^= *data++; // XOR 
        for (uint8_t i = 0; i < 8; i++) {
            if (crc & 0x0001) { 
                crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse)
            } else {
                crc >>= 1;
            }
        }
    }
    return crc; 
}



/*      read struct from FLASH      */
void flash_read(uint32_t addr,FLASH_RECORD* ptr){
    uint8_t* flash_ptr = (uint8_t*)addr;
    uint8_t* dest = (uint8_t*)ptr;
    for(int i = 0;i < FLASH_RECORD_SIZE;i++)
        dest[i] = flash_ptr[i]; 
}

void compact_page(){
    FLASH_RECORD latest[PARAM_COUNT] = {0};
    for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) {
        FLASH_RECORD rec;
        flash_read(i,&rec);
        uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
       
        if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) {
            // if the crc does not match, we check further
            latest[rec.data_id] = rec;
        }
        else 
        // if 
            continue; 
    }

    erase_sector(6);
    write_ptr = SECTOR_6; // Сброс на начало
    for (int i = 0; i < PARAM_COUNT; i++) {
        if (latest[i].data_id != 0xFF) {
            // alignment
            if (write_ptr % 4 != 0) {
                write_ptr += (4 - (write_ptr % 4));
            }
            flash_write(write_ptr, &latest[i]);

        }
    }
}


void write_param(uint8_t param_id, uint8_t val) {
    FLASH_RECORD param_flash = {param_id, val};
    // __disable_irq(); // Interrupt off

    param_flash.crc = validate_crc16((uint8_t*)&param_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти
    
    // check alignment
    if (write_ptr % 4 != 0) {
        write_ptr += (4 - (write_ptr % 4));
    }
    
    // check buffer overflow
    if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
        compact_page(); // after compact_page update
        // alignment
        if (write_ptr % 4 != 0) {
            write_ptr += (4 - (write_ptr % 4));
        }
    }
    
    flash_write(write_ptr, &param_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure
    
    // __enable_irq(); // Interrupt on
}

FLASH_RECORD* load_params(){
    __disable_irq();
    static FLASH_RECORD latest[PARAM_COUNT] = {0};
    FLASH_RECORD res;
    for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
        flash_read(addr,&res);
        /*          check  CRC          */
        uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
        if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) 
            continue;
        else{
            latest[res.data_id] = res;
    }
    write_ptr = addr + FLASH_RECORD_SIZE;
}

    __enable_irq();
    return latest;
}