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

/**** Inline macros ****/
// 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)); 
}

/**** header get set macros ****/

void inline Storage_interface::get_measure_sch(uint16_t header_idx, uint8_t *schedule){
    EEPROM.get(header_idx + OFFSET_MEASURES_SCH, *schedule);
}

void inline Storage_interface::set_measure_sch(uint16_t header_idx, uint8_t *schedule){
    EEPROM.put(header_idx + OFFSET_MEASURES_SCH, *schedule);
}

void inline Storage_interface::get_nb_photo_sensor(uint16_t header_idx, uint8_t *nb_photo_sensor){
    EEPROM.get(header_idx + OFFSET_NB_PHOTO_SENSOR, *nb_photo_sensor);
}

void inline Storage_interface::set_nb_photo_sensor(uint16_t header_idx, uint8_t *nb_photo_sensor){
    EEPROM.put(header_idx + OFFSET_NB_PHOTO_SENSOR, *nb_photo_sensor);
}

void inline Storage_interface::get_nb_temp_sensor(uint16_t header_idx, uint8_t *nb_temp_sensor){
    EEPROM.get(header_idx + OFFSET_NB_TEMP_SENSOR, *nb_temp_sensor);
}

void inline Storage_interface::set_nb_temp_sensor(uint16_t header_idx, uint8_t *nb_temp_sensor){
    EEPROM.put(header_idx + OFFSET_NB_TEMP_SENSOR, *nb_temp_sensor);
}

void inline Storage_interface::get_photo_meas_size(uint16_t header_idx, uint8_t *photo_meas_size){
    EEPROM.get(header_idx + OFFSET_PHOTO_MEASURES_SIZE, *photo_meas_size);
}

void inline Storage_interface::set_photo_meas_size(uint16_t header_idx, uint8_t *photo_meas_size){
    EEPROM.put(header_idx + OFFSET_PHOTO_MEASURES_SIZE, *photo_meas_size);
}

void inline Storage_interface::get_temp_meas_size(uint16_t header_idx, uint8_t *temp_meas_size){
    EEPROM.get(header_idx + OFFSET_TEMP_MEASURES_SIZE, *temp_meas_size);
}

void inline Storage_interface::set_temp_meas_size(uint16_t header_idx, uint8_t *temp_meas_size){
    EEPROM.put(header_idx + OFFSET_TEMP_MEASURES_SIZE, *temp_meas_size);
}

void inline Storage_interface::get_nb_measures(uint16_t header_idx, uint16_t *nb_measures){
    EEPROM.get(header_idx + OFFSET_NB_MEASURES, *nb_measures);
}

void inline Storage_interface::set_nb_measures(uint16_t header_idx, uint16_t *nb_measures){
    EEPROM.put(header_idx + OFFSET_NB_MEASURES, *nb_measures);
}

void inline Storage_interface::get_next_package(uint16_t header_idx, uint16_t *next_package){
    EEPROM.get(header_idx + OFFSET_NEXT_PACKAGE, *next_package);
}

void inline Storage_interface::set_next_package(uint16_t header_idx, uint16_t *next_package){
    EEPROM.put(header_idx + OFFSET_NEXT_PACKAGE, *next_package);
}

// void inline Storage_interface::get_start_data_measure(uint16_t header_idx, uint16_t *start_data_measure){
//     EEPROM.get(header_idx + OFFSET_START_DATA_MEASURES, *start_data_measure);
// }

// void inline Storage_interface::set_start_data_measure(uint16_t header_idx, uint16_t *start_data_measure){
//     EEPROM.put(header_idx + OFFSET_START_DATA_MEASURES, *start_data_measure);
// }


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;
        get_next_package(start_package, &start_package);
        flags = EEPROM.read(start_package);
        nb_package++;
    }
    return nb_package;
}

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

/**
* @brief get_idx_package
* * Get the EEPROM index of the targeted package
* 
* * @param package_number @c uint16_t
* @return @c uint16_t idx_package_header
*/
uint16_t Storage_interface::get_idx_package(uint16_t package_number){
    uint8_t flags;
    uint16_t start_package = 0, nb_package = 0;
    uint16_t last_header_idx = 0;
    flags = EEPROM.read(start_package);

    while (((flags & 0b1) != 0) && nb_package < package_number){
        last_header_idx = start_package;
        get_next_package(start_package, &start_package);
        flags = EEPROM.read(start_package);
        nb_package++;
    }
    return last_header_idx;
}


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 & 0b10000000) == 0){
        DLOGLN("Missing struct index or bad index")
        //while(true);
    }

    // is the final package ?
    if ((flags & 0b00000100) != 0){
        *is_final_set = true; 
    }
    
    // timestamps reads
    *timestamp = false;
    if ((flags & 0b00100000) != 0 ){
        *timestamp = true;
        get_measure_sch(offset, timestamp_schedule);
    }
    *timestamp_schedule = 0;
    if (flags & 0b01000000){
        *timestamp = true;
        get_measure_sch(offset, timestamp_schedule);
        WARN_IF(*timestamp_schedule != 0, "Incoherent timestamp parameter in the header.")
    }

    // photo res reads
    *photo_sensor = false;
    if (flags & 0b00010000){
        *photo_sensor = true;
        get_nb_photo_sensor(offset, nb_photo_sensor);
    }

    // temp res reads
    *temp_sensor = false;
    if (flags & 0b00001000){
        *temp_sensor = true;
        get_nb_temp_sensor(offset, nb_temp_sensor);
    }

    // gather the index of the next package in the EEPROM
    get_next_package(offset, p_next_package);

    // gather the number of measures
    get_nb_measures(offset, nb_measures);

    // read type size of photo temperature measures
    get_photo_meas_size(offset, photo_size);
    get_temp_meas_size(offset, temp_size);
}

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;
        timestamp_schedule = 0;
        set_measure_sch(offset, &timestamp_schedule);
    } else if (timestamp_schedule != 0) {
        flags = flags | 0b00100000;
        set_measure_sch(offset, &timestamp_schedule);
        
        WARN_IF(!timestamp, "Redundant/conflicting timestamp parameter (expected true for schedule).")
    } else {
        timestamp_schedule = 0;
        set_measure_sch(offset, &timestamp_schedule);
    }


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

    if (temp_sensor){
        flags = flags | 0b00001000;
        set_nb_temp_sensor(offset, &nb_temp_sensor);
    }else{
        nb_temp_sensor = 0;
        set_nb_temp_sensor(offset, &nb_temp_sensor);
        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 to free space
    uint16_t p_next_package = offset + OFFSET_START_DATA_MEASURES;

    set_next_package(offset, &p_next_package);

    // set number of measures to 0
    uint16_t nb_measures = 0;
    set_nb_measures(offset, &nb_measures);

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

    // write types measures size
    set_photo_meas_size(offset, &photo_size);
    set_temp_meas_size(offset, &temp_size);
}

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 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);

    get_next_package(p_last_header, &free_space);
    set_struct(free_space, timestamp, is_final_set, photo_sensor, temp_sensor, timestamp_schedule, nb_photo_sensor, nb_temp_sensor, photo_size, temp_size);
}

// the function ont 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, next_free_space, nb_measure;

    get_last_header_nbpackage(&p_last_header);

    get_next_package(p_last_header, &free_space);
    get_nb_measures(p_last_header, &nb_measure);
    nb_measure++;
    set_nb_measures(p_last_header, &nb_measure);

    EEPROM.put(free_space, timestamp);
    idx = free_space + 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]);
    }
    next_free_space = free_space + sizeof(uint32_t) + sizeof(uint8_t) * (nb_photo + nb_temp);
    set_next_package(p_last_header, &next_free_space);
}

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;

    get_nb_photo_sensor(p_last_header, nb_photo);
    get_nb_temp_sensor(p_last_header, nb_temp);

    flags = EEPROM.read(p_last_header);
    if((flags & 0b01000000) != 0){
        EEPROM.get(p_last_header + OFFSET_START_DATA_MEASURES, timestamp);
        idx += sizeof(uint32_t) * idx_measure;
    }else if ((flags & 0b00100000) != 0){
        uint8_t schedule;
        get_measure_sch(p_last_header, &schedule);
        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);
    }
}

void Storage_interface::get_measure_at(uint8_t* array_photo_val, uint8_t* array_temp_val, uint32_t* timestamp, uint8_t* nb_photo, uint8_t* nb_temp, uint16_t package_number, uint16_t measure_number){
    uint16_t p_header, idx;
    uint8_t flags;

    p_header = get_idx_package(package_number);
    idx = p_header + OFFSET_START_DATA_MEASURES;

    get_nb_photo_sensor(p_header, nb_photo);
    get_nb_temp_sensor(p_header, nb_temp);

    flags = EEPROM.read(p_header);
    if((flags & 0b01000000) != 0){
        EEPROM.get(p_header + OFFSET_START_DATA_MEASURES, timestamp);
        idx += sizeof(uint32_t) * measure_number;
    }else if ((flags & 0b00100000) != 0){
        uint8_t schedule;
        get_measure_sch(p_header, &schedule);
        WARN_IF(schedule == 0, "Struct error for timestamp scheduling.")

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

    idx += ((*nb_photo) + (*nb_temp)) * measure_number * 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);
    }
}


void Storage_interface::write_csv_header(bool* timestamp, bool* photo_sensor, bool* temp_sensor, uint8_t* nb_photo_sensor, uint8_t* nb_temp_sensor){
    String header_csv = "";
    if(*timestamp){
        header_csv += String("Epoch,");
    }
    if(*photo_sensor){
        for(int8_t i; i < *nb_photo_sensor; i++){
            header_csv += String("Photo_sensor") + String(i) + String(",");
        }
    }
    if(*temp_sensor){
        for(int8_t i; i < *nb_photo_sensor; i++){
            header_csv += String("Temp_sensor") + String(i) + String(",");
        }
    }
    if (header_csv.length() > 0) {
        header_csv.remove(header_csv.length() - 1);
    }
    Serial.println(header_csv);
}

/**
* @brief upload_csv
* * 
* Upload all stored package, one file per package
* @attention After the transfert, the entire EEPROM mem will be cleared.
*/
void Storage_interface::upload_csv(){
    const String close_transfert = "END_TRANSFERT", end_package = "END_PACKAGE", start_package = "START_PACKAGE";
    uint16_t nb_package = get_nb_package(), package_number = 0;

    WARN_IF(nb_package == 0, "Warning : upload csv function call but no data stored in memory.")
    
    while(package_number < nb_package){
        Serial.println(start_package);
        uint16_t next_package, nb_measures;
        uint8_t schedule, nb_photo_sensor, nb_temp_sensor, photo_size, temp_size;
        bool timestamp, is_final, photo_sensor, temp_sensor;

        get_struct(package_number, &timestamp, &is_final, &photo_sensor, &temp_sensor, &schedule, &nb_photo_sensor, &nb_temp_sensor, &photo_size, &temp_size, &next_package, &nb_measures);
        write_csv_header(&timestamp, &photo_sensor, &temp_sensor, &nb_photo_sensor, &nb_temp_sensor);

        uint8_t photo_array[nb_photo_sensor], temp_array[nb_temp_sensor];
        uint32_t timestamp_val;
        String line;
        for(uint16_t measure_number = 0; measure_number < nb_measures; measure_number++){
            get_measure_at(photo_array, temp_array, &timestamp_val, &nb_photo_sensor, &nb_temp_sensor, package_number, measure_number);
            line = String("");
            if(timestamp){
            line += String(timestamp_val);
            }
            if(photo_sensor){
                for(int8_t i; i < nb_photo_sensor; i++){
                    line += String(photo_array[i]) + String(",");
                }
            }
            if(temp_sensor){
                for(int8_t i; i < nb_temp_sensor; i++){
                    line += String(temp_array[i]) + String(",");
                }
            }
            if (line.length() > 0) {
                line.remove(line.length() - 1);
            }
            Serial.println(line);
            delay(100);
        }
        Serial.println(end_package);
        package_number++;

    }
    Serial.println(close_transfert);

    // TODO uncomment after upload csv done and must conform to the tests
    // clear_eeprom();
}