#include <avr/sleep.h>
#include <avr/interrupt.h>
#include "rtos.h"

static volatile uint8_t saveRegister;
#define  ENABLE_INTERRUPT sei()
#define DISABLE_INTERRUPT saveRegister = SREG; cli()
#define RESTORE_INTERRUPT SREG = saveRegister
//использовать RESTORE только после DISABLE

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

// внешние переменные
extern uint8_t TWI_WDT;

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

/*
 * Инициализация РТОС и t_delay, время тика - 1 мс
 */
inline void RTOS_Init(void) {
  // настраиваем основной таймер
  TIMER_TCCR = TIMER_CSB;
  TIMER_TCNT = TIMER_CNT;
  TIMER_TIMSK |= TIMER_INT;

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

/*
 * Добавление задачи в список
 */
void RTOS_SetTask (void (*taskFunc)(void), uint16_t taskDelay, uint16_t taskPeriod) {
   uint8_t 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;
   } else {
     while(1);
   }
}

/*
 * Удаление задачи из списка
 */
inline void RTOS_DeleteTask (void (*taskFunc)(void)) {
   uint8_t 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) {
   uint8_t 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)();
      }
   }
}

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

  TDelay = msek;

  do {
    if (TDelay > 1) {
      RTOS_DispatchTask();
    }
    // делать нехрен -- спим, ждём прерывание
    set_sleep_mode(SLEEP_MODE_IDLE);
    sleep_mode();
  } while (TDelay != 0);

}

/******************************************************************************************
 * Таймерная служба РТОС и tdelay (прерывание аппаратного таймера)
 */
#pragma GCC optimize ("O3")
ISR(TIMER_OVF_VECT) {

  // перезарядим таймер
  TIMER_TCNT = TIMER_CNT;

  /* RTOS_Timer */
  uint8_t 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 --;
  }

  if (TWI_WDT != 0) {
    TWI_WDT --;
  }

}