/* -------------------------------------------------------------------------- */
/* --- DEPENDANCIES --------------------------------------------------------- */

#include <pthread.h>
#include <assert.h>
#include <math.h>

#include "jitqueue.h"

/* -------------------------------------------------------------------------- */
/* --- PRIVATE MACROS ------------------------------------------------------- */

/* -------------------------------------------------------------------------- */
/* --- PRIVATE CONSTANTS & TYPES -------------------------------------------- */
#define TX_START_DELAY          1500    /* microseconds */
                                        /* TODO: get this value from HAL? */
#define TX_MARGIN_DELAY         1000    /* Packet overlap margin in microseconds */
                                        /* TODO: How much margin should we take? */
#define TX_JIT_DELAY            1022000   /* Pre-delay to program packet for TX in microseconds */
#define TX_MAX_ADVANCE_DELAY    ((JIT_NUM_BEACON_IN_QUEUE + 1) * 128 * 1E6) /* Maximum advance delay accepted for a TX packet, compared to current time */

#define BEACON_GUARD            3000000 /* Interval where no ping slot can be placed,
                                            to ensure beacon can be sent */
#define BEACON_RESERVED         2120000 /* Time on air of the beacon, with some margin */

/* -------------------------------------------------------------------------- */
/* --- PRIVATE VARIABLES (GLOBAL) ------------------------------------------- */
static pthread_mutex_t mx_jit_queue = PTHREAD_MUTEX_INITIALIZER; /* control access to JIT queue */

/* -------------------------------------------------------------------------- */
/* --- PRIVATE FUNCTIONS DEFINITION ----------------------------------------- */
typedef int (*__compar_d_fn_t) (const void *, const void *, void *);
static void qsort_r(void  *base,
           size_t nel,
           size_t width,
           __compar_d_fn_t comp,
		   void *arg)
{
	size_t wgap, i, j, k;
	char tmp;

	if ((nel > 1) && (width > 0)) {
		assert(nel <= ((size_t)(-1)) / width); /* check for overflow */
		wgap = 0;
		do {
			wgap = 3 * wgap + 1;
		} while (wgap < (nel-1)/3);
		/* From the above, we know that either wgap == 1 < nel or */
		/* ((wgap-1)/3 < (int) ((nel-1)/3) <= (nel-1)/3 ==> wgap <  nel. */
		wgap *= width;			/* So this can not overflow if wnel doesn't. */
		nel *= width;			/* Convert nel to 'wnel' */
		do {
			i = wgap;
			do {
				j = i;
				do {
					register char *a;
					register char *b;

					j -= wgap;
					a = j + ((char *)base);
					b = a + wgap;
					if ((*comp)(a, b, arg) <= 0) {
						break;
					}
					k = width;
					do {
						tmp = *a;
						*a++ = *b;
						*b++ = tmp;
					} while (--k);
				} while (j >= wgap);
				i += width;
			} while (i < nel);
			wgap = (wgap - width)/3;
		} while (wgap);
	}
}

/* -------------------------------------------------------------------------- */
/* --- PUBLIC FUNCTIONS DEFINITION ----------------------------------------- */

uint32_t time_on_air(struct pkt_tx_s *packet) {
    int32_t val;
    uint8_t SF, H, DE;
    uint16_t BW;
    uint32_t payloadSymbNb, Tpacket;
    double Tsym, Tpreamble, Tpayload, Tfsk;

    if (packet == NULL) {
        MSG_DEBUG(DEBUG_INFO, "ERROR: Failed to compute time on air, wrong parameter\n");
        return 0;
    }

    /* Get bandwidth */
    val = bw_getval(packet->bandwidth);
    if (val != -1) {
        BW = (uint16_t)(val / 1E3);
    } else {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: Cannot compute time on air for this packet, unsupported bandwidth (0x%02X)\n", packet->bandwidth);
        return 0;
    }

    /* Get datarate */
    val = sf_getval(packet->datarate);
    if (val != -1) {
        SF = (uint8_t)val;
    } else {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: Cannot compute time on air for this packet, unsupported datarate (0x%02X)\n", packet->datarate);
        return 0;
    }

    /* Duration of 1 symbol */
    Tsym = pow(2, SF) / BW;

    /* Duration of preamble */
    Tpreamble = ((double)(packet->preamble) + 4.25) * Tsym;

    /* Duration of payload */
    H = (packet->no_header==false) ? 0 : 1; /* header is always enabled, except for beacons */
    DE = (SF >= 11) ? 1 : 0; /* Low datarate optimization enabled for SF11 and SF12 */

    payloadSymbNb = 8 + (ceil((double)(8*packet->size - 4*SF + 28 + 16 - 20*H) / (double)(4*(SF - 2*DE))) * (packet->coderate)); /* Explicitely cast to double to keep precision of the division */

    Tpayload = payloadSymbNb * Tsym;

    /* Duration of packet */
    Tpacket = Tpreamble + Tpayload;

    return Tpacket;
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

bool jit_queue_is_full(struct jit_queue_s *queue) {
    bool result;

    pthread_mutex_lock(&mx_jit_queue);

    result = (queue->num_pkt == JIT_QUEUE_MAX)?true:false;

    pthread_mutex_unlock(&mx_jit_queue);

    return result;
}

bool jit_queue_is_empty(struct jit_queue_s *queue) {
    bool result;

    pthread_mutex_lock(&mx_jit_queue);
    result = (queue->num_pkt == 0)?true:false;

    pthread_mutex_unlock(&mx_jit_queue);

    return result;
}

void jit_queue_init(struct jit_queue_s *queue) {
    int i;

    pthread_mutex_lock(&mx_jit_queue);

    memset(queue, 0, sizeof(*queue));
    for (i=0; i<JIT_QUEUE_MAX; i++) {
        queue->nodes[i].pre_delay = 0;
        queue->nodes[i].post_delay = 0;
    }

    pthread_mutex_unlock(&mx_jit_queue);
}

int compare(const void *a, const void *b, void *arg)
{
    struct jit_node_s *p = (struct jit_node_s *)a;
    struct jit_node_s *q = (struct jit_node_s *)b;
    int *counter = (int *)arg;
    int p_count, q_count;

    p_count = p->pkt.count_us;
    q_count = q->pkt.count_us;

    if (p_count > q_count)
        *counter = *counter + 1;

    return p_count - q_count;
}

void jit_sort_queue(struct jit_queue_s *queue) {
    int counter = 0;

    if (queue->num_pkt == 0) {
        return;
    }

    MSG_DEBUG(DEBUG_JIT, "sorting queue in ascending order packet timestamp - queue size:%u\n", queue->num_pkt);
    qsort_r(queue->nodes, queue->num_pkt, sizeof(queue->nodes[0]), compare, &counter);
    MSG_DEBUG(DEBUG_JIT, "sorting queue done - swapped:%d\n", counter);
}

bool jit_collision_test(uint32_t p1_count_us, uint32_t p1_pre_delay, uint32_t p1_post_delay, uint32_t p2_count_us, uint32_t p2_pre_delay, uint32_t p2_post_delay) {
    if (((p1_count_us - p2_count_us) <= (p1_pre_delay + p2_post_delay + TX_MARGIN_DELAY)) ||
        ((p2_count_us - p1_count_us) <= (p2_pre_delay + p1_post_delay + TX_MARGIN_DELAY))) {
        return true;
    } else {
        return false;
    }
}

enum jit_error_e jit_enqueue(struct jit_queue_s *queue, struct timeval *time, struct pkt_tx_s *packet, enum jit_pkt_type_e pkt_type) {
    int i = 0;
    uint32_t time_us = time->tv_sec * 1000000UL + time->tv_usec; /* convert time in µs */
    uint32_t packet_post_delay = 0;
    uint32_t packet_pre_delay = 0;
    uint32_t target_pre_delay = 0;
    enum jit_error_e err_collision;
    uint32_t asap_count_us;

    MSG_DEBUG(DEBUG_JIT, "Current concentrator time is %u, pkt_type=%d\n", time_us, pkt_type);

    if (packet == NULL) {
        MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: invalid parameter\n");
        return JIT_ERROR_INVALID;
    }

    if (jit_queue_is_full(queue)) {
        MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: cannot enqueue packet, JIT queue is full\n");
        return JIT_ERROR_FULL;
    }

    /* Compute packet pre/post delays depending on packet's type */
    switch (pkt_type) {
        case JIT_PKT_TYPE_DOWNLINK_CLASS_A:
        case JIT_PKT_TYPE_DOWNLINK_CLASS_B:
        case JIT_PKT_TYPE_DOWNLINK_CLASS_C:
            packet_pre_delay = TX_START_DELAY + TX_JIT_DELAY;
            packet_post_delay = time_on_air(packet) * 1000UL; /* in us */
            MSG_DEBUG(DEBUG_JIT, "Time on air %uuS\n", packet_post_delay);
            break;
        case JIT_PKT_TYPE_BEACON:
            /* As defined in LoRaWAN spec */
            packet_pre_delay = TX_START_DELAY + BEACON_GUARD + TX_JIT_DELAY;
            packet_post_delay = BEACON_RESERVED;
            break;
        default:
            break;
    }

    pthread_mutex_lock(&mx_jit_queue);

    /* An immediate downlink becomes a timestamped downlink "ASAP" */
    /* Set the packet count_us to the first available slot */
    if (pkt_type == JIT_PKT_TYPE_DOWNLINK_CLASS_C) {
        /* change tx_mode to timestamped */
        packet->tx_mode = TIMESTAMPED;

        /* Search for the ASAP timestamp to be given to the packet */
        asap_count_us = time_us + 1E6; /* TODO: Take 1 second margin, to be refined */
        if (queue->num_pkt == 0) {
            /* If the jit queue is empty, we can insert this packet */
            MSG_DEBUG(DEBUG_JIT, "DEBUG: insert IMMEDIATE downlink, first in JiT queue (count_us=%u)\n", asap_count_us);
        } else {
            /* Else we can try to insert it:
                - ASAP meaning NOW + MARGIN
                - at the last index of the queue
                - between 2 downlinks in the queue
            */

            /* First, try if the ASAP time collides with an already enqueued downlink */
            for (i=0; i<queue->num_pkt; i++) {
                if (jit_collision_test(asap_count_us, packet_pre_delay, packet_post_delay, queue->nodes[i].pkt.count_us, queue->nodes[i].pre_delay, queue->nodes[i].post_delay) == true) {
                    MSG_DEBUG(DEBUG_JIT, "DEBUG: cannot insert IMMEDIATE downlink at count_us=%u, collides with %u (index=%d)\n", asap_count_us, queue->nodes[i].pkt.count_us, i);
                    break;
                }
            }
            if (i == queue->num_pkt) {
                /* No collision with ASAP time, we can insert it */
                MSG_DEBUG(DEBUG_JIT, "DEBUG: insert IMMEDIATE downlink ASAP at %u (no collision)\n", asap_count_us);
            } else {
                /* Search for the best slot then */
                for (i=0; i<queue->num_pkt; i++) {
                    asap_count_us = queue->nodes[i].pkt.count_us + queue->nodes[i].post_delay + packet_pre_delay + TX_JIT_DELAY + TX_MARGIN_DELAY;
                    if (i == (queue->num_pkt - 1)) {
                        /* Last packet index, we can insert after this one */
                        MSG_DEBUG(DEBUG_JIT, "DEBUG: insert IMMEDIATE downlink, last in JiT queue (count_us=%u)\n", asap_count_us);
                    } else {
                        /* Check if packet can be inserted between this index and the next one */
                        MSG_DEBUG(DEBUG_JIT, "DEBUG: try to insert IMMEDIATE downlink (count_us=%u) between index %d and index %d?\n", asap_count_us, i, i+1);
                        if (jit_collision_test(asap_count_us, packet_pre_delay, packet_post_delay, queue->nodes[i+1].pkt.count_us, queue->nodes[i+1].pre_delay, queue->nodes[i+1].post_delay) == true) {
                            MSG_DEBUG(DEBUG_JIT, "DEBUG: failed to insert IMMEDIATE downlink (count_us=%u), continue...\n", asap_count_us);
                            continue;
                        } else {
                            MSG_DEBUG(DEBUG_JIT, "DEBUG: insert IMMEDIATE downlink (count_us=%u)\n", asap_count_us);
                            break;
                        }
                    }
                }
            }
        }
        /* Set packet with ASAP timestamp */
        packet->count_us = asap_count_us;
    }

    /* Check criteria_1: is it already too late to send this packet ?
     *  The packet should arrive at least at (tmst - TX_START_DELAY) to be programmed into concentrator
     *  Note: - Also add some margin, to be checked how much is needed, if needed
     *        - Valid for both Downlinks and Beacon packets
     *
     *  Warning: unsigned arithmetic (handle roll-over)
     *      t_packet < t_current + TX_START_DELAY + MARGIN
     */
    if (queue->num_pkt && (packet->count_us - time_us) <= (TX_START_DELAY + TX_MARGIN_DELAY + TX_JIT_DELAY)) {
        MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: Packet REJECTED, already too late to send it (current=%u, packet=%u, type=%d)\n", time_us, packet->count_us, pkt_type);
        pthread_mutex_unlock(&mx_jit_queue);
        return JIT_ERROR_TOO_LATE;
    }

    /* Check criteria_2: Does packet timestamp seem plausible compared to current time
     *  We do not expect the server to program a downlink too early compared to current time
     *  Class A: downlink has to be sent in a 1s or 2s time window after RX
     *  Class B: downlink has to occur in a 128s time window
     *  Class C: no check needed, departure time has been calculated previously
     *  So let's define a safe delay above which we can say that the packet is out of bound: TX_MAX_ADVANCE_DELAY
     *  Note: - Valid for Downlinks only, not for Beacon packets
     *
     *  Warning: unsigned arithmetic (handle roll-over)
                t_packet > t_current + TX_MAX_ADVANCE_DELAY
     */
    if ((pkt_type == JIT_PKT_TYPE_DOWNLINK_CLASS_A) || (pkt_type == JIT_PKT_TYPE_DOWNLINK_CLASS_B)) {
        if ((packet->count_us - time_us) > TX_MAX_ADVANCE_DELAY) {
            MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: Packet REJECTED, timestamp seems wrong, too much in advance (current=%u, packet=%u, type=%d)\n", time_us, packet->count_us, pkt_type);
            pthread_mutex_unlock(&mx_jit_queue);
            return JIT_ERROR_TOO_EARLY;
        }
    }

    /* Check criteria_3: does this new packet overlap with a packet already enqueued ?
     *  Note: - need to take into account packet's pre_delay and post_delay of each packet
     *        - Valid for both Downlinks and beacon packets
     *        - Beacon guard can be ignored if we try to queue a Class A downlink
     */
    for (i=0; i<queue->num_pkt; i++) {
        /* We ignore Beacon Guard for Class A/C downlinks */
        if (((pkt_type == JIT_PKT_TYPE_DOWNLINK_CLASS_A) || (pkt_type == JIT_PKT_TYPE_DOWNLINK_CLASS_C)) && (queue->nodes[i].pkt_type == JIT_PKT_TYPE_BEACON)) {
            target_pre_delay = TX_START_DELAY;
        } else {
            target_pre_delay = queue->nodes[i].pre_delay;
        }

        /* Check if there is a collision
         *  Warning: unsigned arithmetic (handle roll-over)
         *      t_packet_new - pre_delay_packet_new < t_packet_prev + post_delay_packet_prev (OVERLAP on post delay)
         *      t_packet_new + post_delay_packet_new > t_packet_prev - pre_delay_packet_prev (OVERLAP on pre delay)
         */
        if (jit_collision_test(packet->count_us, packet_pre_delay, packet_post_delay, queue->nodes[i].pkt.count_us, target_pre_delay, queue->nodes[i].post_delay) == true) {
            switch (queue->nodes[i].pkt_type) {
                case JIT_PKT_TYPE_DOWNLINK_CLASS_A:
                case JIT_PKT_TYPE_DOWNLINK_CLASS_B:
                case JIT_PKT_TYPE_DOWNLINK_CLASS_C:
                    MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: Packet (type=%d) REJECTED, collision with packet already programmed at %u (%u)\n", pkt_type, queue->nodes[i].pkt.count_us, packet->count_us);
                    err_collision = JIT_ERROR_COLLISION_PACKET;
                    break;
                case JIT_PKT_TYPE_BEACON:
                    if (pkt_type != JIT_PKT_TYPE_BEACON) {
                        /* do not overload logs for beacon/beacon collision, as it is expected to happen with beacon pre-scheduling algorith used */
                        MSG_DEBUG(DEBUG_JIT_ERROR, "ERROR: Packet (type=%d) REJECTED, collision with beacon already programmed at %u (%u)\n", pkt_type, queue->nodes[i].pkt.count_us, packet->count_us);
                    }
                    err_collision = JIT_ERROR_COLLISION_BEACON;
                    break;
                default:
                    MSG_DEBUG(DEBUG_ERROR, "ERROR: Unknown packet type, should not occur, BUG?\n");
                    assert(0);
                    break;
            }
            pthread_mutex_unlock(&mx_jit_queue);
            return err_collision;
        }
    }

    /* Finally enqueue it */
    /* Insert packet at the end of the queue */
    memcpy(&(queue->nodes[queue->num_pkt].pkt), packet, sizeof(struct pkt_tx_s));
    queue->nodes[queue->num_pkt].pre_delay = packet_pre_delay;
    queue->nodes[queue->num_pkt].post_delay = packet_post_delay;
    queue->nodes[queue->num_pkt].pkt_type = pkt_type;
    if (pkt_type == JIT_PKT_TYPE_BEACON) {
        queue->num_beacon++;
    }
    queue->num_pkt++;
    /* Sort the queue in ascending order of packet timestamp */
    jit_sort_queue(queue);

    /* Done */
    pthread_mutex_unlock(&mx_jit_queue);

    jit_print_queue(queue, false, DEBUG_JIT);
    MSG_DEBUG(DEBUG_JIT, "enqueued packet with count_us=%u (size=%u bytes, toa=%u us, type=%d)\n", packet->count_us, packet->size, packet_post_delay, pkt_type);

    return JIT_ERROR_OK;
}

enum jit_error_e jit_dequeue(struct jit_queue_s *queue, int index, struct pkt_tx_s *packet, enum jit_pkt_type_e *pkt_type) {
    if (packet == NULL) {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: invalid parameter\n");
        return JIT_ERROR_INVALID;
    }

    if ((index < 0) || (index >= JIT_QUEUE_MAX)) {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: invalid parameter\n");
        return JIT_ERROR_INVALID;
    }

    if (jit_queue_is_empty(queue)) {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: cannot dequeue packet, JIT queue is empty\n");
        return JIT_ERROR_EMPTY;
    }

    pthread_mutex_lock(&mx_jit_queue);

    /* Dequeue requested packet */
    memcpy(packet, &(queue->nodes[index].pkt), sizeof(struct pkt_tx_s));
    queue->num_pkt--;
    *pkt_type = queue->nodes[index].pkt_type;
    if (*pkt_type == JIT_PKT_TYPE_BEACON) {
        queue->num_beacon--;
        MSG_DEBUG(DEBUG_BEACON, "--- Beacon dequeued ---\n");
    }

    /* Replace dequeued packet with last packet of the queue */
    memcpy(&(queue->nodes[index]), &(queue->nodes[queue->num_pkt]), sizeof(struct jit_node_s));
    memset(&(queue->nodes[queue->num_pkt]), 0, sizeof(struct jit_node_s));

    /* Sort queue in ascending order of packet timestamp */
    jit_sort_queue(queue);

    /* Done */
    pthread_mutex_unlock(&mx_jit_queue);

    jit_print_queue(queue, false, DEBUG_JIT);

    MSG_DEBUG(DEBUG_JIT, "dequeued packet with count_us=%u from index %d\n", packet->count_us, index);

    return JIT_ERROR_OK;
}

enum jit_error_e jit_peek(struct jit_queue_s *queue, struct timeval *time, int *pkt_idx) {
    /* Return index of node containing a packet inline with given time */
    int i = 0;
    int idx_highest_priority = -1;
    int idx_higher_priority = -1;
    uint32_t time_us;

    if ((time == NULL) || (pkt_idx == NULL)) {
        MSG_DEBUG(DEBUG_ERROR, "ERROR: invalid parameter\n");
        return JIT_ERROR_INVALID;
    }

    if (jit_queue_is_empty(queue)) {
        return JIT_ERROR_EMPTY;
    }

    time_us = time->tv_sec * 1000000UL + time->tv_usec;

    pthread_mutex_lock(&mx_jit_queue);
    /* Search for highest priority packet to be sent */
    for (i=0; i<queue->num_pkt; i++) {
        /* First check if that packet is outdated:
         *  If a packet seems too much in advance, and was not rejected at enqueue time,
         *  it means that we missed it for peeking, we need to drop it
         *
         *  Warning: unsigned arithmetic
         *      t_packet > t_current + TX_MAX_ADVANCE_DELAY
         */
        if (queue->nodes[i].pkt.count_us != 0 && (queue->nodes[i].pkt.count_us - time_us) >= TX_MAX_ADVANCE_DELAY) {
            /* We drop the packet to avoid lock-up */
            queue->num_pkt--;
            if (queue->nodes[i].pkt_type == JIT_PKT_TYPE_BEACON) {
                queue->num_beacon--;
                MSG_DEBUG(DEBUG_WARNING, "WARNING: --- Beacon dropped (current_time=%u, packet_time=%u) ---\n", time_us, queue->nodes[i].pkt.count_us);
            } else {
                MSG_DEBUG(DEBUG_WARNING, "WARNING: --- Packet dropped (current_time=%u, packet_time=%u) ---\n", time_us, queue->nodes[i].pkt.count_us);
            }

            /* Replace dropped packet with last packet of the queue */
            memcpy(&(queue->nodes[i]), &(queue->nodes[queue->num_pkt]), sizeof(struct jit_node_s));
            memset(&(queue->nodes[queue->num_pkt]), 0, sizeof(struct jit_node_s));

            /* Sort queue in ascending order of packet timestamp */
            jit_sort_queue(queue);

            /* restart loop  after purge to find packet to be sent */
            i = 0;
            continue;
        }
        /* 立即消息为次高级，从前向后发送
         * 当没有最高级数据时发送，次高级数据
         */
        if(queue->nodes[i].pkt.tx_mode == IMMEDIATE)
//       if(queue->nodes[i].pkt.count_us == 0)
        {
            if (idx_higher_priority == -1)
                idx_higher_priority = i;
        }
        /* Then look for highest priority packet to be sent:
         *  Warning: unsigned arithmetic (handle roll-over)
         *      t_packet < t_highest
         */
        else
        {
            if ((idx_highest_priority == -1) || (((queue->nodes[i].pkt.count_us - time_us) < (queue->nodes[idx_highest_priority].pkt.count_us - time_us))))
                idx_highest_priority = i;

        }
    }
    /* Peek criteria 1: look for a packet to be sent in next TX_JIT_DELAY ms timeframe
     *  Warning: unsigned arithmetic (handle roll-over)
     *      t_packet < t_current + TX_JIT_DELAY
     */
    if ((queue->nodes[idx_highest_priority].pkt.count_us - time_us) < TX_JIT_DELAY) {
        *pkt_idx = idx_highest_priority;
        MSG_DEBUG(DEBUG_JIT, "peek highest packet with count_us=%u at index %d\n",
            queue->nodes[idx_highest_priority].pkt.count_us, idx_highest_priority);
    }
    else if (idx_higher_priority != -1) {
        *pkt_idx = idx_higher_priority;
        MSG_DEBUG(DEBUG_JIT, "peek higher packet with count_us=%u at index %d\n",
            queue->nodes[idx_higher_priority].pkt.count_us, idx_higher_priority);
    }else {
        *pkt_idx = -1;
    } 
    pthread_mutex_unlock(&mx_jit_queue);

    return JIT_ERROR_OK;
}

void jit_print_queue(struct jit_queue_s *queue, bool show_all, int debug_level) {
    int i = 0;
    int loop_end;

    if (jit_queue_is_empty(queue)) {
        MSG_DEBUG(debug_level, "INFO: [jit] queue is empty\n");
    } else {
        pthread_mutex_lock(&mx_jit_queue);

        MSG_DEBUG(debug_level, "INFO: [jit] queue contains %d packets:\n", queue->num_pkt);
        MSG_DEBUG(debug_level, "INFO: [jit] queue contains %d beacons:\n", queue->num_beacon);
        loop_end = (show_all == true) ? JIT_QUEUE_MAX : queue->num_pkt;
        for (i=0; i<loop_end; i++) {
            MSG_DEBUG(debug_level, " - node[%d]: count_us=%u - type=%d\n",
                        i,
                        queue->nodes[i].pkt.count_us,
                        queue->nodes[i].pkt_type);
        }

        pthread_mutex_unlock(&mx_jit_queue);
    }
}