Multiple Software TImers

/** TIMER ARRAYS **/
#define MAX_TIMERS 255

int Timer_Counter = 0;
typedef struct{
    long Duration;
    long Start;
    int PIN;
    uint16_t BUFFER_PIN[1] = {0x0000};
    boolean State;
    String ID;
    uint16_t Address;
    boolean Is_On = false;
}Timer_Struct;

Timer_Struct Timers[MAX_TIMERS];

/** TIMER ARRAYS **/

static const inline void Timer_Stop(int Nth){
    Timers[Nth].Is_On = false;  // I hope thats enough to turn off the timer of that pin
}

static const inline void Timer_Checker_Loop(){
    if(Timers[Timer_Counter].Is_On == true){    // Let's loop trought the array where the timer properties are ( check if on )
        if(millis() - Timers[Timer_Counter].Start >= Timers[Timer_Counter].Duration){
            // timer is fired at this point, the duration is timed out
            struct QueueSmart PinDec;                           // add to the queue a struct to be able to toggle the pin
            PinDec.Pin      = Timers[Timer_Counter].PIN;        // wich pin to toggle
            PinDec.Address  = Timers[Timer_Counter].Address;    // Wich address the pin is on
            PinDec.ID       = Timers[Timer_Counter].ID;         // The id for the web to toggle the button
            PinDec.Timer    = Timers[Timer_Counter].Duration;   // the duration? idk why
            xQueueSend(xQueue, &PinDec, portMAX_DELAY);         // Actually send to the queue
            // Fired!
        }
    }
    if(Timer_Counter == MAX_TIMERS){Timer_Counter = 0;}     // Reset the counter if we are at the end
    Timer_Counter++;
}

static const inline void Timer_Start(int Nth,int Duration,int PIN,String ID,uint16_t Address){
    // Start a Timer, we need some settings to set!
    Timers[Nth].Duration    = Duration * 1000;  // Timer duration ( user set it in seconds so have to convert to millis)
    Timers[Nth].Start       = millis();         // The actual timer start time
    Timers[Nth].PIN         = PIN;              // The PIN to switch on or off
    Timers[Nth].ID          = ID;               // The ID of the PIN to be able to toggle it on the web after we set it
    Timers[Nth].Address     = Address;          // The address of the expander where the pin is
    Timers[Nth].Is_On       = true;             // Timer toggle on/off bit
}


static const inline void Write_To_Expand(uint16_t Address,int PIN,String ID,int Timer){
    //vTaskSuspend(ModBus_Task_Handle);
    xSemaphoreTake( xMutex, portMAX_DELAY );
    vTaskDelay(10);
    if (!mb.slave()) {
        boolean Is_Set = false;
        if(bitRead(Timers[PIN].BUFFER_PIN[0],PIN) == 0){
            bitSet(Timers[PIN].BUFFER_PIN[0],PIN);
            Is_Set = true;
        }else{
            bitClear(Timers[PIN].BUFFER_PIN[0],PIN);
            Is_Set = false;
        }
        if(mb.writeHreg(Address,0x4032,Timers[PIN].BUFFER_PIN,1,cbWriteSmart)){
            if(Is_Set){
                Timer_Start(PIN,Timer,PIN,ID,Address);
                Send_Async(ID,";true;Gomb");
            }else{
                Timer_Stop(PIN);
                Send_Async(ID,";false;Gomb");
            }
        }
        while (mb.slave()){mb.task();}
    }
    vTaskDelay(10);
    //vTaskResume(ModBus_Task_Handle);
    xSemaphoreGive( xMutex );
}

void Smart_Switch_Run( void * parameter ){
    for ever{
        if(xQueue != NULL){
            struct QueueSmart PINProp;
            if(xQueueReceive(xQueue, &PINProp, portMAX_DELAY) == pdPASS){
                Write_To_Expand(PINProp.Address,PINProp.Pin,PINProp.ID,PINProp.Timer);
            }
        }
        Timer_Checker_Loop();
        vTaskDelay(1);
    }
}