#define EEPROM_SIZE 512
#define EEPROM_ENTRY_SIZE (sizeof(settings_t) + sizeof(state_t))
#define EEPROM_ENTRIES (EEPROM_SIZE / EEPROM_ENTRY_SIZE)

typedef struct {
    uint32_t seq;
    // state_t goes here in actual EEPROM
    uint16_t crc;
} settings_t;

void load_settings() {
    uint8_t *ptr = 0; // byte pointer in EEPROM
    uint32_t cur_seq = 0;
    state_t state_buf;
   
    cli();

    while (queue_num < EEPROM_ENTRIES) {
        settings.seq = eeprom_read_dword((void *) ptr);
        if (settings.seq <= cur_seq || settings.seq == 0xFFFFFFFF) {
            settings.seq = cur_seq;
            break;
        }
        cur_seq = settings.seq;
        ptr += sizeof (settings.seq);
        eeprom_read_block(&state_buf, (void *) ptr, sizeof (state_buf));
        ptr += sizeof (state_buf);
        settings.crc = eeprom_read_word((void *) ptr);
        ptr += sizeof (settings.crc);

        if (settings.crc != crc16((void *) &state_buf, sizeof (state_buf))) {
            LOG("CRC incorrect\r\n");
            break;
        }
        // Since we are here, settings are valid, CRC are good
        state = state_buf;
        queue_num++;
    }
    sei();
}

void save_settings(eSaveMode savemode) {
    cli();

    settings.crc = crc16((void *) &state, sizeof (state)); // Calculate crc16 of state struct

    // Save data to EEPROM
    settings.seq++;
    if (queue_num >= EEPROM_ENTRIES) // Reset queue_num once we reach end of EEPROM
        queue_num = 0;
    uint8_t *ptr = (void *)(queue_num * EEPROM_ENTRY_SIZE); // Get location of next segment in EEPROM
   
   
    eeprom_write_dword((void *) ptr, settings.seq);
    ptr += sizeof (settings.seq);
    eeprom_update_block((void *)&state, (void *) ptr, sizeof (state));
    ptr += sizeof (state);
    eeprom_write_word((void *) ptr, settings.crc);

    sei();
}