import std.concurrency;
import std.datetime;
import std.array;
import std.format;
import std.stdio;
import std.algorithm;
import std.math;

template Queues (T) 
{ 
  struct Package 
  {
    uint sender_id;
    uint sender_sequence;
    Ticks pTime;
    T dummy;
  }
  
  class Node(U) 
  {
    U pkg;
    Node!(U) next;
  }

  alias Node!(Package) PNode;

  
  
  mixin template InjectQBoilerplate()
  {
  private:
    static Tid receiver_Tid; // NB: this is per thread so must be set on *each* sending thread
  public:
    static void set_receiver_Tid(Tid t) { receiver_Tid = t; }
    alias T Data_Type; // so that given a Queue we can reach in and figure out package types
  }
  

  
  private class Trivial_Allocator
  {
    static string short_name() { return "TA"; }
    static string description() { return "(De)Allocate via (GC)new"; }
    static void initialize() {}
    static void FreeNode(shared PNode p) {}
    static shared(PNode) GetNode() 
    {
      return new shared(PNode);
    }
  }

  
  // no need to synchronize bec the user is synchronized  
  private class FreeList_Allocator 
  {
  private:
    static shared PNode head;
    
  public:
    static string short_name() { return "FL"; }
    static string description() { return "Recycle used nodes via freelist."; }
    shared static this() { head = new shared(PNode); }
    static void initialize() { head = new shared(PNode); }
    
    
    static void FreeNode(shared PNode n) shared {
      debug(5) { writefln("FreeNode Called()\n"); }
      n.next = head.next;
      head.next = n;
    }
    
    static shared(PNode) GetNode() shared {
      debug(5) { writefln("GetNode Called()"); }
      scope (success) { debug(5) { writefln("GetNode Succedded()"); } }
      auto n = head.next;
      if (n is null)
        return new shared(PNode);
      head.next = n.next;
      n.next = null;
      return n;
    }
  }
  
  private class Locking_FreeList_Allocator 
  {
  private:
    static shared PNode head;
    static shared Object lock_obj;
  public:    
    static string short_name() { return "FL"; }
    static string description() { return "Recycle used nodes via freelist."; }
    
    shared static this() { 
      head = new shared(PNode); 
      lock_obj = new shared(Object);      
    }
    
    static void initialize() 
    { 
      synchronized(lock_obj) {
        head = new shared(PNode); 
      }
    }
    
    
    static void FreeNode(shared PNode n) shared {
      debug(5) { writefln("FreeNode Called()\n"); }
      synchronized (lock_obj) {
        n.next = head.next;
        head.next = n;
      }
    }
    
    static shared(PNode) GetNode() shared {
      debug(5) { writefln("GetNode Called()"); }
      scope (success) { debug(5) { writefln("GetNode Succedded()"); } }
      
      shared PNode n;
      synchronized (lock_obj) {
        n = head.next;
        if (n is null)
          return new shared(PNode);
        head.next = n.next;
      }
      n.next = null;
      return n;
    }
  }
  
  
  synchronized class CSync_LL_FIFO(Alloc)
  {    
    mixin InjectQBoilerplate;
    
  private:
    static shared PNode head, tail; // global
  public:
    static void initialize() { 
      Alloc.initialize();
      head = tail = Alloc.GetNode(); 
    }
    
    static string short_name() { return "SC,LL,F," ~ Alloc.short_name; }
    static string description() { return "Synchronized class, Linked List Storage, FIFO, " ~ Alloc.description; }
    
    
    static void Enqueue(Package p) // pushfront
    {
      debug (7) { writefln("Entering CSync_LL_FIFO_FL.Enqueue()"); }
      auto pn = Alloc.GetNode();
      pn.pkg = p;
      head.next = pn;
      head = pn;
    }
    
    static bool Dequeue(out Package p) // popback
    {
      if (head is tail) 
        return false;
      auto tmp = tail;
      tail = tail.next;
      p = tmp.next.pkg;
      Alloc.FreeNode(tmp);
      return true;
    }
  }

  alias CSync_LL_FIFO!(Trivial_Allocator) CSync_LL_FIFO_TA;
  alias CSync_LL_FIFO!(FreeList_Allocator) CSync_LL_FIFO_FL;
  
  class ISync_LL_FIFO(Alloc) 
  {
    mixin InjectQBoilerplate;
    
  private:
    static shared PNode tail, head; // global
    static shared Object lock_obj;
    shared static this() 
    {
      lock_obj = new shared(Object);
    }
  public:
    static void initialize() { 
      Alloc.initialize();
      head = tail = Alloc.GetNode(); 
    }
    
    static string short_name() { return "IS,LL,F," ~ Alloc.short_name; }
    static string description() { return "Internally Synchronized, Linked List Storage, FIFO, " ~ Alloc.description; }
    
    
    static void Enqueue(Package p) // pushfront
    {
      debug (7) { writefln("Entering ISync_LL_FIFO_FL.Enqueue()"); }

      auto pn = Alloc.GetNode();
      pn.pkg = p;
      synchronized (lock_obj) {
        head.next = pn;
        head = pn;
      }
    }
    
    static bool Dequeue(out Package p) // popback
    {
      debug (7) { writefln("Entering ISync_LL_FIFO_FL.Dequeue()"); }
      
      shared PNode tmp;
      synchronized (lock_obj) {
        if (head is tail) 
          return false;
        tmp = tail;
        tail = tail.next;
        p = tmp.next.pkg;
      }
      Alloc.FreeNode(tmp);
      return true;
    }
  }

  alias ISync_LL_FIFO!(Trivial_Allocator) ISync_LL_FIFO_TA;
  alias ISync_LL_FIFO!(Locking_FreeList_Allocator) ISync_LL_FIFO_FL;
  
  
  private class LockFree_FreeList 
  {
  private:
    static shared PNode head;
    
  public:
    shared static this() { head = new shared(PNode); }
    
    static void FreeNode(shared PNode n) {
      
      debug(5) { printf("FreeNode Called()\n"); }
      scope (success) { debug(5) { printf("FreeNode Succedded()\n"); } }
      do {
        n.next = head.next;
      } while (!cas(&head.next,n.next,n)); 
    }
    
    static shared(PNode) GetNode() {
      
      debug(5) { printf("GetNode Called()\n"); }
      scope (success) { debug(5) { printf("GetNode Succedded()\n"); } }
      shared PNode n;
      do {
        n = head.next;
        if (n is null)
          return new shared(PNode);
      } while (!cas(&head.next,n,n.next));
      n.next = null;
      return n;
    }
  }


  class HPRecType { 
  public:
    immutable int K = 2; // available HPs/thread
    
    // shared static (global) members   
  private:
    shared static HPRecType head_; // global
    shared static int listLen_ = 0; // global
    shared static int R_ = 10; // global
    

    // per Hazard Pointer
    shared HPRecType next_;
    shared int active_;    
    
  public:
    shared PNode hazard_;
    

    static shared(HPRecType) head() { return head_; } 
    static int listLen() { return listLen_; }
    static int R() { return R_; } 
    static void setR(int r) { R_ = r; } 

    static shared(HPRecType) Acquire() {
      debug (7) {
        writefln("->HPRecType.Acquire()");
      }
      
      //try to recycle an old one
      shared(HPRecType) p = head_;
      for (; (p !is null) ; p = p.next_) {
        if (p.active_ || !cas(&p.active_,0,1)) continue;
	debug (7) { writefln("Acquired old hp @ %s.",cast(const(void*))p); }  
        return p; // not active and then cas succeeded marking it active
      }
      debug(7) writefln("HPRecType.Acquire(): cannot recycle an HP.  Making new.");

      // we need a new one, increment list length
      int oldLen;
      do {
        oldLen = listLen_;
      } while (!cas(&listLen_,oldLen,oldLen+1));
      //listLen_changed_ = true;
      debug(7) writefln("HPRecType.Acquire(): Incremented listLen_.");
      p = new shared(HPRecType); 
      p.active_ = 1; // it's in use
      p.hazard_ = cast(PNode)null;
      shared(HPRecType) old;
      do { // add it to list
        old = head_;
        p.next_ = old;
      } while (!cas(&head_,old,p));      
      debug (7) { writefln("Acquired new hp @ %s.",cast(const(void*))p); }  
      return p;
    }

    static void Release(shared HPRecType p) {
      p.hazard_ = cast(shared PNode)null;
      p.active_ = 0; // so it can be re-used
      debug (7) { writefln("Released hp @ %s.",cast(const(void*))p); }  
    }
  }

  alias Appender!(shared(PNode)[],shared(PNode)) NodeList;
  //alias shared(Node)*[] NodeList;
  
  
  class HPLF_FreeList {
  public:
    /*
      shared static HPRecType[K][] HPList; // global
      static HPRecType HP[K]; // one per thread
      static this() { 
      foreach(k; 0..HPRecType.K) HP[k] = new HPRecType;
      HPList ~= HP; 
      // now we can loop over these
    */

  private:
    
    static NodeList rlist; // thread local
    shared static PNode head; // is shared  
    

    // once per thread
    static this() { 
      rlist.reserve(HPRecType.R());
    }

  public:
    shared static this() { head = new shared(PNode); }
    
    static shared(PNode) GetNode() 
    { 
      shared PNode n;
      do {
        n = head.next;
        if (n is null) 
          return new shared(PNode);
      } while (!cas(&head.next,n,n.next));
      debug (7) writefln("Returning reclaimed node @ %s",cast(const(void*))n);
      n.next = null;
      return n;
    }
    
    
    static void FreeNode(shared PNode n) 
    {
      debug (7) writefln("FreeNode @ %s",cast(const(void*))n);
      rlist.put(n); 
      if (rlist.data().length >= HPRecType.R())
        Scan();
    }
 

 private:
    // now we add it back to list
    static void ReclaimNode(shared PNode n) 
    {
      debug (7) writefln("Reclaiming node @ %s",cast(const(void*))n);
      do {
        n.next = head.next;
      } while (!cas(&head.next,n.next,n));
    }
    
    static void Scan() 
    {
      debug(8) writefln("HP Scan.");
      debug (2) auto sTime = systime();
      
      
      // stage 1
      NodeList hp; // thread local
      hp.reserve(HPRecType.listLen());
      shared HPRecType h = HPRecType.head;
      while (h !is null) {
        shared PNode n = h.hazard_;
        if (n !is null) { 
          hp.put(n);
        }
        h = h.next_;
      }
        
      debug (8) {
        {
          
        auto a = appender!string;
        
        a.put("Pre-sort hazard pointers: ");
        foreach (np; hp.data)
          formattedWrite(a,"%s ",cast(const(void *))np);
        a.put("\n");
        
        a.put("Pre-sort rlist pointers: ");
        foreach (np; rlist.data)
          formattedWrite(a,"%s ",cast(const(void *))np);
        writefln("%s",a.data);
        } 
      }

      auto old_rlist_length = rlist.data.length;
      
      NodeList tmp;
      tmp.reserve(old_rlist_length);

      
      static bool PNodeLessByObjectAddress(in PNode a, in PNode b)
      {
	return cast(const(void*))a < cast(const(void*))b;
      }

      
      // All the casting here should be unnecessary, fixed in next version of D?
      PNode[] sorted_hp = cast(PNode[])hp.data.dup;
      PNode[] sorted_rl = cast(PNode[])rlist.data.dup;
      
      sort!(PNodeLessByObjectAddress)(sorted_hp);
      sort!(PNodeLessByObjectAddress)(sorted_rl);

      
      debug (8) {
        {  
          auto a = appender!string;
          
          a.put("Post-sort hazard pointers: ");
          foreach (np; sorted_hp)
            formattedWrite(a,"%s ",cast(const(void *))np);
          a.put("\n");
          
          a.put("Post-sort rlist pointers: ");
          foreach (np; sorted_rl)
            formattedWrite(a,"%s ",cast(const(void *))np);
          writefln("%s",a.data);
        }
        
      }

	
      
      foreach (np; setIntersection!(PNodeLessByObjectAddress)(sorted_rl,sorted_hp))
        tmp.put(cast(shared(PNode))np);
      
      foreach (np; setDifference!(PNodeLessByObjectAddress)(sorted_rl,sorted_hp))
        ReclaimNode(cast(shared(PNode))np);

      rlist=tmp;
      

      HPRecType.setR(cast(int)lrint(3*HPRecType.listLen()));

      debug (2) {    
        auto eTime = systime();
        auto elapsed = (eTime - sTime).toMicroseconds!(double);
        int reclaimed = old_rlist_length - rlist.data.length;
        if (reclaimed < old_rlist_length)
          printf("HP Scan Complete (%f us). Reclaimed %i of %i\n",elapsed, reclaimed, old_rlist_length);
      }
    }
  }
  
  class FIFO_LinkedList_LockFree_Queue
  {
    mixin InjectQBoilerplate;
  private:
    static shared PNode first,last;
        
  public:
    static void initialize() { 
      first = last = HPLF_FreeList.GetNode(); 
    }

    shared static this() { initialize(); }
        
    static string short_name() { return "F,LL,LF"; }
    static string description() { return "FIFO: Lock Free (using cas & HPs), using a linked list as storage."; }
    
    
    static void Enqueue(in Package p)
    {
      debug (7) {
        writefln("Entering FIFO_LinkedList_LockFree_Queue.Enqueue()."); 
        scope (success) { writefln("Succeeded FIFO_LinkedList_LockFree_Queue.Enqueue()."); }
      }
      
      shared HPRecType hp = HPRecType.Acquire();
      scope (exit) { HPRecType.Release(hp); }
      
      shared PNode oldLast,oldNext;

      auto n = HPLF_FreeList.GetNode(); // get a new or reclaimed one
      n.pkg = p;
      
      bool updatedNewLink = false;
      while (!updatedNewLink) {
        debug (7) writefln("in loop");
        oldLast = last;
        hp.hazard_ = oldLast; // protect oldLast
        if (last !is oldLast) continue;
        debug (7) writefln("hp attached");
        oldNext = oldLast.next;
        if (last is oldLast) {  // still consistent?
          if (oldNext is null) { // yes, so far. Are we at the actual tail?
            updatedNewLink = cas(&last.next,cast(PNode)null,n); // yes.  Then try to add new node
	    debug (7) updatedNewLink ? writefln("at tail, added new node. last=%s; last.next=%s; n.next=%s",cast(const(void*))last,cast(const(void*))last.next,cast(const(void*))n.next) : writefln("at tail, failed to new node.");   
          }
          else { 
            auto fixed = cas(&last,oldLast,oldNext); // no. lagging tail, try to fix
            debug (7) fixed ? writefln("fixed lagging tail. last=%s (was %s), last.next=%s",cast(const(void*))last,cast(const(void*))oldLast,cast(const(void *))last.next) : writefln("Failed to fix lagging tail");
          }
        }
      }
      auto updated_last = cas(&last,oldLast,n); // success! Now, try to update last, if we fail, next produce() will fix
      debug (7) updated_last ? writefln("Updated last. last=%s",cast(const(void*))last) : writefln("Failed to update last");
    } 
    

    static bool Dequeue(out Package result) 
    {
      debug (7) 
        {
          writefln("Entering FIFO_LinkedList_Lockfree.Dequeue()");
          scope (success) writefln("FIFO_LinkedList_Lockfree.Dequeue() succeeded.");
        }
      
      shared PNode oldFirst;
      {
        
        shared HPRecType hp0 = HPRecType.Acquire();
        scope (exit) { HPRecType.Release(hp0); }
        
        shared HPRecType hp1 = HPRecType.Acquire();
        scope (exit) { HPRecType.Release(hp1); }
        
        bool haveAdvancedFirst = false;
        while (!haveAdvancedFirst) {
          oldFirst = first;
          hp0.hazard_ = oldFirst;
          if (oldFirst != first) continue;
          shared PNode oldLast = last;
          shared PNode oldFirstNext = oldFirst.next;
          hp1.hazard_ = oldFirstNext;
          if (oldFirst == first) { 
            if (oldFirst == oldLast) {
              if (oldFirstNext is null) {
                return false; // HPs freed
              }
              cas(&last,oldLast,oldFirstNext);
            }
            else {
              result = oldFirstNext.pkg;
              haveAdvancedFirst = cas(&first,oldFirst,oldFirstNext);
            }
          }
        }
      } // if we get here, HP's have been freed, I think.??
      debug (7) writefln("About to FreeNode(%s)",cast(const(void*))oldFirst);
      HPLF_FreeList.FreeNode(oldFirst);
      return true;    
    }
  }
  

  class MP_FIFO 
  {    
    static void initialize() {}
    static string short_name() { return "MP,,F"; }
    static string description() { return "Message Passing, Unknown storage, FIFO"; }
    
    mixin InjectQBoilerplate;
    
    static void Enqueue(Package p) // pushfront
    {
      debug (7) { writefln("sending"); }
      send(receiver_Tid,p);
      debug (7) { writefln("sent"); }
    }
    
    static bool Dequeue(out Package p) // popback
    { 
      debug (7) { writefln("Waiting to receive:"); }
        receive((Package pkg) { p=pkg; });
        debug (7) { writefln("rec'd!"); }
        return true;
    }
  }
  
  string package_tostring(Package p) {
    auto a = appender!string();
    formattedWrite(a,"P: time=%s; sender_id=%s; sender_seq=%s",p.pTime.value,p.sender_id,p.sender_sequence);
    return a.data;
  }
}