#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <pthread.h>

/** TO COMPILE:
 *  - Select gcc 4.2.1+
 *  #> g++ -g3 -march=i686 cas.C -lpthread -o cas
 */

namespace LockFree {
   
#define CAS(a,b,c) __sync_bool_compare_and_swap(a,b,c)

enum StackType
{
   FIFO, /// First In, First Out
   LIFO  /// Last In, First Out
};

class Descriptor
{
   public: ///< structors
      Descriptor() : size(0), head(0), tail(0), counter(0) {;}

   public: ///< methods
      void Reset()
      {
         size = 0;
         head = 0;
         tail = 0;
         wd.Reset();
      }

      void Dump()
      {
         std::cout << "this [" << this << "] : size [" << size << "] head [" << head << "] tail [" << tail << "] counter [" << counter << "]" << std::endl;
         wd.Dump();
      }

   public: ///< data
      std::size_t size;
      std::size_t head;
      std::size_t tail;
      std::size_t counter;
      class WriteDescriptor
      {
         public: ///< structors
            WriteDescriptor() : old_value(0), new_value(0), pos(0), pending(false) {;}

         public: ///< methods
            void Reset()
            {
               old_value = 0;
               new_value = 0;
               pos = 0;
               pending = false;
            }
            
            void Dump()
            {
               std::cout << "   |--WD--> OV [" << old_value << "] NV [" << new_value << "] pos [" << pos << "] pending [" << pending << "]" << std::endl;
            }

         public: ///< data
            void*  old_value;
            void*  new_value;
            std::size_t pos;
            bool   pending;
      } wd;
};

template <std::size_t POOL_SIZE>
class PointerStack
{ 
   public:
      PointerStack(StackType _stackType=FIFO, std::size_t _allocUnit=10)
      : m_pDescriptor(0),
        m_stackType(_stackType),
        m_pMemory(0),
        m_allocUnit(_allocUnit),
        m_totalUnits(0),
        m_dPoolSize(POOL_SIZE)
      {
         m_descriptorPool[0].counter++; ///< we fake that the first descriptor is in use so we can use it ;)
         m_pDescriptor = &(m_descriptorPool[0]);
         //DumpDescriptorPool();
      }

      ~PointerStack()
      {
         delete m_pMemory;
         m_pMemory = 0;
         m_pDescriptor = 0;
      }

      bool Push(void* _pVoid)
      {
         CompleteWrite(m_pDescriptor);

         if ( Length() == m_totalUnits )
         {
            std::cout << "Memory Overflow : [" << Length() << "]" << std::endl;
            return false;
         }

         Descriptor* next(0);
         do
         {
            ///< Finding an empty descriptor we could use
            for (std::size_t idx=0; idx<m_dPoolSize; ++idx)
            {
               if (CAS(&m_descriptorPool[idx].counter, 0, 1))
               { ///< we successfully got a descriptor for ourselves
                  next = &m_descriptorPool[idx];
                  next->Reset();
                  (next->wd).new_value = _pVoid;
                  (next->wd).pending = true;
                  break;
               }
            }
         } while (!next);

         Descriptor* current(0);
         std::size_t counter(0);
         while(1)
         { ///< loop until we can add this descriptor to the stack
            if (current)
            { ///< we will get a new current, must decrease our interest on previous current
               do { counter = current->counter; } while (!CAS(&(current->counter), counter, counter-1));
            }

            current = m_pDescriptor; ///< safe to point to m_pDescriptor as it won't be reused until we remove our interest on it

            do { counter = current->counter; } while (!CAS(&(current->counter), counter, counter+1));

            if ( (current->head == current->tail) && Length() >= m_totalUnits )
            { ///< there is no space available to write
               next->Reset();
               CAS(&(next->counter), 1, 0);
               std::cout << "Memory Overflow : [" << Length() << "]" << std::endl;
               return false;
            }

            /// Preparing next with the current values
            (next->wd).old_value = At(current->head);
            (next->wd).pos = current->head;
            next->size = current->size+1;
            next->head = current->head+1;
            next->tail = current->tail;

            if ( next->head > m_totalUnits )
            { ///< wrap around so we can write more from the beginning
               next->head = 0;
               (next->wd).old_value = At(next->head);
               (next->wd).pos = next->head;
               next->head++;
            }

            if ( CAS(&m_pDescriptor, current, next) ) break;
         }
         CompleteWrite(next);
         do { counter = current->counter; } while (!CAS(&(current->counter), counter, counter-2) );
         return true;
      };

      void* Pop(void)
      {
         Descriptor* current(0);
         Descriptor* next(0);
         void* ptr(0);

         do
         {
            ///< Finding an empty descriptor we could use
            for (std::size_t idx=0; idx<m_dPoolSize; ++idx)
            {
               if (CAS(&m_descriptorPool[idx].counter, 0, 1))
               { ///< we successfully got a descriptor for ourselves
                  next = &m_descriptorPool[idx];
                  next->Reset();
                  break;
               }
            }
         } while (!next);

         std::size_t counter(0);
         do
         { ///< loop until we can update the stack with the removed item
            CompleteWrite(m_pDescriptor);

            if (current)
            { ///< we will get a new current, must decrease our interest on previous current
               counter = current->counter;
               while (!CAS(&(current->counter), counter, counter-1)) { counter = current->counter; }
            }
            counter = m_pDescriptor->counter;
            while (!CAS(&(m_pDescriptor->counter), counter, counter+1)) { counter = m_pDescriptor->counter; } ///< now we are interested in this current, increase our interesse on it
            current = m_pDescriptor; ///< safe to point to m_pDescriptor as it won't be reused until we remove our interest on it

            if ( 0 == current->size )
            {
               next->Reset();
               std::cout << "Pop : The queue is empty." << std::endl;
               return 0;
            }

            ///< Setting up next values based on current values
            next->size = current->size-1;
            next->head = current->head;
            next->tail = current->tail;

            switch(m_stackType) 
            {
            case FIFO:
               ptr = At( next->tail );
               next->tail++;
               if ( next->tail >= m_totalUnits )
               {
                  next->tail = 0;
               }
               break;
            case LIFO:
               next->head--;
               ptr = At(next->head);
               break;
            }
         } while (!CAS(&m_pDescriptor, current, next));
         counter = current->counter;
         CAS(&(current->counter), counter, counter-2);
         CompleteWrite(next); ///< attempt to write next (which is the new current) If it fails, don't worry, let the next thread worry.
         return ptr;
      };

      std::size_t Length()
      {
         if ( (m_pDescriptor->wd).pending )
         {
            if ( m_pDescriptor->size > 0 )
               return (m_pDescriptor->size - 1);
            else
               return 0;
         }
         return m_pDescriptor->size;
      };

      void* At(std::size_t idx)
      {
         if ( idx > m_totalUnits )
         { ///< an invalid index was given!
            return 0;
         }
         return m_pMemory[idx];
      }

      bool CompleteWrite(Descriptor* d)
      {
         if ( !(d->wd).pending ) return true;
         if ( CAS(&((d->wd).pending), true, false) )
         {
            if ( !CAS(&(m_pMemory[(d->wd).pos]), (d->wd).old_value, (d->wd).new_value) )
            {
               std::cout << "T[" << pthread_self() << "] Could not update Pos [" << (d->wd).pos << "] OV[" << m_pMemory[(d->wd).pos] << "] Expected OV[" << (d->wd).old_value << "] NV[" << (d->wd).new_value << "]" << std::endl;
               std::cout.flush();
               return false;
            }
         }
         return true;
      }

      /** Reserve can only be called once to create the memory.
       *  If we need to grow, the upper-layer would create a new stack of size(X+Y)
       *  and then all push would go to the new stack, all pop from the previous stack, until
       *  the previous stack is empty. Once the previous stack is empty, pop/unpop operations would
       *  use the new stack, and the previous stack would be deleted (or not).
       */
      void Reserve(std::size_t _numUnits)
      {
         m_totalUnits = _numUnits * m_allocUnit;
         m_pMemory = new void*[m_totalUnits];
         memset(m_pMemory, 0, m_totalUnits*sizeof(void*));
      }

      void DumpDescriptorPool()
      {
         std::cout << "Current [" << m_pDescriptor << "]" << std::endl;
         for (std::size_t idx=0; idx<m_dPoolSize; ++idx)
         {
            m_descriptorPool[idx].Dump();
         }
      }

      void DumpMemory(std::size_t begin=0)
      {
         using std::setfill;
         using std::setw;
         using std::endl;
   
         if ( !m_pMemory)
            return;
         
         Descriptor *current = m_pDescriptor;
         int size = m_totalUnits + 1;
         if ( size > 30 )
            size = 30;
   
         // new line
         std::cout << endl << endl << "PointerStack:[" << size-1 << "]," << " Available to Read:[" << Length() << "]," << " Tail:["
                  << current->tail << "]," << " Head:[" << current->head << "]" << endl << endl;
   
         //////////////////////////////////////////////////////////////////////
         // ptrs
         // TAIL
         for (int i=begin; i<begin+size; ++i)
         {
            if (current->tail == i)
            {
               std::cout << "|" << setw(5) << setfill(' ') << "T" << "|";
               break;
            }
            else
               std::cout << " " << setw(5) << setfill(' ') << " ";
         }
         std::cout << endl;
         // HEAD
         for (int i=begin; i<begin+size; ++i)
         {
            if (current->head == i)
            {
               std::cout << "|" << setw(5) << setfill(' ') << "H" << "|";
               break;
            }
            else
               std::cout << " " << setw(5) << setfill(' ') << " ";
         }
         std::cout << endl;
         //////////////////////////////////////////////////////////////////////
         // header
         for (int i=begin; i<begin+size; ++i)
            std::cout << "|" << setw(5) << setfill('0') << i;
         std::cout << "|" << endl;
         // line delimiter
         for (int i=0; i<size; ++i)
            std::cout << "+" << setw(5) << setfill('-') << "-";
         std::cout << "+" << endl;
   
         //////////////////////////////////////////////////////////////////////
         // body
         for (int i=begin; i<begin+size-1; ++i)
         {
            if (m_pMemory)
               std::cout << "|" << setw(5) << setfill('0') << *(int*)m_pMemory ;
            else
               std::cout << "|" << setw(5) << setfill('0') << 0;
         }
         std::cout << "|" << endl;
         // line delimiter
         for (int i=begin; i<begin+size; ++i)
            std::cout << "+" << setw(5) << setfill('-') << "-";
         std::cout << "+" << endl << endl;
      }

   protected:
      Descriptor*  m_pDescriptor;                ///< use for lock-free algorithm (insures atomic operations on the stack)
      StackType    m_stackType;                  ///< the type of this stack (LIFO or FIFO)

      void**       m_pMemory;                    ///< internal memory buffer pointer
      std::size_t  m_allocUnit;                  ///< size of a memory unit
      std::size_t  m_totalUnits;                 ///< how many units we have available
      const std::size_t m_dPoolSize;             ///< size of our descriptor pool
      Descriptor   m_descriptorPool[POOL_SIZE];  ///< our pool of descriptors
};
} ///< namespace LockFree

LockFree::PointerStack<10> lockfreeStack(LockFree::FIFO,10);

extern "C"
{ 
   std::size_t count(0);

   void* Dequeue(void* data)
   {
      struct timeval tval_ini ;
      struct timeval tval_end ;
      struct timezone tzp ;

      const std::size_t numMsgs(*reinterpret_cast<std::size_t*>(data));
      pthread_t tid = pthread_self();

      std::size_t msgsPopped(1);

      gettimeofday (&tval_ini, &tzp) ;

      while (msgsPopped < numMsgs)
      {
         while (lockfreeStack.Length() == 0 )
         {
            sleep(1);
         }
         while(lockfreeStack.Length() > 0)
         {
            std::size_t* value = reinterpret_cast<std::size_t*>(lockfreeStack.Pop());
            if (value)
            {
               //std::cout << "[" << tid << "] : Popped [" << msgsPopped << "] = [" << *value << "]" << std::endl;
               //std::cout.flush();
               ++msgsPopped;
            }
            else
            {
               std::cout << "[" << tid << "] : Could not pop msg idx [" << msgsPopped << "]" << std::endl;
               std::cout.flush();
            }
         }
      }
      gettimeofday (&tval_end, &tzp) ;
      double delta = ((double)tval_end.tv_sec * 1000000 + tval_end.tv_usec) - ((double)tval_ini.tv_sec * 1000000 + tval_ini.tv_usec);
      std::cout << "Delta [" << delta/(msgsPopped-1) << "] (Msgs [" << (msgsPopped-1) << "])" << std::endl ;
      return 0;
   }

   void* Enqueue(void* data)
   {
      const std::size_t numMsgs(*reinterpret_cast<std::size_t*>(data));
      pthread_t tid = pthread_self();

      std::size_t* array = new std::size_t[numMsgs];
      for ( std::size_t i=1; i<=numMsgs; ++i )
      {
         std::size_t oldcount = count;
         while ( !CAS(&count, oldcount, oldcount+1) ){ oldcount = count; }
         array[i-1] = oldcount+1;
      }

      for (std::size_t i=0; i<numMsgs; ++i)
      {
         //std::cout << "[" << tid << "] : Pushing [" << array << "]" << std::endl;
         //std::cout.flush();
         if ( !lockfreeStack.Push(&array) )
            std::cout << "TID [" << tid << "] Pushing [" << array << "] failed." << std::endl;
      }

      //delete [] array;
      return 0;
   }
}

int main(int argc, char *argv[])
{
   int rc(0);
   const std::size_t numEnqueuerThreads(3);
   lockfreeStack.Reserve(20000);
   std::size_t numMessagesPushed(1000);
   std::size_t numMessagesPopped(numMessagesPushed*numEnqueuerThreads);
   pthread_t enqueuerThreads[numEnqueuerThreads];
   pthread_t dequeuerThread;
   
   struct timeval tval_ini ;
   struct timeval tval_end ;
   struct timezone tzp ;

   gettimeofday (&tval_ini, &tzp);
   for (std::size_t i=0; i<numEnqueuerThreads; ++i)
   {
      /** Creating the enqueuer thread now */
      int rc = pthread_create(&enqueuerThreads, 0, Enqueue, &numMessagesPushed);
      if (rc)
      {
         std::cout << "ERROR; return code from pthread_create() is [" << rc << "] = [" << strerror(rc) << "]" << std::endl;
         exit(-1);
      }
   }
   void* thread_result;
   for (std::size_t i=0; i<numEnqueuerThreads; ++i)
   {
      pthread_join(enqueuerThreads, &thread_result);
   }
   gettimeofday (&tval_end, &tzp);
   std::size_t length(0);
   length = lockfreeStack.Length();
   double delta = ((double)tval_end.tv_sec * 1000000 + tval_end.tv_usec) - ((double)tval_ini.tv_sec * 1000000 + tval_ini.tv_usec);
   std::cout << "Global Delta [" << delta/length << "] (Msgs [" << length << "])" << std::endl ;
   for ( std::size_t i=1; i<=length; ++i)
   {
      std::size_t* value = reinterpret_cast<std::size_t*>(lockfreeStack.Pop());
      /*
      if (value)
         std::cout << "Popped [" << i << "] = [" << *value << "]" << std::endl;
      */
      if (!value)
         std::cout << "ERROR: Popped [" << i <<"] = [0]" << std::endl; 
   }
   return 0;
}