#include <util/atomic.h>

#include "buffer.h"

void insert_to_buffer(uint16_t val, volatile buffer_t* buf){
    ATOMIC_BLOCK(ATOMIC_FORCEON){
        buf->position++;
        if(buf->position == BUFFER_SIZE)
            buf->position = 0;
        buf->values[buf->position] = val;
    }
}

float get_buffer_mean(volatile buffer_t* buf){

    ///* discard lowest and highest value */
    //uint16_t low=0xFFFF;
    //uint16_t high=0;
    //uint16_t index_l = 0;
    //uint16_t index_h = 0;
    //for(uint16_t i=0; i<BUFFER_SIZE; i++){
    //    if(buf->values[i] < low){
    //        low=buf->values[i];
    //        index_l = i;
    //    }
    //    if(buf->values[i] > high){
    //        high=buf->values[i];
    //        index_h = i;
    //    }
    //}
        

    uint32_t sum = 0;
    for(uint16_t i=0; i<BUFFER_SIZE; i++){
        //if(i == index_h || i == index_l)
        //    continue;
        sum += buf->values[i];
    }

    uint16_t res = sum/(BUFFER_SIZE/*-2*/);
    return res;
}