#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <pthread.h>
#include <sys/time.h>
#include <ucore/ucore_threadqueue.h>

/**
 * @param timeout a timeout duration
 * @param result the absolute time from now when the timeout expires
 */
static inline void uc_get_absolute_time(struct timespec *result, 
                                        const struct timespec *timeout) 
{
    struct timeval now;
    //TODO investigate using clock_gettime() instead of gettimeofday
    gettimeofday(&now, NULL);
    result->tv_sec = now.tv_sec + timeout->tv_sec;
    result->tv_nsec = (now.tv_usec * 1000) + timeout->tv_nsec;

    if (result->tv_nsec >= 1000000000) {
        result->tv_sec++;
        result->tv_nsec -= 1000000000;
    }
}

int uc_thread_queue_init(struct uc_threadqueue *queue, long max_elements)
{
    int rc = 0;
    if (queue == NULL || max_elements <= 0) {
        return EINVAL;
    }

    memset(queue, 0, sizeof(struct uc_threadqueue));

    rc = pthread_cond_init(&queue->read_cond, NULL);
    if (rc != 0) {
        return rc;
    }

    rc = pthread_cond_init(&queue->write_cond, NULL);
    if (rc != 0) {
        pthread_cond_destroy(&queue->read_cond);
        return rc;
    }

    rc = pthread_mutex_init(&queue->mutex, NULL);
    if (rc != 0) {
        pthread_cond_destroy(&queue->read_cond);
        pthread_cond_destroy(&queue->write_cond);
        return rc;
    }

    queue->max_elements = max_elements;
    queue->last = &queue->first;

    return rc;
}

int uc_thread_queue_tryadd(struct uc_threadqueue *queue, const struct timespec *timeout, struct uc_threadmsg *msg)
{
    int rc = 0;
    struct timespec abstimeout;

    if (queue == NULL || msg == NULL) {
        return EINVAL;
    }

    if (timeout) {
        uc_get_absolute_time(&abstimeout, timeout);
    }

    pthread_mutex_lock(&queue->mutex);

    //Wait if the queue is full
    while(queue->num_elements >= queue->max_elements && rc != ETIMEDOUT) {

        queue->num_write_waiters++;
        if(timeout) {
            //fastpath, check for a poll
            if(timeout->tv_sec == 0 && timeout->tv_nsec == 0) {
                rc = ETIMEDOUT;
            } else {
                rc = pthread_cond_timedwait(&queue->write_cond, &queue->mutex, &abstimeout);
            }

        } else {
            pthread_cond_wait(&queue->write_cond, &queue->mutex);
        }
        queue->num_write_waiters--;
    }

    if (rc == ETIMEDOUT) {
        pthread_mutex_unlock(&queue->mutex);
        return rc;
    }
 
    //insert the new element
    msg->next = NULL;
    if (msg->msgtype >= 0) { //add at the tail
        *(queue->last) = msg;
        queue->last = &msg->next;
    } else { //add at the head ("priority message")
        msg->next = queue->first;
        if (queue->first == NULL) {
            queue->last = &msg->next;
        }
        queue->first = msg;
    }

    queue->num_elements++;

    //signal blocked readers
	if (queue->num_read_waiters == 1) {
        //if there's just one thread waiting to pop elements
        //use _cond_signal. _cond_broadcast can be more expensive (os dependent)
		pthread_cond_signal(&queue->read_cond);
	} else if (queue->num_read_waiters > 1) {
        //signall all threads that there's items available.
		pthread_cond_broadcast(&queue->read_cond);
    }


    pthread_mutex_unlock(&queue->mutex);

    return 0;

}

int uc_thread_queue_tryget(struct uc_threadqueue *queue, const struct timespec *timeout, struct uc_threadmsg **msg)
{
    int rc = 0;
    struct timespec abstimeout;
    struct uc_threadmsg *first_msg;

    if (queue == NULL || msg == NULL) {
        return EINVAL;
    }

    if (timeout) {
        uc_get_absolute_time(&abstimeout, timeout);
    }

    pthread_mutex_lock(&queue->mutex);

    /* Will wait until awakened by a signal or broadcast */
    while (queue->first == NULL && rc != ETIMEDOUT) {  
        //Need to loop to handle spurious wakeups, or the case
        //another thread popped the remaining elment before we did
        queue->num_read_waiters++;
        if (timeout) {
            //fastpath, check for a poll
            if(timeout->tv_sec == 0 && timeout->tv_nsec == 0) {
                rc = ETIMEDOUT;
            } else {
                rc = pthread_cond_timedwait(&queue->read_cond, &queue->mutex, &abstimeout);
            }
        } else {
            pthread_cond_wait(&queue->read_cond, &queue->mutex);

        }
        queue->num_read_waiters--;
    }

    if (rc == ETIMEDOUT) {
        pthread_mutex_unlock(&queue->mutex);
        return rc;
    }

    //remove the first element
    first_msg = queue->first;
    queue->first = first_msg->next;
    if (queue->first == NULL)
        queue->last = &queue->first;
    first_msg->next = NULL;

    queue->num_elements--;

    //signal blocked writers
	if (queue->num_write_waiters == 1) {
        //if there's just one thread waiting to push elements
        //use _cond_signal. _cond_broadcast can be more expensive (os dependent)
		pthread_cond_signal(&queue->write_cond);
	} else if (queue->num_write_waiters > 1) {
        //signall all threads that there's items available.
		pthread_cond_broadcast(&queue->write_cond);
    }

    pthread_mutex_unlock(&queue->mutex);
    *msg = first_msg;

    return 0;
}

static void uc_thread_queue_walk_unlocked(struct uc_threadqueue *queue, uc_thread_queue_walk_func walk_func)
{
    struct uc_threadmsg *p;
    struct uc_threadmsg *next;
    for(p = queue->first; p; p = next) {
        next = p->next;
        walk_func(p);
    }
}

int uc_thread_queue_walk(struct uc_threadqueue *queue, uc_thread_queue_walk_func walk_func)
{
    if (queue == NULL || !walk_func) {
        return EINVAL;
    }

    pthread_mutex_lock(&queue->mutex);

    uc_thread_queue_walk_unlocked(queue, walk_func);

    pthread_mutex_unlock(&queue->mutex);

    return 0;
}

int uc_thread_queue_destroy(struct uc_threadqueue *queue, uc_thread_queue_walk_func free_func)
{
    int rc;

    if (queue == NULL) {
        return EINVAL;
    }

    pthread_mutex_lock(&queue->mutex);

     /* We don't always know if there's threads still using the queue,
      * e.g. blocking on the mutex.
     * It's up to the user to ensure consistency when destroying the queue.
     *
     * But if we *know* there are some threads still using the queue,
     * we can't destroy it.
     */
    if (queue->num_read_waiters != 0 || queue->num_write_waiters != 0) {
        pthread_mutex_unlock(&queue->mutex);
        return EBUSY;
    }

    if (free_func) {
        uc_thread_queue_walk_unlocked(queue, free_func);
    }

    pthread_mutex_unlock(&queue->mutex);
    rc = pthread_mutex_destroy(&queue->mutex);
    pthread_cond_destroy(&queue->read_cond);
    pthread_cond_destroy(&queue->write_cond);

    return rc;
}


long uc_thread_queue_length(struct uc_threadqueue *queue)
{
    long length;

    if (queue == NULL ) {
        return -EINVAL;
    }
    // get the length properly
    pthread_mutex_lock(&queue->mutex);
    length = queue->num_elements;
    pthread_mutex_unlock(&queue->mutex);

    return length;
}