/**************************************************************************
* NAME: 	   scheduler.c                                                *
* 																		  *
* AUTHOR: 	   Jonas Holmberg											  *
* 																		  *
* PURPOSE: 	   Defining the the scheduler to be used in the system to     *
* 			   organize the runtime for the tasks in the system based on  *
* 			   priority.     											  *
* INFORMATION: Many elements and ideas obtained from beta/cleanflight.    *
* 																		  *
* GLOBAL VARIABLES:                                                       *
* Variable        Type          Description                               *
* --------        ----          -----------								  *
* SystemTasks	  task_t[]		Contains all the tasks that can exist in  *
* 								the system right now					  *
*																		  *
* averageSystem	  uint16_t		The current load of the system in percent.*
* LoadPercent					May or may not display correctly.		  *
*																		  *
***************************************************************************/

#include "Scheduler/scheduler.h"
#include "Scheduler/tasks.h"
#include "drivers/system_clock.h"
#include "utilities/math_helpers.h"
#include <string.h>
#include "drivers/led.h"


/* Variables ------------------------------------------- */

static task_t *currentTask = NULL;						//The current task that the is run

static uint32_t totalSchedulerPasses = 0;				/* Number of times the scheduler has been invoked */
static uint32_t totalSchedulerReadyTasks = 0;			/* The amount of tasks that totally has been ready in the scheduler */

uint32_t currentTime = 0;								//The current time in microseconds
uint16_t averageSystmeLoad = 0;
						//
static int taskReadyQueueSize = 0;						//The number of tasks in the ready queue

static task_t * taskReadyQueue[TASK_COUNT + 1];			/* Array holding all the tasks that are ready to run in the system + 1 for a NULL value at the end*/

uint16_t averageSystemLoadPercent = 0;					//The load of the system in percent

static uint32_t tasksExecutionTimeUs = 0;				//Total execution time of the task for one pass of the system task
//static uint32_t lastSystemLoadTimeValUs = 0;			//Not used right now, would be used to calculate load with time vals

uint32_t taskAgeCycleStatisitcs[taskAgeCycleCounterSize];		//Age cycle statistics array

/* Functions to operate on the task queue ------------------------------------------------------- */

/**************************************************************************
* BRIEF:   	   Clears the ready queue.                                                               *
* INFORMATION: Clears the entire queue and sets the size to the number of
* 	  		   possible tasks that exist in the system.                                                           *
**************************************************************************/
void taskReadyQueueClear(void)
{
	//Empties the queue by settin all the positions to 0(NULL)
	memset(taskReadyQueue, 0, sizeof(taskReadyQueue));
	taskReadyQueueSize = 0;
}


/**************************************************************************
* BRIEF: 	   Checks if a task already exists in the ready queue.
*                                                                         *
* INFORMATION: Given a task it will be compared to the tasks in the queue.
* 			   If it exist inside the queue the function will return true,
* 			   otherwise it will return false.
**************************************************************************/
bool taskReadyQueueContains(task_t *task)
{
	//Go through the taskReadyQueue to see if the current task is contained
	for (int i = 0; i < taskReadyQueueSize; i++)
	{
		if (taskReadyQueue[i] == task)
		{
			return true;
		}
	}

	return false;
}


/**************************************************************************
* BRIEF:       Adds a new task to the ready queue.
*                                                                         *
* INFORMATION: This function will add a new task to the system if it does
* 			   not already exist in it, or if the task queue is full (it
* 			   should not be able to be that if every task is implemented
* 			   correctly). Otherwise the task will be added to the queue
* 			   based on priority and then period.                                                           *
**************************************************************************/
bool taskReadyQueueAdd(task_t *task)
{
	int i;		/* Iterator */
	int j; 		/* Inner iterator used to sort based on desired period */

	//Check if the task is in the queue already
	if (taskReadyQueueContains(task) || taskReadyQueueSize >= TASK_COUNT)
	{
		return false;
	}

	//Try to add the task to the taskReadyQueue at a good position
	//Order is: staticPriority > desired period > all else
	//<= taskReadyQueueSize because if we go through the entire thing we should get a null value
	//representing the position directly after the last one in the queue.
	for (i = 0; i < taskReadyQueueSize; i++)
	{
		//check if the static prio of the current position in the queue is less than the task we want to add
		if(taskReadyQueue[i]->staticPriority < task->staticPriority)
		{
			//order tasks based on lowest period first, within the ones with the same priority
			//ToDo: check if setting j to i-1 initially will produce the same result, in that case add another iterator to get item from taskReadyQueue
			for (j = i; taskReadyQueue[j]->staticPriority == task->staticPriority; j++ )
			{
				//If the new task has a shorter period place in front
				if (task->desiredPeriod <= taskReadyQueue[j]->desiredPeriod)
				{
					//remove one increment because otherwise it will be wrong
					j--;
				}
			}
			//task does not have shorter period than any other with the same priority, add behind them
			j++;
			memmove(&taskReadyQueue[j+1], &taskReadyQueue[j], sizeof(task) * (taskReadyQueueSize - j));
			taskReadyQueue[j] = task;
			++taskReadyQueueSize;
			return true;
		}
	}

	//If this is true it needs to be placed at the very back of the queue
	if (taskReadyQueue[i] == NULL )
	{
		taskReadyQueue[i] = task;	//ToDo: check if this works correctly, that a new NULL value will exist at i+1
		taskReadyQueueSize ++;
		return true;
	}

	//Could not add the task in the anywhere in the queue
	return false;
}


/**************************************************************************
* BRIEF:   	   Removes a task from the ready queue.
*                                                                         *
* INFORMATION: Given a task it will remove it from the ready queue if it
* 			   exist somewhere inside the queue.                                                           *
**************************************************************************/
bool taskReadyQueueRemove(task_t *task)
{
	for (int i = 0; i < taskReadyQueueSize; ++i) {
	        if (taskReadyQueue[i] == task) {
	            memmove(&taskReadyQueue[i], &taskReadyQueue[i+1], sizeof(task) * (taskReadyQueueSize - i));
	            --taskReadyQueueSize;
	            return true;
	        }
	    }
	    return false;
}


/**************************************************************************
* BRIEF:	   The function that will run when the scheduler chooses to run
* 			   the SYSTEM task.
* 			                                                                *
* INFORMATION: This task function is responsible for calculating the load
* 			   of the system. More things can be added to this task if
* 			   needed.     	                                                      *
**************************************************************************/
void systemTaskSystem(void)
{
    /* Calculate system load */
    if (totalSchedulerPasses > 0) {
        averageSystemLoadPercent = 100 * totalSchedulerReadyTasks / totalSchedulerPasses;
        totalSchedulerPasses = 0;
        totalSchedulerReadyTasks = 0;
    	/*uint32_t timeAtInstantUs = clock_get_us();
    	uint32_t timeBetween = timeAtInstantUs - lastSystemLoadTimeValUs;
    	float divVal = ((float)tasksExecutionTimeUs / (float)timeBetween);

    	averageSystemLoadPercent = 100* divVal;
    	lastSystemLoadTimeValUs = timeAtInstantUs;*/
    	tasksExecutionTimeUs = 0;

#ifdef USE_LED_WARNINGS_SYSTEM_LOAD
        //ToDo: Test to light a led if the system is utilizing more than 100%
        if (averageSystemLoadPercent >= 100)
        {
        	ledSetWarningType(LEDWARNING_LED0_ON);
        }
        else if(averageSystemLoadPercent >= 80)
        {
        	ledSetWarningType(LEDWARNING_LED0_BLINK);
        }
        else if(averageSystemLoadPercent >= 60)
		{
			ledSetWarningType(LEDWARNING_LED1_ON);
		}
        else if(averageSystemLoadPercent >= 40)
		{
			ledSetWarningType(LEDWARNING_LED1_BLINK);
		}
        else
        {
        	ledSetWarningType(LEDWARNING_OFF);
        }
#endif
    }

}


/**************************************************************************
* BRIEF: 	   Enables or disables a task to be able to run in the system.
*
* INFORMATION: Given an id for a task it can be added or removed from the
* active tasks in the system. Meaning tasks that can be selected by the
* scheduler to run.
**************************************************************************/
void enableTask(taskId_t taskId, bool enabled)
{
	if (taskId == TASK_SELF || taskId < TASK_COUNT) {
	        task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
	        if (enabled && task->taskFunction) {
	            taskReadyQueueAdd(task);
	        } else {
	            taskReadyQueueRemove(task);
	        }
	    }
}


/**************************************************************************
* BRIEF: 	   Returns the delta time value of a task.
*
* INFORMATION: Given an id for a task it will return is delta time value.
*			   The time between its latest run and the one before.
**************************************************************************/
uint32_t getTaskDeltaTime(taskId_t taskId)
{
	if (taskId == TASK_SELF || taskId < TASK_COUNT) {
	        task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
	        return task->taskLatestDeltaTime;
	    } else {
	        return 0;
	    }
}


/**************************************************************************
* BRIEF: 	   Gives a task a new period.
*
* INFORMATION: Given an id for a task its period can be changed to a new
* 			   desired value.
**************************************************************************/
void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros)
{
	if (taskId == TASK_SELF || taskId < TASK_COUNT)
	{
		task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
		task->desiredPeriod = MAX(100, newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
	}
}


/**************************************************************************
* BRIEF: 	   Returns an array of ageCyckle values observed when
* 			   scheduling the tasks.
*
* INFORMATION: If a task has an age cycle greater than 1 it means it has
* 			   missed one or more "periods". Each slot in the 16 value
* 			   sized array matches a cycle age value observed for a task.
* 			   Cycle age 1 or less is not counted since it would be to
* 			   many and we don't care about when it is correct, this is only
* 			   used to observe how many is wrong. Every task will generate
* 			   a age cycle value each scheduling attempt. Each time a task
* 			   missed his period the values in the array that this function
* 			   return will increase.
**************************************************************************/
#ifdef USE_TASK_AGE_CYCLE_STATISTICS
void getSchedulerAgeCycleData(uint32_t outValue[taskAgeCycleCounterSize])
{
	outValue = taskAgeCycleStatisitcs;
}
#endif


/**************************************************************************
* BRIEF: 	   Get some information of a task.
*
* INFORMATION: Given an id for a task its current values can be obtain.
* 			   The variable taskInfo will point to the obtained information
* 			   and act as an out variable.
**************************************************************************/
void getTaskInfo(taskId_t taskId, taskInformation_t * taskInfo)
{
	taskInfo->taskName = SystemTasks[taskId].taskName;
	taskInfo->subTaskName = SystemTasks[taskId].subTaskName;
	taskInfo->isEnabled = taskReadyQueueContains(&SystemTasks[taskId]);
	taskInfo->desiredPeriod = SystemTasks[taskId].desiredPeriod;
	taskInfo->staticPriority = SystemTasks[taskId].staticPriority;
	taskInfo->maxExecutionTime = SystemTasks[taskId].maxExecutionTime;
	taskInfo->totalExecutionTime = SystemTasks[taskId].totalExecutionTime;
	taskInfo->averageExecutionTime = SystemTasks[taskId].averageExecutionTime;
	taskInfo->latestDeltaTime = SystemTasks[taskId].taskLatestDeltaTime;
}


/**************************************************************************
* BRIEF: 	   Enables the tasks that should run.
*
* INFORMATION: All the tasks in the system that should be added to the run
* 			   queue will be added when this function is invoked.
**************************************************************************/
void initSchedulerTasks(void)
{
	//Enable the tasks in the system
	enableTask(TASK_SYSTEM, true);

	enableTask(TASK_GYROPID, true);

	enableTask(TASK_ACCELEROMETER, true);

	enableTask(TASK_ATTITUDE, true);

	enableTask(TASK_RX, true);

	enableTask(TASK_RX_CLI, true);

	enableTask(TASK_SERIAL, true);

	enableTask(TASK_BATTERY, true);

#ifdef BARO
	enableTask(TASK_BARO, true);
#endif

#ifdef COMPASS
	enableTask(TASK_COMPASS, true);
#endif

#ifdef GPS
	enableTask(TASK_GPS, true);
#endif

#ifdef SONAR
	enableTask(TASK_SONAR, true);
#endif

#if defined(BARO) || defined(SONAR)
	enableTask(TASK_ALTITUDE, true);
#endif

#if BEEPER
	enableTask(TASK_BEEPER, true);
#endif
}


/**************************************************************************
* BRIEF: 	   Initiates the scheduler and makes it ready to run.
*
* INFORMATION: The init will reset the ready queue of the system and add
* 			   all the tasks that should be added. The tasks that are
* 			   supposed to run with the current configuration of the system
**************************************************************************/
void initScheduler(void)
{
	//Clear the queue
	taskReadyQueueClear();

	//Init all the ToDo: add back when it is known that the scheduler works
#ifndef USE_DEBUG_TASKS
	initSchedulerTasks();
#endif

	//If we use debug tasks only init those
#ifdef USE_DEBUG_TASKS
	enableTask(TASK_DEBUG_1, true);
	enableTask(TASK_DEBUG_2, true);
	enableTask(TASK_DEBUG_3, true);
	enableTask(TASK_SYSTEM, true);
#endif
}


/**************************************************************************
* BRIEF: 	   The main scheduler function.
*
* INFORMATION: This function is responsible for choosing the next task that
* 			   the system should run. This is the main part of the system
* 			   that will always run, and is the part of the code that will
* 			   run in each iteration of the system loop.
**************************************************************************/
void scheduler(void)
{
	//Get the current time in Micro seconds
	currentTime = clock_get_us();
	task_t * taskToRun;
	task_t * selectedTask = NULL;
	uint16_t currentDynamicPriority = 0;
	uint32_t currentReadyTasks = 0;
	float currentAgeCyckleExact = 0;
	float ageCycleExact = 0;
	bool isRealTime = false;

	/* Check if a task has entered a new period and assign its new dynamic priority */
	/* Go through all the tasks to check if they should be able to run */
	for (int i = 0; i < taskReadyQueueSize; i++)
	{
		//Get the next task in the queue
		taskToRun = taskReadyQueue[i];

		/* Check if the task is an event, else its a time controlled task  */
		if (taskToRun->checkEventTriggered != NULL)
		{
			//ToDO: Handle event tasks, they should get to operate at a higher priority
			//ToDo: test if it works
			if (taskToRun->dynamicPriority > 0)
			{
				taskToRun->taskAgeCycle = 1 + ((currentTime - taskToRun->lastSignaledAt) / taskToRun->desiredPeriod);
				taskToRun->dynamicPriority = 1 + taskToRun->staticPriority * taskToRun->taskAgeCycle;
				currentReadyTasks++;
			}
			else if (taskToRun->checkEventTriggered(currentTime - taskToRun->lastExecutedAt))
			{
				taskToRun->lastSignaledAt = currentTime;
				taskToRun->taskAgeCycle = 1;
				taskToRun->dynamicPriority = 1 + taskToRun->staticPriority;
				currentReadyTasks++;
			}
			else
			{
				taskToRun->taskAgeCycle = 0;
			}


		}
		else	//Handle time controlled tasks
		{
			//Calculate a new age cycle for the task. It represents if the task is in a new "period" in relation to its last execution
			//If the value is 0 it is still in the same "period" as the last execution
			taskToRun->taskAgeCycle = (currentTime - taskToRun->lastExecutedAt) / taskToRun->desiredPeriod;

#ifdef USE_TASK_AGE_CYCLE_STATISTICS
			if (taskToRun->taskAgeCycle > 1)
				taskAgeCycleStatisitcs[taskToRun->taskAgeCycle] ++;	//increment the statistic counter for age cycles if we miss period
#endif
#ifdef USE_LED_WARNINGS_MISSED_PERIOD
			if (taskToRun->taskAgeCycle > 1)
				ledSetWarningType(LEDWARNING_LED0_BLINK_ONCE);
#endif

			//May not be used. Could be used to check what task is closest to its next period instance
			ageCycleExact = ((float)currentTime - (float)taskToRun->lastExecutedAt) / (float)taskToRun->desiredPeriod;

			//Check if > 0, if it has entered a new "period"
			if (taskToRun->taskAgeCycle > 0)
			{
				//Calculate the new dynamic priority for the task
				taskToRun->dynamicPriority = 1 + taskToRun->taskAgeCycle * taskToRun->staticPriority;
				currentReadyTasks ++;	//Increase the number of ready tasks
			}
		}

		//If the current task has the largest dynamic priority so far it could be a candidate for execution
		if(taskToRun->dynamicPriority >= currentDynamicPriority && taskToRun->taskAgeCycle >= 1)
		{
			bool changeTask = false;	//flag used to check if the task should be updated
			/* If a realtime task is found the bool isRealTime will be set to true
			 * meaning no other tasks than realtime will be able to enter the function */
			if (taskToRun->staticPriority == PRIORITY_REALTIME)		//if it is a realtime task
			{
				isRealTime = true;		//toggle this flag so that no non realtime task can be selected this cycle

				changeTask = true;
			}
			else if (!isRealTime)	//If task is not realtime
			{
				if (taskToRun->dynamicPriority == currentDynamicPriority)	//if the tasks have the same priority
				{
					if (ageCycleExact > currentAgeCyckleExact)	//if the current tasks deadline is closer than selected task
					{
						changeTask = true;
						currentAgeCyckleExact = ageCycleExact;	//update the value
					}
				}
				else
				{
					changeTask = true;
				}
			}

			if (changeTask == true)		//if the task should change
			{
				selectedTask = taskToRun;	//update the selected task
				currentDynamicPriority = taskToRun->dynamicPriority;	//update the current highest dynamic priority
			}
		}
	}	/* for-loop end */


	//Used for meassuring load
	totalSchedulerReadyTasks += currentReadyTasks;
	totalSchedulerPasses ++;

	//Assign the selected task to the pointer for the current task
	currentTask = selectedTask;

	//If we have found a task to run, execute the function that the task is responsible for
	if (selectedTask != NULL)
	{
		selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt;	//calculate the time between now and last execution
		selectedTask->lastExecutedAt = currentTime;	//update the last execution time to this moment
		selectedTask->dynamicPriority = 0;	//reset the dynamic priority to 0

		//handle the execution of the tasks function
		const uint32_t timeBeforeExecution = clock_get_us();
		selectedTask->taskFunction();					//Run the function associated with the current task
		const uint32_t TimeAfterExecution = clock_get_us();
		const uint32_t taskExecutionTime = TimeAfterExecution - timeBeforeExecution;	//calculate the execution time of the task

		tasksExecutionTimeUs += taskExecutionTime;	//Add the task execution time for each task execution, will be used to

		//Save statistical values
		selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;
		selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
		selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
	}

}