#include "RTOS.h"        
 
// Максимальный индекс массива очереди задач
const unsigned char MAX_IND_TASK_QUEUE = TASK_QUEUE_SIZE - 1;

// Для фиксации ошибок в указании размеров очередей (занимает 2 Байта EEPROM)-----------------------------------
// Все ошибки будут фиксироватся в EEPROM памяти в определённой ячейке
eeprom unsigned char eep_ErrTaskQueue       @0x00;  //Ячейка для сохранения ошибок очереди задач
eeprom unsigned char eep_ErrSoftTimerQueue  @0x01;  //Ячейка для сохранения ошибок очереди програмных таймеров
//Коды ошибок
const unsigned char ERR_TASK_QUEUE       = 0xAA;  //Ошибка в указании размера очереди задач
const unsigned char ERR_SOFT_TIMER_QUEUE = 0xBB;  //Ошибка в указании размера очереди програмных таймеров 
//--------------------------------------------------------------------------------------------------------------  
              
volatile TPTR TaskQueue[TASK_QUEUE_SIZE]; // Очередь указателей задач

volatile struct
{
  TPTR         GoToTask;  //Указатель перехода к задаче-функции
  unsigned int Time;      //Выдержка в миллисекундах
} 
SoftTimer[SOFT_TIMER_QUEUE_SIZE];  //Очередь програмных таймеров

// Реализация функций -----------------------------------------------------------------------------------------


/* Функция простоя ядра */
void CPU_Stop(void)
{
  /* Эта функция переводит чип AVR в режим холостого хода. В этом режиме CPU
     останавливается, но таймеры/счётчики, сторожевой таймер (Wathdog) и
     система прерываний продолжают работать. CPU можно разбудить внешними и
     внутренними прерываниями. 
  */              
  idle();
} 
 
 
/* 
	 Функция устанавливающая задачу в очередь по таймеру
	 Входные параметры:
	 	 TPTR         Task    - Задача (функция)
	 	 unsigned int NewTime - Новое время, которое нужно отсчитать
	 Дополнительные параметры:
	 	 struct SoftTimer           - Очередь програмных таймеров
	 	 eeprom eep_ErrSoftTimerQueue - Там будет фиксироваться ошибка "Очередь переполнена"
	 Описание:
		 На входе адрес перехода (имя задачи) и время в тиках службы таймера — миллисекундах.
		 Если в очереди таймеров уже есть таймер с такой задачей, то происходит апдейт времени.
		 Если не находим, то втыкаем данные в первую свободную ячейку. 
		 Две одинаковых задачи в очереди таймеров не возможны. 
		 Если очередь переполнена то это фиксируется в EEPROM памяти.
*/
void SetTimerTask(TPTR Task, unsigned int NewTime)
{
  unsigned char i;    // Индексная переменная 
  unsigned char sreg; // Сохраняет регистр SREG чтобы потом восстановить его
             
  sreg = SREG; // Сохраняем регистр SREG
  #asm("cli")  // Запрещаем прерывания. Помним об атомарном доступе!
  for (i = 0; i != SOFT_TIMER_QUEUE_SIZE; i++) // Прочесываем очередь таймеров
  {
		if (SoftTimer[i].GoToTask == Task) // Если уже есть запись с таким адресом
		{                                   
			SoftTimer[i].Time = NewTime; // Перезаписываем ей выдержку
			SREG = sreg;                   // Восстанавливаем регистр SREG
			return;                        // Выходим  
		}
  }
  for (i = 0; i != SOFT_TIMER_QUEUE_SIZE; i++) // Если не находим похожий таймер, то ищем любой пустой
  {
		if (SoftTimer[i].GoToTask == CPU_Stop)
		{                               
		  SoftTimer[i].GoToTask = Task; // Заполняем поле перехода задачи
		  SoftTimer[i].Time = NewTime;  // И поле выдержки времени   
		  SREG = sreg;                    // Восстанавливаем регистр SREG
		  return;                         // Выходим  
		}
  }
  if (eep_ErrSoftTimerQueue != ERR_SOFT_TIMER_QUEUE) // Зафиксировать ошибку только один раз
  	eep_ErrSoftTimerQueue = ERR_SOFT_TIMER_QUEUE;    // Код ошибки
  SREG = sreg; // Восстанавливаем регистр SREG
}
 
 
/* 
	 Функция инициализации RTOS должна быть вызвана до остальных функций. 
	 Дополнительные параметры:
	 	TPTR   TaskQueue                            - Очередь указателей
	 	struct SoftTimer[SOFT_TIMER_QUEUE_SIZE + 1] - Очередь програмных таймеров
*/
void InitRTOS(void)
{ 
  unsigned char i; // Индексная переменная

  sleep_enable(); // Разрешить перевод в режим пониженного энергопотребления  
  for (i = 0; i != TASK_QUEUE_SIZE; i++) // Во все позиции записываем CPU_Stop   
  	TaskQueue[i] = CPU_Stop;
  for (i = 0; i != SOFT_TIMER_QUEUE_SIZE; i++) // Обнуляем все таймеры   
  	SoftTimer[i].GoToTask = CPU_Stop;
}


/* 
	 Функция диспетчер задач
	 Дополнительные параметры:
	 	 TPTR TaskQueue - Очередь указателей
	 Описание:
	 	 Очередь выполнения задач не жестко заданная, а динамическая, конвеерного типа. То есть у нас
	   есть в памяти массив из указателей на задачи-функции. Диспетчер берет указатель и, если он не указывает на
	 	 CPU_Stop, осуществлет переход по этому адресу. Предварительно удалив задачу-функцию из очереди, заменив её CPU_Stop.
*/ 
void TaskManager(void)
{
  unsigned char i; // Индексная переменная
  TPTR GoToTask;   // Указатель на адреса переходов (задач)

  /* Запрещаем прерывания! Прерывания надо запрещать потому что идёт обращение
     к глобальной очереди задач диспетчера. Её могут менять и прерывания.
     Поэтому заботимся об атомарности операции. 
  */                         
  #asm("cli")
  GoToTask = TaskQueue[0]; // Берём первое значение из очереди
  if (GoToTask == CPU_Stop)  // Если там пусто
  {
    #asm("sei") // Разрешаем прерывания
    CPU_Stop(); // Переходим на функцию простоя ядра
  }
  else
  {
		for (i = 0; i != MAX_IND_TASK_QUEUE; i++) // В противном случае сдвигаем всю очередь
		  TaskQueue[i] = TaskQueue[i + 1];
		TaskQueue[MAX_IND_TASK_QUEUE] = CPU_Stop; // В последнюю запись пихаем затычку - простоя ядра
		#asm("sei") // Разрешаем прерывания
		GoToTask(); // Переходим к задаче   
  }
}


/* 
	 Функция отмены задачи 
	 Входные параметры:
	 	 TPTR Task - Задача (функция) которую нужно отменить
	 Дополнительные параметры:
	 	 TPTR TaskQueue   - Очередь указателей на задачи
	 	 struct SoftTimer - Очередь програмных таймеров
	 Описание:
	 	 Функция отменяет задачу из очереди задач и таймеров
*/
void DeleteTask(TPTR Task)
{
	unsigned char i;    // Индексная переменная
	unsigned char ind;  // Индексная переменная 
	unsigned char sreg; // Сохраняет регистр SREG чтобы потом восстановить его

	sreg = SREG; // Сохраняем регистр SREG
	#asm("cli")  // Запрещаем прерывания. Помним об атомарном доступе!
	// Прочёсываем очередь задач
	i = 0;
	while (i != TASK_QUEUE_SIZE)
  {
		if (TaskQueue[i] == Task) // Если задача найдена
		{
			// Сдвигаем очередь начиная с найденного индекса до конца очереди. Последнюю запись помечаем свободной.
			for (ind = i; ind != MAX_IND_TASK_QUEUE; ind++)  
		   	TaskQueue[ind] = TaskQueue[ind + 1];
			TaskQueue[MAX_IND_TASK_QUEUE] = CPU_Stop; // В последнюю запись пихаем затычку - простоя ядра
		}
		else i++;             
  }
	// Прочёсываем очередь таймеров
	i = 0;
	while (i != SOFT_TIMER_QUEUE_SIZE)  
  {   
		if (SoftTimer[i].GoToTask == Task) // Если есть запись с таким-же адресом задачи, тогда 
		{   
			SoftTimer[i].GoToTask = CPU_Stop; // В эту запись пихаем затычку - простоя ядра
			i = SOFT_TIMER_QUEUE_SIZE; // Завершить прочёсывание очереди таймеров         
		}
		 else i++;              
  }
	SREG = sreg; // Восстанавливаем регистр SREG
}


/* 
	 Функция постановщик в очередь задач 
	 Входные параметры:
	 	 TPTR Task - Задача (функция)
	 Дополнительные параметры:
	 	 TPTR   TaskQueue      - Очередь указателей
	 	 eeprom eep_ErrTaskQueue - Там будет фиксироваться ошибка "Очередь переполнена"  
	 Описание:
	 	 Заброс указателей-задач в очередь осуществляется другими задачами и прерываниями, а также программными
	 	 таймерами, а переход делается диспетчером. Если очередь переполнена то это фиксируется в EEPROM памяти.
*/ 
void SetTask(TPTR Task)
{
  unsigned char i = 0; // Индексная переменная
  unsigned char sreg;  // Сохраняет регистр SREG чтобы потом восстановить его
       
  sreg = SREG; // Сохраняем регистр SREG
  #asm("cli")  // Запрещаем прерывания. Помним об атомарном доступе!
  //Прочесываем очередь задач на предмет свободной ячейки с значением CPU_Stop - конец очереди
  while (TaskQueue[i] != CPU_Stop)   
  {
		i++;
		// Если очередь переполнена то выходим                            
		if (i == TASK_QUEUE_SIZE)
		{
			if (eep_ErrTaskQueue != ERR_TASK_QUEUE) // Зафиксировать ошибку только один раз			  
				eep_ErrTaskQueue = ERR_TASK_QUEUE; 		// Код ошибки			  
			SREG = sreg; // Восстанавливаем регистр SREG
			return;      // Выходим
		}
  }
  // Если нашли свободное место, то
  TaskQueue[i] = Task; // Записываем в очередь задачу  
  SREG = sreg; // Восстанавливаем регистр SREG 
}


/* 
	 Функция таймерная служба. Обслуживает очередь таймеров. Вызывать в прерывании каждую 1 миллисекунду.
	 Дополнительные параметры:
	 	 struct SoftTimer[SOFT_TIMER_QUEUE_SIZE + 1] - Очередь програмных таймеров
	 Описание:
		 Мы прочесываем массив структур таймеров поэлементно. Если в поле GoToTask у нас CPU_Stop, то очевидно 
		 что это пустой таймер. Т.к. задача такой быть не может. Такой таймерный слот пропускается.
		 Если таймер не пуст, то мы проверяем поле Time на ноль. Если не ноль, то уменьшаем и переходим к
		 следующему элементу массива.
		 А коли таймер дощёлкал, то мы забрасываем в очередь задачу лежащую в поле GoToTask предварительно 
		 удалив задачу-функцию из очереди, заменив её CPU_Stop. И потом диспетчер
		 задач запустит нам нужную задачу.   
*/  
void TimerService(void)
{
  unsigned char i; // Индексная переменная 
          
  for (i = 0; i != SOFT_TIMER_QUEUE_SIZE; i++) // Прочесываем очередь таймеров
  {
		if (SoftTimer[i].GoToTask == CPU_Stop) continue; // Если нашли пустышку — следующая итерация      
		if (SoftTimer[i].Time != 0) // Если таймер не выщелкал, то щёлкаем еще раз
		  SoftTimer[i].Time--;      //Уменьшаем число в ячейке   
		else // Дощёлкали до нуля?
		{
			SetTask(SoftTimer[i].GoToTask);   // Забрасываем в очередь задачу    
			SoftTimer[i].GoToTask = CPU_Stop; // А в ячейку пишем затычку     
		}
  }
}