#include "custom_fifo.h"
#include <string.h>

int fifo_init(custom_fifo *p_fifo, uint8_t *p_buff, uint16_t len)
{
    if ((p_fifo == (void *)0) || (p_buff == (void *)0))
    {
        return -1;
    }
    else
    {
        p_fifo->fifo_size = len;
        p_fifo->p_buff_head = p_buff;
        p_fifo->p_buff_tail = p_fifo->p_buff_head + p_fifo->fifo_size - 1;
        p_fifo->p_input = p_fifo->p_buff_head;
        p_fifo->p_output = p_fifo->p_buff_head;
        return 0;
    }
}

uint16_t fifo_clear(custom_fifo *p_fifo)
{
    p_fifo->p_input = p_fifo->p_buff_head;
    p_fifo->p_output = p_fifo->p_buff_head;
    return 0;
}

int fifo_push(custom_fifo *p_fifo, uint8_t dat)
{
    uint8_t *pTmp = p_fifo->p_input;
    *pTmp = dat; // 使用pTmp写，是因为判断fifo是否为空时根据if(p_fifo->p_input == p_fifo->p_output)为空
    pTmp++;
    if (pTmp > p_fifo->p_buff_tail)
    {
        pTmp = p_fifo->p_buff_head;
    }

    if (pTmp != p_fifo->p_output) // 如果fifo没有满了
    {
        p_fifo->p_input = pTmp;
        return 0;
    }
    else
    {
        return -1;
    }
}

int fifo_push_buff(custom_fifo *p_fifo, void *dat, uint16_t len)
{
    int cur_len = fifi_get_counter(p_fifo);
    cur_len = p_fifo->fifo_size - cur_len;
    if (len > cur_len)
    {
        return -1;
    }
#if 0
    uint8_t *p = (uint8_t *)dat;
    for (int i = 0; i < len; i++)
    {
        fifo_push(p_fifo, *p++);
    }
#else
    cur_len = p_fifo->p_buff_tail - p_fifo->p_input;
    if (len <= cur_len)
    {
        memcpy(p_fifo->p_input, dat, len);
        p_fifo->p_input = p_fifo->p_input + len;
    }
    else
    {
        memcpy(p_fifo->p_input, dat, cur_len);
        len -= cur_len;
        memcpy(p_fifo->p_buff_head, (uint8_t *)dat + cur_len, len);
        p_fifo->p_input = p_fifo->p_buff_head + len;
    }
#endif
    return 0;
}

uint8_t fifo_pop(custom_fifo *p_fifo, int *p_status)
{
    uint8_t reDat = 0;
    if (p_fifo->p_input != p_fifo->p_output) // 如果有数据
    {
        reDat = *p_fifo->p_output;
        p_fifo->p_output++;
        if (p_fifo->p_output > p_fifo->p_buff_tail)
        {
            p_fifo->p_output = p_fifo->p_buff_head;
        }
        if (p_status != (void *)0)
        {
            *p_status = 0;
        }
    }
    else
    {
        if (p_status != (void *)0)
        {
            *p_status = -1;
        }
    }
    return reDat;
}

uint16_t fifo_pop_buff(custom_fifo *p_fifo, uint8_t *p_buff, uint16_t len)
{
    uint8_t *p_input = p_fifo->p_input;
    uint16_t count = ((p_input + p_fifo->fifo_size) - p_fifo->p_output);
    if (count >= p_fifo->fifo_size)
    {
        count = count - p_fifo->fifo_size;
    }

    if (len > count)
    {
        len = count;
    }

    if (len > 0) // 如果有数据弹出
    {
        if ((p_fifo->p_output + len) < p_fifo->p_buff_tail)
        {
            memcpy(p_buff, p_fifo->p_output, len);
            p_fifo->p_output += len;
        }
        else if ((p_fifo->p_output + len) > p_fifo->p_buff_tail)
        {
            count = p_fifo->p_buff_tail - p_fifo->p_output + 1;
            memcpy(p_buff, p_fifo->p_output, count);
            p_buff += count;
            count = len - count;
            memcpy(p_buff, p_fifo->p_buff_head, count);
            p_fifo->p_output = p_fifo->p_buff_head + count;
        }
        else
        {
            memcpy(p_buff, p_fifo->p_output, len);
            p_fifo->p_output = p_fifo->p_buff_tail;
        }

        return len;
    }
    else
    {
        return 0;
    }
}

uint16_t fifi_get_counter(custom_fifo *p_fifo)
{
    uint16_t count = 0;
    count = (p_fifo->p_input + p_fifo->fifo_size) - p_fifo->p_output;
    if (count >= p_fifo->fifo_size)
    {
        count = count - p_fifo->fifo_size;
    }

    return count;
}