#include "queue.h"

#include <stdlib.h>
#include <string.h>

queue_t* queue_init() {
    queue_t* queue = calloc(1, sizeof(queue_t));

    pthread_mutex_init(&queue->lock, NULL);
    pthread_cond_init(&queue->trigger, NULL);

    queue->len = 0;
    queue->packet = calloc(QUEUE_MAX_LEN, sizeof(uint8_t *));
    for (int i = 0; i < QUEUE_MAX_LEN; ++i) {
        queue->packet[i] = malloc(PACKET_MAX_LEN);
    }
    queue->packet_len = calloc(QUEUE_MAX_LEN, sizeof(uint16_t));
    queue->destination = malloc(QUEUE_MAX_LEN);

    return queue;
}

int queue_add(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t destination) {
    int rv = 0;

    pthread_mutex_lock(&queue->lock);

    if (queue->len < QUEUE_MAX_LEN) {
        queue->packet_len[queue->len] = packet_len;
        memcpy(queue->packet[queue->len], packet, packet_len);
        queue->destination[queue->len] = destination;
        ++queue->len;
    } else {
        rv = 1;
    }

    pthread_mutex_unlock(&queue->lock);

    return rv;
}