#include "main.h"

/*
 * Переменные модуля
 */
static volatile task TaskArray[MAX_TASKS]; // очередь задач
static volatile uint8_t arrayTail;         // "хвост" очереди

// счётчик задержки
static volatile uint32_t TDelay;

/*
 * Инициализация РТОС и t_delay, время тика - 1 мс
 */
inline void RTOS_Init(void) {
  // настраиваем основной таймер
  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 */

  // "хвост" в 0
  arrayTail = 0;
}

/*
 * Добавление задачи в список
 */
void RTOS_SetTask (void (*taskFunc)(void), uint32_t taskDelay, uint32_t taskPeriod) {
   int i;

   if (!taskFunc) {
    return;
   }

   // поиск задачи в текущем списке
   for(i = 0; i < arrayTail; i++) {
      // если нашли, то обновляем переменные
      if (TaskArray[i].pFunc == taskFunc) {
         __disable_irq();

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

         __enable_irq();
         // обновив, выходим
         return;
      }
   }

   // если такой задачи в списке нет
   if (arrayTail < MAX_TASKS) {
      // и есть место, то добавляем
      __disable_irq();

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

      // увеличиваем "хвост"
      arrayTail++;
      __enable_irq();
   } else {
     //!!! no left space for new task :-(
     // tube power off
     TIM1->CCER &= ~(TIM_CCER_CC1E);
     TIM3->CCER &= ~(TIM_CCER_CC4E);
     TIM3->CCER &= ~(TIM_CCER_CC3E);
     TIM3->CCER &= ~(TIM_CCER_CC2E);
     TIM3->CCER &= ~(TIM_CCER_CC1E);
     GPIOA->BSRR = 0x10;
     // color - orange
     TIM1->CCR2 = 0xff;
     TIM1->CCR3 = 0x7f;
     TIM1->CCR4 = 0x0;
     while (1);
   }
}

/*
 * Удаление задачи из списка
 */
inline void RTOS_DeleteTask (void (*taskFunc)(void)) {
   int i;

   // проходим по списку задач
   for (i=0; i<arrayTail; i++) {
      // если задача в списке найдена
      if (TaskArray[i].pFunc == taskFunc) {
         __disable_irq();
         // переносим последнюю задачу
         if (i != (arrayTail - 1)) {
            // на место удаляемой
            TaskArray[i] = TaskArray[arrayTail - 1];
         }
         // уменьшаем указатель "хвоста"
         arrayTail--;
         __enable_irq();
         return;
      }
   }
}

/*
 * Диспетчер РТОС, вызывается в main
 */
void RTOS_DispatchTask(void) {
   int i;
   void (*function) (void);
   // проходим по списку задач
   for (i=0; i<arrayTail; i++) {
      // если флаг на выполнение взведен,
      if (TaskArray[i].run != 0) {
        // запоминаем задачу, т.к. во время выполнения может измениться индекс
        function = TaskArray[i].pFunc;
        if (TaskArray[i].period == 0) {
          // если период равен 0, удаляем задачу из списка,
          RTOS_DeleteTask(TaskArray[i].pFunc);
        } else {
          // иначе снимаем флаг запуска
          TaskArray[i].run = 0;
          // если задача не изменила задержку
          if (!TaskArray[i].delay) {
            // задаем ее
            TaskArray[i].delay = TaskArray[i].period-1;
            // задача для себя может сделать паузу
          }
        }
        // выполняем задачу
        (*function)();
      }
   }
}

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

  TDelay = msek;

  do {
    if (TDelay > 1) {
      /* что-бы зря не терять время -- крутим диспетчер */
      RTOS_DispatchTask();
    }
    /* делать нехрен -- спим, ждём прерывание */
    __WFI();
  } while (TDelay != 0);

}

/******************************************************************************************
 * Таймерная служба РТОС и tdelay (прерывание аппаратного таймера)
 */
#pragma GCC optimize ("O3")
/**
  * @brief  This function handles SysTick Handler.
  * @param  None
  * @retval None
  */
void SysTick_Handler(void) {

  /* RTOS_Timer */
  int i;
  // проходим по списку задач
  for (i=0; i<arrayTail; i++) {
    // если время до выполнения истекло
    if (TaskArray[i].delay == 0) {
      // взводим флаг запуска,
      TaskArray[i].run = 1;
    } else {
      // иначе уменьшаем время
      TaskArray[i].delay--;
    }
  }

  /* TDelay_Timer -- уменьшим счётчик */
  if (TDelay != 0) {
    TDelay --;
  }
}