#include "rtos.h"

/* Private varibles */
static volatile task TaskArray[MAX_TASKS]; // очередь задач
static volatile u08 arrayTail;             // "хвост" очереди

/* Private defines */

/* External variables */
extern volatile uint8_t TWI_WDT;

/* Delay counter */
volatile uint16_t TDelay;

/**
 * Инициализация РТОС
 */
inline void RTOS_Init()
{
  // "хвост" в 0
  arrayTail = 0;
}

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

  if (!taskFunc) {
    return;
  }

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

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

      RESTORE_INTERRUPT;
      return;                             // обновив, выходим
    }
  }

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

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

    arrayTail ++;                         // увеличиваем "хвост"
    RESTORE_INTERRUPT;
  }
}

/**
 * Удаление задачи из списка
 */
inline void RTOS_DeleteTask (void (*taskFunc)(void))
{
  u08 i;
  for (i=0; i<arrayTail; i++) {           // проходим по списку задач
    if (TaskArray[i].pFunc == taskFunc) { // если задача в списке найдена

      DISABLE_INTERRUPT;
      if (i != (arrayTail - 1)) {         // переносим последнюю задачу на место удаляемой
        TaskArray[i] = TaskArray[arrayTail - 1];
      }
      arrayTail --;                       // уменьшаем указатель "хвоста"
      RESTORE_INTERRUPT;
      return;
    }
  }
}

/**
 * Диспетчер РТОС, вызывается в main
 */

void RTOS_DispatchTask(void)
{
  u08 i;
  void (*function) (void);

  for (i=0; i<arrayTail; i++) {               // проходим по списку задач
    if (TaskArray[i].run == 1) {              // если флаг на выполнение взведен, запоминаем задачу, т.к. во
      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)();                          // выполняем задачу
    }
  }
}

/**
 * Delay, msek
 * также, что-бы зря не терять время -- крутим диспетчер
 */
void tdelay_ms(uint16_t msek)
{

  TDelay = msek;

  do {
    RTOS_DispatchTask();
    // делать нехрен -- спим, ждём прерывание
#if defined(__GNUC__)
    set_sleep_mode(SLEEP_MODE_IDLE);
    sleep_mode();
#elif defined(__ICCAVR__)
    MCUCR = 1<<SE;
    __sleep();
#endif
  } while (TDelay>0);

}

/**
 * Таймерная служба РТОС и tdelay (прерывание аппаратного таймера)
 */
#if defined(__ICCAVR__)
#pragma vector=TIMER0_OVF_vect
__interrupt void TIMER0_OVF_ISR(void)
#elif defined(__GNUC__)
ISR(TIMER0_OVF_vect)
#endif
{
  /* reload timer counter */
  TCNT0 = TIMER0_CNT;
  //PINB |= 0x20; // LED toggle

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

  /* Delay_ms counter */
  if (TDelay > 0) {
    TDelay --;
  }

  /* I2C wachdog counter */
  if (TWI_WDT > 0) {
    TWI_WDT --;
  }

}