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


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 * 4)) = data[i];
    }
    
    // Clear program bit
    FLASH->CR &= ~FLASH_CR_PG;
    write_ptr = addr + (size * 4);  //increase variable storing addr
    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; 
}



uint16_t calc_crc_struct(FLASH_RECORD* res){
        
    uint8_t arr_res[FLASH_RECORD_SIZE - 2];
    uint16_t crc_res;
        /*          sorting data without CRC         */
    arr_res[0] = res->data_id;
    arr_res[1] = res->data_type;

        /*          from 32 to 8  bit                */
    for(int i = 0;i < 4;i++)
        arr_res[i + 2] = (uint8_t)(res->value >> i * 4);

    crc_res = validate_crc16(arr_res,FLASH_RECORD_SIZE - 2);
    return crc_res;
}



/*      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 = calc_crc_struct(&rec);
       
        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, uint32_t val) {
    FLASH_RECORD param_flash;
    // __disable_irq(); // Interrupt off
    param_flash.data_id = param_id;
    param_flash.value = val;
    param_flash.data_type = sizeof(uint8_t);
    param_flash.crc = calc_crc_struct(&param_flash);    
    
     // check alignment
    if (write_ptr % 8 != 0) {
        write_ptr += (8 - (write_ptr % 8));
    }
    
    // check buffer overflow
    if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
        compact_page(); // after compact_page update
        // alignment
        if (write_ptr % 8 != 0) {
            write_ptr += (8 - (write_ptr % 8));
        }
    }
    
    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);

        
        uint16_t calculated_crc = calc_crc_struct(&res);
        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;
}