/* Non-Blocking Concurrent Queue Algorithm 
 * from Michael and Scott
 * https://www.cs.rochester.edu/research/synchronization/pseudocode/queues.html.
 * Originally from "Simple, Fast, and 
 * Practical Non-Blocking and Blocking 
 * Concurrent Queue Algorithms", PODC96.
 * 
 * The free in the algorithm (setFree method 
 * in Dequeue in this code) is meant to 
 * represent a function putting the node back 
 * on to a locally-maintained special-use free 
 * list and not the partner to malloc.
 * http://blog.shealevy.com/2015/04/23/use-after-free-bug-in-maged-m-michael-and-michael-l-scotts-non-blocking-concurrent-queue-algorithm/#up1
 */
#include <civlc.cvh>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include <assert.h>

typedef struct pointer_t pointer_t;
typedef struct queue_t queue_t;
typedef struct node_t node_t;
typedef struct freeList freeList;

struct node_t;

struct pointer_t {
  node_t* ptr;
  int count;
};

struct node_t {
  int value;
  pointer_t next;
};

struct queue_t {
  pointer_t Head;
  pointer_t Tail;
};

struct freeList{ //sepcial-use free list for putting node 
  node_t *node;
  freeList *next;
};

freeList* list; //declare global list

void initialize(queue_t *Q) {
  node_t *node = (node_t*)malloc(sizeof(node_t));
  
  list=(freeList*)malloc(sizeof(freeList));
  list->node = NULL;
  list->next = NULL;
  node->next.ptr = NULL;         
  node->next.count = 0;	 
  Q->Head.ptr = Q->Tail.ptr = node; 
}

void setFree(node_t* freeNode){
  $atomic{
    freeList *temp = (freeList*)malloc(sizeof(freeList));
   
    temp->node = freeNode;
    temp->next = list->next;
    list->next = temp;
  }
}

void deallocate(freeList *list){
  freeList *q;
  
  while(list != NULL){
    q = list->next;
    free(list->node);
    free(list);
    list = q;
  }
}

_Bool equal(pointer_t p1, pointer_t p2){
  return (p1.ptr == p2.ptr) && (p1.count == p2.count);
}

_Bool CAS(pointer_t *dest, pointer_t oldval, pointer_t newval){
  $atomic {
    if (equal(*dest, oldval)) {
      *dest = newval;
      return true;
    }
    return false;
  }
}

void enqueue(queue_t *Q, int value) {
  pointer_t tail, next;
  node_t *node = (node_t*)malloc(sizeof(node_t));
  node->value = value;		
  node->next.ptr = NULL;
  
  while (true){					
    tail = Q->Tail;	  // Read Tail.ptr and Tail.count together
    next = tail.ptr->next; // Read next ptr and count fields together
    if (equal(tail, Q->Tail))  // Are tail and next consistent?
      // Was Tail pointing to the last node?
      if (next.ptr == NULL){
	// Try to link node at the end of the linked list
	if (CAS(&tail.ptr->next, next, (pointer_t){ node, next.count + 1 }))
	  break;		// **Enqueue is done.  Exit loop
      }
      else{   // Tail was not pointing to the last node
	// Try to swing Tail to the next node
	CAS(&Q->Tail, tail, (pointer_t){ next.ptr, tail.count + 1 });
      }
  }
  // Enqueue is done.  Try to swing Tail to the inserted node
  CAS(&Q->Tail, tail, (pointer_t){ node, tail.count + 1 });
}

_Bool dequeue(queue_t *Q, int *pvalue) {
  pointer_t head, tail, next;

  while (true){					
    head = Q->Head;		// Read Head
    tail = Q->Tail;	        // Read Tail
    next = head.ptr->next;     // Read Head.ptr->next
    if (equal(head, Q->Head)) // Are head, tail, and next consistent?
      if (head.ptr == tail.ptr){	        
	if (next.ptr == NULL)	// Is queue empty?
	  return false;		// Queue is empty, couldn't dequeue
	// Tail is falling behind.  Try to advance it
	CAS(&Q->Tail, tail, (pointer_t){ next.ptr, tail.count + 1 });
      }
      else{
	// Read value before CAS
	// Otherwise, another dequeue might free the next node
	*pvalue = next.ptr->value;
	if (CAS(&Q->Head, head, (pointer_t){ next.ptr, head.count + 1 }))
	  break;// **Dequeue is done.  Exit loop
      }
  }
  setFree(head.ptr); // It is safe now to "free" the old node
  return true;      // Queue was not empty, dequeue succeeded
}

/*****************************************************/
/******************** Tests **************************/
/*****************************************************/

/* Determines whether an array of n integers is 
 * a permutation of the numbers 0..n-1. */
_Bool is_permutation(int n, int *data) {
  _Bool seen[n];

  for (int i=0; i<n; i++)
    seen[i] = 0;
  for (int i=0; i<n; i++) {
    int value = data[i];

    if (value < 0 || value >= n)
      return 0;
    if (seen[value])
      return 0;
    seen[value] = 1;
  }
  return 1;
}

void test1() {
  int d;
  queue_t sq;
	
  initialize(&sq);
  for (int i = 0; i < 10; i++) {
    enqueue(&sq, i);
  }
  for (int i = 0; i < 10; i++) {
    _Bool result = dequeue(&sq, &d);
    
    assert(result);
    assert(d == i);
  }
  deallocate(list);
  free(sq.Head.ptr);
}

void test2(int n) {   //Test whether dequeued array is a permutation
  queue_t sq;
  int array[n];
  
  initialize(&sq);
  $parfor(int i: 0 .. (n-1)) {
    enqueue(&sq, i);
    dequeue(&sq, &array[i]);
  }
  assert(is_permutation(n, array));  
  deallocate(list);
  free(sq.Head.ptr);
}

void test3(int t, int n) {  //t is the number of threads,
  int RESULT[t*n];          //n is the number of enqueued values 
  int R[t][n];      //global array to store scaned result
  int counter[t];   //global array, each thread has a counter;
  queue_t sq;
  
  void thread(int tid){
    for(int i=0; i<n; i++) {
      enqueue(&sq, i+1+tid*n);
    }
  }
  void scan(int x){   // helper method for assertFIFO()
    int tid = 0;     // thread id
  
    tid = (x-1)/n;   //calculate the id of thread;
    R[tid][counter[tid]++] = x;  //store scaned elements
  }
  void assertFIFO(int *data) {  // assert method for testing FIFO
    for(int i=0; i<t*n; i++)
      scan(data[i]);  //scan dequeued RESULT[t*n];
    for(int i=0; i<t; i++)     //assert FIFO for each thread
      for(int j=0; j<n-1; j++)
	assert(R[i][j] < R[i][j+1]);
  }
  for(int i=0; i<t; i++)   //initialize global R[t][n]
    for(int j=0; j<n; j++)
      R[i][j] = 0;   
  for(int i=0; i<t;i++) ///initialize global counter[t]
    counter[i] = 0;
  for(int i=0; i<t*n; i++)
    RESULT[i] = 0;
  
  initialize(&sq);  
  $parfor(int i: 0 .. t-1)
    thread(i);
  for(int i=0; i<t*n; i++)  
    dequeue(&sq, &RESULT[i]); 
  for(int i=0; i<t*n; i++){ //print dequeued result
    if(i%(t*n)==0)
      printf("dequeue: ");
    printf("%d\t", RESULT[i]);
  } 
  printf("\n");
  assertFIFO(RESULT);
  deallocate(list);
  free(sq.Head.ptr);
}

void main() {
  test1();
  test2(2);
  test3(2, 3);
}