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

#include "litmus.h"
#include "spinlocks.h"

#define NR_RESOURCES sizeof(resource_mask_t)*8

#define MEASURE TRUE

int events;

#if MEASURE==TRUE
static inline long diff_ns(const struct timespec *s, const struct timespec *e)
{
	const int64_t ns_in_s = 1000000000LL;
	struct timespec temp = *e;
	temp.tv_sec -= s->tv_sec;
	temp.tv_nsec -= s->tv_nsec;
	if (temp.tv_nsec < 0) {
		--temp.tv_sec;
		temp.tv_nsec += ns_in_s;
	}
	return temp.tv_sec * ns_in_s + temp.tv_nsec;
}
#endif

void spin_init(spinlock_t *lock){
    lock->serving = 0;
    lock->next_ticket = 0;
}

void spin_lock(spinlock_t *lock){
    int ticket = __sync_fetch_and_add(&(lock->next_ticket), 1);
//    printf("%d:%d waiting for ticket %d serving %d\n",
//            __sync_fetch_and_add(&events,1), gettid(), ticket, lock->serving);
    while(lock->serving != ticket);
}

void spin_unlock(spinlock_t *lock){
    lock->serving += 1;
}

void rwrnlp_init(rwrnlp *lock)
{
    int i;
    events = 0;
    lock->wlocked     = 0;
    lock->wentitled   = 0;
    lock->unavailable = 0;
    lock->enqueue     = (spinlock_t*)(malloc(sizeof(spinlock_t)));
    lock->state       = (spinlock_t*)(malloc(sizeof(spinlock_t)));

    for(i = 0; i < NR_RESOURCES; i++){
        lock->whead[i]    = 0;
        lock->wtail[i]    = 0;
    }
    for(i = 0; i < NR_CPUS; i++){
        lock->enter[i] = 0;
        lock->leave[i] = 0;
        lock->curr[i]  = 0;
    }
    spin_init(lock->enqueue);
    spin_init(lock->state);
}

long rwrnlp_read_lock(rwrnlp *lock, resource_mask_t resources, int processor)
{
    request *req;
#if MEASURE==TRUE
    long overhead = 0;
    struct timespec now, last;
	clock_gettime(CLOCK_MONOTONIC, &last);
#endif
    
    enter_np();

    req = &lock->requests[processor][lock->curr[processor]];
    req->resources = resources;
    req->type      = read_req;
    req->status    = waiting;
    lock->enter[processor]+=1;

//    printf("%d:%d rwrnlp_read_lock %lu\n",__sync_fetch_and_add(&events,1), gettid(), req->resources);

    spin_lock(lock->state);
    if((req->resources & lock->unavailable) == 0){
        req->status = acquired;
    }
    spin_unlock(lock->state);

    if(req->status != acquired){
//        printf("%d:%d waiting to become entitled\n",__sync_fetch_and_add(&events,1), gettid());
#if MEASURE==TRUE
        clock_gettime(CLOCK_MONOTONIC, &now);
        overhead += diff_ns(&last,&now);
#endif

        while((req->resources & lock->wentitled) != 0);
#if MEASURE==TRUE
        clock_gettime(CLOCK_MONOTONIC, &last);
#endif
        req->status = entitled;
#if MEASURE==TRUE
        clock_gettime(CLOCK_MONOTONIC, &now);
        overhead += diff_ns(&last, &now);
#endif

//        printf("%d:%d waiting to acquire\n", __sync_fetch_and_add(&events,1), gettid());
        while((req->resources & lock->wlocked) != 0);
    }
#if MEASURE==TRUE
    else{
        //acquire, but measure overheads
        clock_gettime(CLOCK_MONOTONIC, &now);
        overhead += diff_ns(&last, &now);
    }
    return overhead;
#else
    return 0;
#endif
    
//    printf("%d:%d reader satisfied\n", __sync_fetch_and_add(&events,1), gettid());
}

long rwrnlp_write_lock(rwrnlp *lock, resource_mask_t resources, int processor)
{
    int r,i,start,end;
    request *req, *contender;
    resource_mask_t tmp = resources;
#if MEASURE==TRUE
    struct timespec now, last;
    long overhead = 0;
    clock_gettime(CLOCK_MONOTONIC, &last);
#endif

    enter_np();

//    printf("%d:%d rwrnlp_write_lock\n", __sync_fetch_and_add(&events,1), gettid());

    req = &lock->requests[processor][lock->curr[processor]];

    req->resources = resources;
    req->type = write_req;
    req->status = waiting;
    lock->enter[processor]+=1;

    spin_lock(lock->enqueue);
    r = ffsl(tmp);
    while(r != 0){
        r = r-1; // ffsl gives 1-indexed, since 0 is the return value for 0.
        lock->wqueue[r][lock->wtail[r]] = req;
        lock->wtail[r] = (lock->wtail[r] + 1) % NR_CPUS;
        tmp &= ~(1<<r);
        r = ffsl(tmp);
    }
    spin_unlock(lock->enqueue);

    tmp = resources;
    r = ffsl(tmp);
    while(r != 0){
        r = r-1;
//        printf("%d:%d write waiting to become head of %d\n", __sync_fetch_and_add(&events,1), gettid(), r);
#if MEASURE==TRUE
        clock_gettime(CLOCK_MONOTONIC, &now);
        overhead += diff_ns(&last, &now);
#endif
        while(lock->wqueue[r][lock->whead[r]] != req);
#if MEASURE==TRUE
        clock_gettime(CLOCK_MONOTONIC, &last);
#endif
        tmp &= ~(1<<r);
        r = ffsl(tmp);
    }

    spin_lock(lock->state);
    lock->unavailable |= req->resources;
    req->status = entitled;
    spin_unlock(lock->state);
    
    for(i = 0; i < NR_CPUS; i++){
        if(i != processor){
            end = lock->leave[i];
            contender = &lock->requests[i][lock->curr[processor]];
            start = lock->enter[i];
            if(start <= end ||
               contender->type == write_req ||
               contender->status == waiting ||
               (contender->resources & req->resources) == 0)
                continue;

//            printf("%d:%d waiting for reader on processor %d\n", __sync_fetch_and_add(&events,1), gettid(), i);

#if MEASURE==TRUE
            clock_gettime(CLOCK_MONOTONIC, &now);
            overhead += diff_ns(&last, &now);
#endif
            while(lock->leave[i] < start);
#if MEASURE==TRUE
            clock_gettime(CLOCK_MONOTONIC, &last);
#endif
        }
    }

    spin_lock(lock->state);
    lock->wentitled &= ~(req->resources);
    lock->wlocked |= req->resources;
    spin_unlock(lock->state);

//    printf("%d:%d writer satisfied\n", __sync_fetch_and_add(&events,1), gettid());
#if MEASURE==TRUE
    clock_gettime(CLOCK_MONOTONIC, &now);
    overhead += diff_ns(&last, &now);
    return overhead;
#else
    return 0;
#endif
}

long rwrnlp_read_unlock(rwrnlp *lock, int processor)
{
#if MEASURE==TRUE
    struct timespec now, last;
    clock_gettime(CLOCK_MONOTONIC, &last);
#endif
    lock->leave[processor] += 1;
    lock->curr[processor] = (lock->curr[processor] + 1) % 2;
//    printf("%d:%d rwrnlp_read_unlock\n", __sync_fetch_and_add(&events,1), gettid());
    exit_np();

#if MEASURE==TRUE
    clock_gettime(CLOCK_MONOTONIC, &now);
    return diff_ns(&last, &now);
#else
    return 0;
#endif
}

long rwrnlp_write_unlock(rwrnlp *lock, int processor)
{
    int r;
    request *req;
    resource_mask_t tmp;
#if MEASURE==TRUE
    struct timespec now, last;
    clock_gettime(CLOCK_MONOTONIC, &last);
#endif
    req= &lock->requests[processor][lock->curr[processor]];
    tmp = req->resources;

//    printf("%d:%d rwrnlp_write_unlock\n", __sync_fetch_and_add(&events,1), gettid());

    spin_lock(lock->state);
    r = ffsl(tmp);
    while(r != 0){
        r = r-1; // ffsl gives 1-indexed, since 0 is the return value for 0.
        lock->whead[r] = (lock->whead[r] + 1) % NR_CPUS;
        tmp &= ~(1<<r);
        r = ffsl(tmp);
    }
    lock->wlocked &= ~(req->resources);
    lock->unavailable &= ~(req->resources);
    spin_unlock(lock->state);

    lock->leave[processor] += 1;
    lock->curr[processor] = (lock->curr[processor] + 1) % 2;
    
//    printf("%d:%d write unlocked %lu\n", __sync_fetch_and_add(&events,1), gettid(), req->resources);
//    printf("unavailable %lu\nwentitled %lu\nwlocked %lu\n", lock->unavailable, lock->wentitled, lock->wlocked);
    exit_np();
#if MEASURE==TRUE
    clock_gettime(CLOCK_MONOTONIC, &now);
    return diff_ns(&last, &now);
#else
    return 0;
#endif
}