#include "storage_interface.h"
#include <Arduino.h>
#include <EEPROM.h>
#include "debug_config.h"


// Manual implements of EEPROM.put() for few uint type

uint16_t inline read_eeprom_uint16(uint8_t idx){
    uint8_t tmp;
    tmp = EEPROM.read(idx); // read high byte
    return EEPROM.read(idx + 1) | (tmp << 8); // read low byte
}

void inline write_eeprom_uint16(uint8_t idx, uint16_t value){
    EEPROM.write(idx, value >> 8); // write high byte
    EEPROM.write(idx + 1, value & 0xFF); // write low byte
}

uint32_t inline read_eeprom_uint32(uint8_t idx) {
    uint32_t result = 0;
    
    // Byte 1 (High level : MSB)
    result |= (uint32_t)EEPROM.read(idx) << 24; 
    
    // Byte 2
    result |= (uint32_t)EEPROM.read(idx + 1) << 16;
    
    // Byte 3
    result |= (uint32_t)EEPROM.read(idx + 2) << 8;
    
    // Byte 4 (Low level : LSB)
    result |= (uint32_t)EEPROM.read(idx + 3); 
    
    return result;
}

void inline write_eeprom_uint32(uint8_t idx, uint32_t value) {
    
    // Byte 1 (High level : MSB)
    EEPROM.write(idx, (uint8_t)(value >> 24)); 
    
    // Byte 2
    EEPROM.write(idx + 1, (uint8_t)(value >> 16));
    
    // Byte 3
    EEPROM.write(idx + 2, (uint8_t)(value >> 8));
    
    // Byte 4 (Low level : LSB)
    EEPROM.write(idx + 3, (uint8_t)(value & 0xFF)); 
}

Storage_interface::Storage_interface(){}

Storage_interface::~Storage_interface(){}

void Storage_interface::clear_eeprom(){
    for (uint16_t i = 0 ; i < EEPROM.length() ; i++) {
        EEPROM.write(i, 0);
    }
}

void Storage_interface::clear_eeprom_at(uint16_t idx){
    for (uint16_t i = idx ; i < EEPROM.length() ; i++) {
        EEPROM.write(i, 0);
    }
}

/**
* @brief get_last_header_nbpackage
* * 
* 
* * @param last_header_idx @c uint16_t EEPROM index of the last header
* @return @c uint16_t EEPROM stored package number
*/
uint16_t Storage_interface::get_last_header_nbpackage(uint16_t* last_header_idx){
    uint8_t flags;
    uint16_t start_package = 0, nb_package = 0;
    *last_header_idx = 0;
    flags = EEPROM.read(start_package);

    while ((flags & 0b1) != 0){
        nb_package++;
        *last_header_idx = start_package;
        start_package = read_eeprom_uint16(start_package + OFFSET_NEXT_PACKAGE);
        flags = EEPROM.read(start_package);
        nb_package++;
    }
    return nb_package;
}

// factorise reused code implementation and keep level acces
uint16_t Storage_interface::get_nb_package(){
    uint16_t pointer;
    return get_last_header_nbpackage(&pointer);
}


void Storage_interface::get_struct(uint16_t offset, bool* timestamp, bool* is_final_set, bool* photo_sensor, bool* temp_sensor, uint8_t* timestamp_schedule, uint8_t* nb_photo_sensor, uint8_t* nb_temp_sensor, uint8_t* photo_size, uint8_t* temp_size, uint16_t* p_next_package, uint16_t* nb_measures){
    uint8_t flags;
    flags = EEPROM.read(offset);

    // Checking there if a struct is possibly at the index 
    if ((flags & 0b1) == 0){
        DLOGLN("Missing struct index or bad index")
        //while(true);
    }

    // timestamps reads
    *timestamp = false;
    if ((flags & 0b001) != 0 ){
        *timestamp = true;
        *timestamp_schedule = EEPROM.read(offset + OFFSET_MEASURES_SCH);
    }
    *timestamp_schedule = 0;
    if (flags & 0b01){
        *timestamp = true;
        WARN_IF(EEPROM.read(offset + OFFSET_MEASURES_SCH) != 0, "Incoherent timestamp parameter in the header.")
    }

    // photo res reads
    *photo_sensor = false;
    if (flags & 0b0001){
        *photo_sensor = true;
        *nb_photo_sensor = EEPROM.read(offset + OFFSET_NB_PHOTO_SENSOR);
    }

    // temp res reads
    *temp_sensor = false;
    if (flags & 0b00001){
        *temp_sensor = true;
        *nb_temp_sensor = EEPROM.read(offset + OFFSET_NB_TEMP_SENSOR);
    }

    // gather the index of the next package in the EEPROM
    *p_next_package = read_eeprom_uint16(offset + OFFSET_NEXT_PACKAGE);

    // gather the number of measures
    *nb_measures = read_eeprom_uint16(offset + OFFSET_NB_TEMP_SENSOR);
}

void Storage_interface::set_struct(uint16_t offset, bool timestamp, bool is_final_set, bool photo_sensor, bool temp_sensor, uint8_t timestamp_schedule, uint8_t nb_photo_sensor, uint8_t nb_temp_sensor, uint8_t photo_size, uint8_t temp_size){
    clear_eeprom_at(offset);
    uint8_t flags = 0b00000000;

    // Existing package flag for function package searcher  
    flags = flags | 0b10000000;

    const uint8_t TIMESTAMP_MASK = 0b01100000;
    flags = flags & (~TIMESTAMP_MASK);

    // Timestamp init struct


    if (timestamp && (timestamp_schedule == 0)) {
        flags = flags | 0b01000000;
        EEPROM.write(offset + OFFSET_MEASURES_SCH, 0); // Écrire 0 si pas de schedule
    } 
    else if (timestamp_schedule != 0) {
        flags = flags | 0b00100000;
        EEPROM.write(offset + OFFSET_MEASURES_SCH, timestamp_schedule);
        
        WARN_IF(!timestamp, "Redundant/conflicting timestamp parameter (expected true for schedule).")
    }
    else {
        EEPROM.write(offset + OFFSET_MEASURES_SCH, 0); 
    }


    // Sensor part
    if (photo_sensor){
        flags = flags | 0b00010000;
        EEPROM.write(offset + OFFSET_NB_PHOTO_SENSOR, nb_photo_sensor);
    }else{
        EEPROM.write(offset + OFFSET_NB_PHOTO_SENSOR, 0);
        WARN_IF(nb_photo_sensor != 0, "Bad photo sensor parameter for header writer.")
    }

    if (temp_sensor){
        flags = flags | 0b00001000;
        EEPROM.write(offset + OFFSET_NB_TEMP_SENSOR, nb_temp_sensor);
    }else{
        EEPROM.write(offset + OFFSET_NB_TEMP_SENSOR, 0);
        WARN_IF(nb_temp_sensor != 0, "Bad temperature sensor parameter for header writer.")
    }
        
    // TODO Analyse if is_final_set just be remove from parameter
    if (is_final_set){
        flags = flags | 0b00000100;
    }

    // uint16_t next package pointer set at 0
    write_eeprom_uint16(offset + OFFSET_NEXT_PACKAGE, offset + OFFSET_START_DATA_MEASURES);

    // set number of measures to 0
    write_eeprom_uint16(offset + OFFSET_NB_MEASURES, 0);

    // write flags header
    EEPROM.write(offset , flags);

    //clear_eeprom_at(offset + OFFSET_START_DATA_MEASURES);
}

void Storage_interface::add_last_package(bool timestamp, bool is_final_set, bool photo_sensor, bool temp_sensor, uint8_t timestamp_schedule, uint8_t nb_photo_sensor, uint8_t nb_temp_sensor, uint8_t photo_size, uint8_t temp_size, uint16_t nb_measures){
    uint16_t p_last_header, free_space;
    uint8_t flags;

    get_last_header_nbpackage(&p_last_header);
    flags = EEPROM.read(p_last_header);

    // change is_last_package flag 
    flags = flags & 0b11111011;
    EEPROM.write(p_last_header , flags);

    free_space = EEPROM.read(p_last_header + OFFSET_NEXT_PACKAGE);
    set_struct(free_space, timestamp, is_final_set, photo_sensor, temp_sensor, timestamp_schedule, nb_photo_sensor, nb_temp_sensor, photo_size, temp_size);
}

// Dont check if the stored measure structure match with the header
void Storage_interface::add_measure(uint8_t* array_photo_val, uint8_t* array_temp_val, uint32_t timestamp, uint8_t nb_photo, uint8_t nb_temp){
    uint16_t p_last_header, free_space, idx;

    get_last_header_nbpackage(&p_last_header);

    free_space = EEPROM.read(p_last_header + OFFSET_NEXT_PACKAGE);
    EEPROM.write(p_last_header + OFFSET_NB_MEASURES, EEPROM.read(p_last_header + OFFSET_NB_MEASURES) + 1);

    EEPROM.put(free_space, timestamp);
    idx = p_last_header + sizeof(uint32_t);
    for(int i = 0; i < nb_photo; i++, idx += sizeof(uint8_t)){
        EEPROM.put(idx, array_photo_val[i]);
    }
    for(int i = 0; i < nb_temp; i++, idx += sizeof(uint8_t)){
        EEPROM.put(idx, array_temp_val[i]);
    }
}

void Storage_interface::get_measure(uint8_t* array_photo_val, uint8_t* array_temp_val, uint32_t* timestamp, uint8_t* nb_photo, uint8_t* nb_temp, uint16_t idx_measure){
    uint16_t p_last_header, idx;
    uint8_t flags;

    get_last_header_nbpackage(&p_last_header);
    idx = p_last_header + OFFSET_START_DATA_MEASURES;

    *nb_photo = EEPROM.read(p_last_header + OFFSET_NB_PHOTO_SENSOR);
    *nb_temp = EEPROM.read(p_last_header + OFFSET_NB_TEMP_SENSOR);

    flags = EEPROM.read(p_last_header);
    if((flags & 0b010) != 0){
        EEPROM.get(p_last_header + OFFSET_START_DATA_MEASURES, timestamp);
        idx += sizeof(uint32_t) * idx_measure;
    }else if ((flags & 0b001) != 0){
        uint8_t schedule;
        schedule = EEPROM.read(p_last_header + OFFSET_MEASURES_SCH);
        WARN_IF(schedule == 0, "Struct error for timestamp scheduling.")

        if(idx_measure % schedule != 0)
            idx += sizeof(uint32_t);
        idx += sizeof(uint32_t) * (idx_measure / schedule);
    }

    idx += ((*nb_photo) + (*nb_temp)) * idx_measure * sizeof(uint8_t);
    
    for(int i = 0; i < *nb_photo; i++, idx += sizeof(uint8_t)){
        array_photo_val[i] = EEPROM.read(idx);
    }
    for(int i = 0; i < *nb_temp; i++, idx += sizeof(uint8_t)){
        array_temp_val[i] = EEPROM.read(idx);
    }
}