// HERE IS ONE OF ANTIRTOS CLASSes below : del_fQP (see the rest desc. on https://github.com/WeSpeakEnglish/ANTIRTOS )
/// @brief delayed functional pointers queue without parameters, only time delay parameter
class del_fQ{
private:
    int first;
    volatile int last;
    int length;
    unsigned long time;
    typedef void(*fP)(void);
    fP * fQueue;
    fP * del_fQueue;             // delayed functions
    bool * execArr;             //is need to be executed?
    unsigned long * execTime;    //execution time arr 
public:
    del_fQ(int sizeQ);
    ~del_fQ();
    int push_delayed(fP pointerF, unsigned long delayTime);
    int push(fP);
    void tick(void);       
    int pull(void);
};

del_fQ::del_fQ(int sizeQ){ // initialization of Queue
  fQueue = new fP[sizeQ];
  del_fQueue = new fP[sizeQ];
  execArr = new bool[sizeQ];
  execTime = new unsigned long[sizeQ];
  last = 0;
  first = 0;
  time = 0;
  for(unsigned int i = 0; i < sizeQ; i++){
    execArr[i] = false;
  }
  length = sizeQ;
};

del_fQ::~del_fQ(){ // initialization of Queue
  delete [] fQueue;
  delete [] del_fQueue;
  delete [] execArr;
  delete [] execTime;
};

int del_fQ::push_delayed(fP pointerF, unsigned long delayTime){ // push element from the queue
  
  bool fullQ = true;                                      // is Queue full?
     for(unsigned int i = 0; i < length; i++){
      if (!execArr[i] ){
       del_fQueue[i] = pointerF;                          // put pointer into exec queue 
       execArr[i] = true;                                 // true flag for execution
       execTime[i] = time + delayTime;                    //calc execution time, no worry if overload
       fullQ = false;
       break;
       }
  }
  if (fullQ) return 1;
  return 0;
};

void del_fQ::tick(void){
  static unsigned int i = 0 ;  //uses in search cycle every tick
   for(i=0; i < length; i++){
     if(execTime[i] == time)
      if(execArr[i]){
       push(del_fQueue[i]);  // bump into normal queue part of delayed Queue
       execArr[i] = false;
     }
   }
  time++;
}

int del_fQ::push(fP pointerF){ // push element from the queue
  if ((last+1)%length == first){
    return 1;
  }
  fQueue[last++] = pointerF;
  last = last%length;
  return 0;
};

int del_fQ::pull(void){ // pull element from the queue
  if (last != first){
  fQueue[first++]();
  first = first%length;
  return 0;
  }
  else{
   return 1;
  }
};

/// @brief delayed functional pointers queue with parameters
template <typename T>
class del_fQP {
private:
    int first;
    volatile int last;
    int length;
    unsigned int time;
    typedef void (*fP)(T);
    fP * FP_Queue;
    fP * del_FP_Queue;                  // delayed functions
    bool * execArr;                     //is need to be executed?
    unsigned int * execTime;            //execution time arr 
    T* PARAMS_array;
    T* delayed_PARAMS_array;
    int push(void (*pointerQ)(T), T parameterQ);
    
public:
    del_fQP(int sizeQ);
    ~del_fQP();
    int push_delayed(void (*pointerQ)(T), T parameterQ, unsigned int delayTime);
    void tick(void);
    int pull();
};

template <typename T>
del_fQP<T>::del_fQP(int sizeQ) {
    FP_Queue = new fP[sizeQ];
    del_FP_Queue = new fP[sizeQ];
    execArr = new bool[sizeQ];
    PARAMS_array = new T[sizeQ];
    delayed_PARAMS_array = new T[sizeQ];
    execTime = new unsigned int[sizeQ];
    last = 0;
    first = 0;
    time = 0;
    for(unsigned int i = 0; i < sizeQ; i++){
      execArr[i] = false;
    }
    length = sizeQ;
}

template <typename T>
del_fQP<T>::~del_fQP() {
    delete[] FP_Queue;
    delete[] del_FP_Queue;
    delete[] PARAMS_array;
    delete[] delayed_PARAMS_array;
    delete [] execArr;
    delete [] execTime;
}

template <typename T>
int del_fQP<T>::push(void (*pointerQ)(T), T parameterQ) {
    if ((last + 1) % length == first) return 1;
    FP_Queue[last] = pointerQ;
    PARAMS_array[last] = parameterQ;
    last = (last + 1) % length;
    return 0;
}

template <typename T>
int del_fQP<T>::push_delayed(void (*pointerQ)(T), T parameterQ, unsigned int delayTime) {
bool fullQ = true;                                              // is Queue full?
     for(unsigned int i = 0; i < length; i++){
      if (!execArr[i] ){
       del_FP_Queue[i] = pointerQ;                              // put function pointer into exec queue 
       delayed_PARAMS_array[i] = parameterQ;                    // put parameter into exec queue    
       execArr[i] = true;                                       // true flag for execution
       execTime[i] = time + delayTime;                          //calc execution time, no worry if overload
       fullQ = false;
       break;
       }
  }
  if (fullQ) return 1;
  return 0;
}

template <typename T>
void del_fQP<T>::tick(void){
  static unsigned int i = 0 ;  //uses in search cycle every tick
   for(i=0; i < length; i++){
     if(execTime[i] == time)
      if(execArr[i]){
       push(del_FP_Queue[i],delayed_PARAMS_array[i]);  // bump into normal queue part of delayed Queue
       execArr[i] = false;
     }
   }
  time++;
}

template <typename T>
int del_fQP<T>::pull() {
    fP pullVar;
    if (last != first) {
        T Params = PARAMS_array[first];
        pullVar = FP_Queue[first];
        first = (first + 1) % length;
        pullVar(Params);
        return 0;
    }
    else {
        return 1;
    }
}
//////////////////////////////////////////// HERE THE SIMLE TEST BELOW//////////////////////////////////
typedef 
struct pinOut{ // structure (index - pin number, logic - 1/0 = ON/OFF)
  int index;
  bool logic;
} pinout ;

del_fQP<pinout> Q1(8); // maximum 8 function pointers with parameters in queue

void writePin(pinout cmd){ // write a pin true =- ON
  
  digitalWrite(cmd.index, cmd.logic);

}

void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);

  TCCR1A = 0x00;  //Normal Mode
  TCCR1B = 0x00;  //TC1 is OFF
  TCNT1 = 0;
  OCR1A = 6250;  //0.1s delay; prescaler 256
  bitSet(TIMSK1, OCIE1A);   //local intterupt is active
  TCCR1B |= bit(CS12);   //Start TC1 with prescale 256
  
  Q1.push_delayed(writePin,{12,true},20); //yellow led ON after 2 sec. (0.1*20 = 2 seconds)
  Q1.push_delayed(writePin,{12,false},30); //yellow led OFF after 3 sec.
  Q1.push_delayed(writePin,{13,true},50); //red led ON after 5 sec.
  Q1.push_delayed(writePin,{13,false},80); //red led OFF after 8 sec.
 
}

void loop() {
  // put your main code here, to run repeatedly:
  Q1.pull(); // pull from the queue

}

ISR(TIMER1_COMPA_vect) // timer interrupt ticks one per 0.1 sec
{
  TCNT1 = 0;
  OCR1A = 6250;
  Q1.tick(); // execute tick method for make delayed functionality works
}