#include "RdsPiBuffer.hpp"

RdsPiBuffer::RdsPiBuffer()
{
    this->clear();
}

RdsPiBuffer::State
RdsPiBuffer::add(uint16_t value,
                 bool     error)
{
    this->pos = (this->pos + 1) % BUFF_SIZE;
    this->buff[this->pos] = value;

    const uint8_t errorPos = this->pos / 8;
    const uint8_t errorBitPos = this->pos % 8;

    if (error)
    {
        this->errorBuff[errorPos] |= (1 << errorBitPos);
    }
    else
    {
        this->errorBuff[errorPos] &= ~(1 << errorBitPos);
    }

    if (this->fill < BUFF_SIZE)
    {
        this->fill++;
    }

    return this->getState(value);
}

void
RdsPiBuffer::clear()
{
    this->fill = 0;
    this->pos = (uint8_t)-1;
}

RdsPiBuffer::State
RdsPiBuffer::getState(uint16_t value)
{
    uint8_t count = 0;
    uint8_t correctCount = 0;

    for (uint8_t i = 0; i < this->fill; i++)
    {
        if (this->buff[i] == value)
        {
            count++;
            if ((this->errorBuff[i / 8] & (1 << (i % 8))) == 0)
            {
                correctCount++;
            }
        }
    }

    if (correctCount >= 2)
    {
        return STATE_CORRECT;
    }
    else if (count >= 2 && correctCount)
    {
        return STATE_VERY_LIKELY;
    }
    else if (count >= 3)
    {
        return STATE_LIKELY;
    }
    else if (count == 2 || correctCount)
    {
        return STATE_UNLIKELY;
    }

    return STATE_INVALID;
}