#include "state.h"

void senbuf_init(SensorBuffer *sb) {
    sb->front = 0;
    sb->rear = 0;
    sb->size = 0;
}

int senbuf_is_full(SensorBuffer *sb) {
    return sb->size == STATE_SENSOR_BUFFER_SIZE;
}

int senbuf_push(SensorBuffer *sb, uint8_t value) {
    if (sb->size < STATE_SENSOR_BUFFER_SIZE) {
        sb->buffer[sb->rear] = value;
        sb->rear = (sb->rear + 1) % STATE_SENSOR_BUFFER_SIZE;
        sb->size++;
        return 1; // Success
    }
    return 0; // Buffer full
}

int senbuf_pop(SensorBuffer *sb) {
    if (sb->size > 0) {
        sb->front = (sb->front + 1) % STATE_SENSOR_BUFFER_SIZE;
        sb->size--;
        return 1; // Success
    }
    return 0; // Buffer empty
}

void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead) {
    if (lookahead >= 0 && lookahead < sb->size) {
        // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead
        int index = (sb->rear - 1 - lookahead + STATE_SENSOR_BUFFER_SIZE) % STATE_SENSOR_BUFFER_SIZE;
        *out = sb->buffer[index];
    }
}