#include "main.h"

/**
 * @brief Task Structure
 */
typedef struct task {
   void (*pFunc) (void);  /**< @brief function pointer */
   uint32_t delay;  /**< @brief delay before the first start of the task */
   uint32_t period; /**< @brief task start period */ 
        int run;    /**< @brief task ready flag */
} task_t;

/*
 * Local variables
 */
static volatile task_t TaskArray[MAX_TASKS];/**< @brief task queue */
static volatile int arrayTail;              /**< @brief "tail" of the queue */
static volatile uint32_t TDelay;            /**< @brief delay counter */

/**
 * @brief Initialization of RTOS and t_delay, tick time - 1 ms
 */
inline void RTOS_Init(void) {
  // setting up the main timer
  SysTick->LOAD  = (uint32_t)(SystemCoreClock/1000 - 1UL);          /* set reload register */
  NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
  SysTick->VAL   = 0UL;                                             /* Load the SysTick Counter Value */
  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
                   SysTick_CTRL_TICKINT_Msk   |
                   SysTick_CTRL_ENABLE_Msk;                         /* Enable SysTick IRQ and SysTick Timer */

  // shrink "tail"
  arrayTail = 0;
}

/**
 * @brief Adding a task to the list
 * 
 * @param taskFunc      function pointer
 * @param taskDelay     delay before the first start of the task
 * @param taskPeriod    task start period
 */
void RTOS_SetTask (void (*taskFunc)(void), uint32_t taskDelay, uint32_t taskPeriod) {
   int i;

   if (!taskFunc) {
    return;
   }

   // search for a task in the current list
   for(i = 0; i < arrayTail; i++) {
      // if found, then update the variables
      if (TaskArray[i].pFunc == taskFunc) {
         __disable_irq();

         TaskArray[i].delay  = taskDelay;
         TaskArray[i].period = taskPeriod;
         TaskArray[i].run    = 0;

         __enable_irq();
         // update, exit
         return;
      }
   }

   // if there is no such task in the list
   if (arrayTail < MAX_TASKS) {
      // and there is a place, then we add
      __disable_irq();

      TaskArray[arrayTail].pFunc  = taskFunc;
      TaskArray[arrayTail].delay  = taskDelay;
      TaskArray[arrayTail].period = taskPeriod;
      TaskArray[arrayTail].run    = 0;

      // enlarge the "tail"
      arrayTail++;
      __enable_irq();
   } else {
     //!!! no left space for new task :-(
     while (1);
   }
}

/**
 * @brief Removing a task from the list
 * 
 * @param taskFunc  function pointer
 */
inline void RTOS_DeleteTask (void (*taskFunc)(void)) {
   int i;

   // go through the list of tasks
   for (i=0; i<arrayTail; i++) {
      // if the task is found in the list
      if (TaskArray[i].pFunc == taskFunc) {
         __disable_irq();
         // move the last task
         if (i != (arrayTail - 1)) {
            // in place of the removed
            TaskArray[i] = TaskArray[arrayTail - 1];
         }
         // decrement "tail" pointer
         arrayTail--;
         __enable_irq();
         return;
      }
   }
}

/**
 * @brief RTOS dispatcher, called in main
 */
void RTOS_DispatchTask(void) {
   int i;
   void (*function) (void);
   // go through the list of tasks
   for (i=0; i<arrayTail; i++) {
      // if the execution flag is set,
      if (TaskArray[i].run != 0) {
        // we remember the task, because index may change during runtime
        function = TaskArray[i].pFunc;
        if (TaskArray[i].period == 0) {
          // if the period is 0, remove the task from the list,
          RTOS_DeleteTask(TaskArray[i].pFunc);
        } else {
          // otherwise remove the start flag
          TaskArray[i].run = 0;
          // if the task has not changed the delay
          if (!TaskArray[i].delay) {
            // set her
            TaskArray[i].delay = TaskArray[i].period-1;
            // task for itself can pause
          }
        }
        // run the task
        (*function)();
      }
   }
}

/**
 * @brief Inserts a delay time.
 * 
 * @param msek   specifies the delay time length, in milliseconds.
 */
void tdelay_ms(uint32_t msek) {

  TDelay = msek;

  do {
    if (TDelay > 1) {
      /* so as not to waste time in wait - turn the dispatcher */
      RTOS_DispatchTask();
    }
    /* there is nothing to do - sleep, waiting for an interruption */
    __WFI();
  } while (TDelay != 0);

}

/**
 * @brief RTOS timer service and tdelay (hardware timer interrupt)
 */
#pragma GCC optimize ("O3")
/**
  * @brief  This function handles SysTick Handler.
  */
void SysTick_Handler(void) {

  /* RTOS_Timer */
  int i;
  // go through the list of tasks
  for (i=0; i<arrayTail; i++) {
    // if the time to execute has expired
    if (TaskArray[i].delay == 0) {
      // raise the launch flag,
      TaskArray[i].run = 1;
    } else {
      // otherwise reduce the time
      TaskArray[i].delay--;
    }
  }

  /* decrement the counter */
  if (TDelay != 0) {
    TDelay --;
  }
}