Leakage-Control/calib_board/usr/algo/algo_queue.c

#include "algo_queue.h"

/* 计算队列中第i个元素的地址 */
#define at(i)                       (((char *)(queue->base))+(i)*(queue->dsize))
/* 数据赋值：将源地址s的数据复制到目标地址d */
#define assign(d, s)                memcpy((d), (s), queue->dsize)
/* 获取队列中逻辑位置i的元素地址（考虑循环队列） */
#define qa(i)                       at((queue->head + queue->capacity + (i)) % (queue->capacity))

/* 简易内存复制函数 */
static void* memcpy(void* dest, const void* src, int n)
{
    char* dst = dest;
    const char* s = src;
    while (n--) *dst++ = *s++;
    return dest;
}

/* 将队列中从index开始的size个元素向前移动一位 */
static void move_forward(queue queue, int index, int size)
{
    for (int i = index; i < index + size; i++) assign(qa(i - 1), qa(i));
}

/* 将队列中从index开始的size个元素向后移动一位 */
static void move_backward(queue queue, int index, int size)
{
    for (int i = index + size; i > index; i--) assign(qa(i), qa(i - 1));
}

/**
 *  \brief 标准入队操作（队尾添加）
 *  \param[in] queue: 队列对象指针
 *  \param[in] data: 待添加数据地址（NULL表示仅移动指针）
 *  \return 1=成功, 0=失败（队列无效或已满）
 *  
 *  \details 在队列尾部添加新元素。如果队列已满则操作失败。
 *           当data不为NULL时，将数据复制到队列存储区。
 *           更新尾指针和队列大小。
 */
int queue_push(queue queue, void* data)
{
    if (!queue) return 0;               // 检查队列指针有效性
    if (queue->size == queue->capacity) return 0;  // 队列已满
    if (data) assign(at(queue->tail), data);  // 数据复制到队尾
    queue->tail = (queue->tail + 1) % queue->capacity;  // 更新尾指针（循环）
    queue->size++;                      // 更新元素数量
    return 1;
}

/**
 *  \brief 强制入队操作（覆盖式添加）
 *  \param[in] queue: 队列对象指针
 *  \param[in] data: 待添加数据地址（NULL表示仅移动指针）
 *  \return 1=成功, 0=失败（仅当队列无效时）
 *  
 *  \details 当队列未满时等同于queue_push()。
 *           当队列已满时覆盖最旧数据（队头数据），
 *           实现循环缓冲效果。
 */
int queue_push2(queue queue, void* data)
{
    if (!queue) return 0;
    if (queue->size < queue->capacity) return queue_push(queue, data);  // 队列未满时调用标准入队
    
    // 队列已满时覆盖队头数据（实现循环覆盖）
    if (data) assign(at(queue->tail), data);
    queue->tail = (queue->tail + 1) % queue->capacity;
    queue->head = (queue->head + 1) % queue->capacity;  // 头指针后移（覆盖最旧数据）
    return 1;
}

/**
 *  \brief 出队操作（队头移除）
 *  \param[in] queue: 队列对象指针
 *  \param[out] data: 数据接收地址（NULL表示丢弃数据）
 *  \return 1=成功, 0=失败（队列无效或为空）
 *  
 *  \details 从队列头部移除元素。如果data不为NULL，
 *           将移除的元素复制到指定地址。
 *           更新头指针和队列大小。
 */
int queue_pop(queue queue, void* data)
{
    if (!queue) return 0;
    if (queue->size == 0) return 0;     // 队列为空
    if (data) assign(data, at(queue->head));  // 复制队头数据到输出
    queue->head = (queue->head + 1) % queue->capacity;  // 更新头指针（循环）
    queue->size--;                      // 更新元素数量
    return 1;
}

/**
 *  \brief 指定位置插入元素
 *  \param[in] queue: 队列对象指针
 *  \param[in] index: 插入位置索引（0=队头, size=队尾）
 *  \param[in] data: 待插入数据地址（NULL表示仅空出位置）
 *  \return 1=成功, 0=失败（索引越界或队列已满）
 *  
 *  \details 在指定位置插入新元素，后续元素后移。
 *           自动选择最优移动方向（前移或后移元素）。
 *           时间复杂度O(min(index, size-index))。
 */
int queue_insert(queue queue, int index, void* data)
{
    if (!queue) return 0;
    if (index < 0 || index > queue->size) return 0;  // 索引越界检查
    if (queue->size == queue->capacity) return 0;    // 队列已满

    // 根据插入位置选择最优移动方向
    if (index <= queue->size / 2) {
        // 向前移动前半部分元素
        move_forward(queue, 0, index);
        queue->head = (queue->head + queue->capacity - 1) % queue->capacity;  // 头指针前移
    } else {
        // 向后移动后半部分元素
        move_backward(queue, index, queue->size - index);
        queue->tail = (queue->tail + 1) % queue->capacity;  // 尾指针后移
    }
    
    // 在空出的位置插入数据
    if (data) assign(at((queue->head + index) % (queue->capacity)), data);
    queue->size++;
    return 1;
}

/**
 *  \brief 指定位置删除元素
 *  \param[in] queue: 队列对象指针
 *  \param[in] index: 删除位置索引（0=队头, size-1=队尾）
 *  \param[out] data: 数据接收地址（NULL表示丢弃数据）
 *  \return 1=成功, 0=失败（索引越界或队列为空）
 *  
 *  \details 删除指定位置元素，后续元素前移。
 *           自动选择最优移动方向。
 *           时间复杂度O(min(index, size-index))。
 */
int queue_erase(queue queue, int index, void* data)
{
    if (!queue) return 0;
    if (index < 0 || index >= queue->size) return 0;  // 索引越界检查
    if (queue->size == 0) return 0;     // 队列为空

    // 保存被删除元素（如果需要）
    if (data) assign(data, at((queue->head + index) % (queue->capacity)));

    // 根据删除位置选择最优移动方向
    if (index <= queue->size / 2) {
        // 向后移动前半部分元素
        move_backward(queue, 0, index);
        queue->head = (queue->head + 1) % queue->capacity;  // 头指针后移
    } else {
        // 向前移动后半部分元素
        move_forward(queue, index + 1, queue->size - index - 1);
        queue->tail = (queue->tail + queue->capacity - 1) % queue->capacity;  // 尾指针前移
    }
    
    queue->size--;
    return 1;
}

/**
 *  \brief 清空队列
 *  \param[in] queue: 队列对象指针
 *  
 *  \details 重置队列状态（头尾指针归零，大小归零）。
 *           不释放存储内存，保持队列容量不变。
 */
void queue_clear(queue queue)
{
    if (!queue) return;
    queue->tail = 0;
    queue->head = 0;
    queue->size = 0;
}

/**
 *  \brief 获取元素地址
 *  \param[in] queue: 队列对象指针
 *  \param[in] index: 元素索引（0=队头, size-1=队尾）
 *  \return 元素地址（失败返回NULL）
 *  
 *  \details 获取队列中指定位置元素的直接指针。
 *           可用于直接修改元素值（无需复制）。
 *           \warning 返回的指针在队列结构修改后可能失效。
 */
void* queue_data(queue queue, int index)
{
    if (!queue) return 0;
    if (index < 0 || index >= queue->size) return 0;  // 索引越界检查
    return (void*)at((queue->head + index) % (queue->capacity));
}