// HERE IS ONE OF ANTIRTOS CLASSes below : del_fQP (see the rest desc. on https://github.com/WeSpeakEnglish/ANTIRTOS )
/// @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;
  int logic;
  int period;
} pinout ;

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

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

  Q1.push_delayed(writePin,cmd,cmd.period);
}

void setup() {
  // put your setup code here, to run once:
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, 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,{5,HIGH, 5},5);      // period 1 sec. 
  Q1.push_delayed(writePin,{6,HIGH, 10},10);    // period 2 sec.
  Q1.push_delayed(writePin,{7,HIGH, 20},20);    // period 4 sec.
  Q1.push_delayed(writePin,{8,HIGH, 40},40);    // period 8 sec.
  Q1.push_delayed(writePin,{9,HIGH, 80},80);    // period 16 sec.
  Q1.push_delayed(writePin,{10,HIGH, 160},160); // period 32 sec.
  Q1.push_delayed(writePin,{11,HIGH, 320},320); // period 64 sec.
  Q1.push_delayed(writePin,{12,HIGH, 640},640); //period 128 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
}