#ifndef ENVIRONNEMENT_QUEUE_SORT_QUEUE_H
#define ENVIRONNEMENT_QUEUE_SORT_QUEUE_H

#include <stdint.h>
#include <iostream>
#include "Parameters.h"
#include "Queue.h"

namespace environnement {
namespace queue {
  
  /*
   * Cicurlar queue, sort by delay
   */
  
  template <class T>
  class Sort_Queue : public Queue<T>
  {
    // *****[ variables ]*****
  protected : uint32_t     _ptr_read;  // pointer of the next slot to read
  protected : uint32_t     _ptr_write; // pointer of the next slot to write
      
      // *****[ constructor ]*****
    public : Sort_Queue (std::string name,
			 Parameters * param) : Queue <T> (name,param->_size)
      {
      };
      
      // *****[ destructor ]*****
    public : ~Sort_Queue ()
      {
      };
      
      // *****[ reset ]*****
      // Reset the queue in the empty state
    public : void reset ()
      {
	_ptr_read  = 0;
	_ptr_write = 0;
	Queue <T>::_nb_slot   = 0;
      };
      
      // *****[ transition ]*****
      // Decrease the delay at all cycle
    public : void transition ()
      {
	for (uint32_t it = 0; it < Queue <T>::_nb_slot; it ++)
	  {
	    uint32_t ptr = (_ptr_read + it)%Queue <T>::_size;
	    
	    if (Queue <T>::_slot[ptr]._delay != 0)
	      Queue <T>::_slot[ptr] ._delay --;
	  }
      }
      
      // *****[ read ]*****
      // return the n-eme slot.
      // (first = 0)
    public : slot_t<T> read (uint32_t num)
      {
	if (num >= Queue <T>::_nb_slot)
	  {
	    std::cerr << "<Sort_Queue.read> {ERROR} can't read because : num (" << num << ") >= nb_slot (" << Queue <T>::_nb_slot << ")" << std::endl;
	    exit(1);
	  }

	return Queue <T>::_slot[(_ptr_read + num)%Queue <T>::_size];
      };
      
      // *****[ pop ]*****
      // read the queue, and update the pointer
    public : T pop  ()
      {
	return pop (0);
      }

      // *****[ pop ]*****
      // read the queue, and update the pointer
    public : T pop  (uint32_t num)
      {
	if (num >= Queue <T>::_nb_slot)
	  {
	    std::cerr << "<Sort_Queue.pop> {ERROR} can't read because : num (" << num << ") >= nb_slot (" << Queue <T>::_nb_slot << ")" << std::endl;
	    exit(1);
	  }

	T val = Queue <T>::_slot [(_ptr_read+num)%Queue <T>::_size]._data;
	
	// Reorganize the queue

 	for (uint32_t it = 0; it < num; it ++)
	  {
	    uint32_t ptr      = (_ptr_read + num - it   )%Queue <T>::_size;
	    uint32_t _ptr_next = (ptr-1)%Queue <T>::_size;

	    Queue <T>::_slot [ptr] = Queue <T>::_slot [_ptr_next];
	  }

	Queue <T>::_nb_slot --;
	_ptr_read = (_ptr_read+1)%Queue <T>::_size;
	
	return val;
      }
      
      // *****[ push ]*****
      // Push a new value (they must have a slot free)
      // Is sort by delay
    public : bool push (uint32_t delay, T val)
      {
	// If full -> quit
	if (Queue <T>::_nb_slot == Queue <T>::_size)
	  return false;
	
	uint32_t ptr = _ptr_write;
	
	// Scan the fill to find the good position (keep the sort)
	for (uint32_t it = 0; it < Queue <T>::_nb_slot; it ++)
	  {
	    uint32_t _ptr_scan = (ptr-1)%Queue <T>::_size;
	    
	    if (Queue <T>::_slot[_ptr_scan]._delay <= delay)
	      break; //find
	    
	    // reformor the queue
	    Queue <T>::_slot [ptr] = Queue <T>::_slot [_ptr_scan];
	    ptr                  = _ptr_scan;
	  }
	
	Queue <T>::_slot [ptr] = slot_t <T> (delay,val);
	_ptr_write            = (_ptr_write+1)%Queue <T>::_size; // update the pointer
	Queue <T>::_nb_slot ++;
	
	return true;
      }

      // *****[ print ]*****
    public    : friend std::ostream& operator<< (std::ostream& output, const Sort_Queue & x)
      {
	output << "<" << x._name << ">" << std::endl;
	output << " * ptr_read    : " << x._ptr_read  << std::endl;
	output << " * ptr_write   : " << x._ptr_write << std::endl;
	output << " * nb_slot     : " << x._nb_slot   << std::endl;
	output << " * size        : " << x._size      << std::endl;
	
	for (uint32_t it = 0; it < x._nb_slot; it ++)
	  {
	    uint32_t ptr = (x._ptr_read+it)%x._size;
	    output << x._slot [ptr] << std::endl;
	  }
	
	return output;
      };
    };
  
};
};
#endif