/*******************************************************************
File: usage.c
Date: 9-September-2020
Author: Peter Spindler
Description: Functions to determine and show the cpu usage
*********************************************************************/

#include "FreeRTOS.h"
#include "task.h"
#include "stm32f7xx_hal.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

//#define DEMCR           (*(volatile uint32_t *)0xE000EDFC)
//#define DWT_CTRL        (*(volatile uint32_t *)0xE0001000)
//#define DWT_CYCCNT      (*(volatile uint32_t *)0xE0001004)

static uint32_t Usage_IdleTickStart, Usage_IdleTickEnd, Usage_IdleTickTotal = 0;
static uint32_t Usage_MeasTickStart, Usage_MeasTickEnd, Usage_MeasTickTotal = 0, Usage_MeasCnt = 0;
extern volatile void *pxCurrentTCB;


#define TASKSTATUS_SIZE    16
static TaskStatus_t Usage_TaskStatusArray[TASKSTATUS_SIZE];
static TaskStatus_t Usage_TaskStatusArrayPrev[TASKSTATUS_SIZE];
static TaskStatus_t *Usage_TaskStatusArraySort[TASKSTATUS_SIZE];
uint32_t Usage_TaskStatusTimePrev = 0;
static uint32_t Usage_SOL = 0;

uint32_t Usage_GetTicks( void )
{
	return DWT->CYCCNT;
}


void Usage_TaskSwitchedIn( void )
{
	if( pxCurrentTCB == xTaskGetIdleTaskHandle() )  {
		Usage_IdleTickStart = Usage_GetTicks();
	}
}

void Usage_TaskSwitchedOut( void )
{
	if( pxCurrentTCB == xTaskGetIdleTaskHandle() )  {
		Usage_IdleTickEnd = Usage_GetTicks();
		Usage_IdleTickTotal += Usage_IdleTickEnd - Usage_IdleTickStart;
	}
}

uint8_t Usage_GetUsage( void )
{
	uint8_t Usage = 0;
	Usage_MeasTickEnd = Usage_GetTicks();
	Usage_MeasTickTotal = Usage_MeasTickEnd - Usage_MeasTickStart;

	Usage_IdleTickTotal /= 1024;  // Divide to prevent overflows by multiplication with 100
	Usage_MeasTickTotal /= 1024;  // Divide to prevent overflows by multiplication with 100

	Usage = 100-(100*Usage_IdleTickTotal)/Usage_MeasTickTotal;

	Usage_MeasTickStart = Usage_MeasTickEnd;
	Usage_IdleTickTotal = 0;
	if( Usage_MeasCnt < 0xff ) {
		Usage_MeasCnt++;
	}

	// First measurement can be wrong!
	if( Usage_MeasCnt > 1 ) {
		return Usage;
	} else {
		return 0;
	}

}



static int Usage_TaskStatusCompare (const void * a, const void * b)
{
	TaskStatus_t *aa = *((TaskStatus_t**) a);
	TaskStatus_t *bb = *((TaskStatus_t**) b);
	return ((int)aa->xTaskNumber - (int)bb->xTaskNumber);
}

void Usage_PrintStats( void )
{
	uint32_t TaskCnt, x, i;
	uint8_t Ignore = 0;
	int32_t DiffCounter;
	TaskStatus_t *TS;
	uint32_t TaskStatusTime, Usage, TotalTimeDiff;
	uint32_t NonIdleCounter = 0;


	TaskCnt = uxTaskGetNumberOfTasks();
	if( TaskCnt > TASKSTATUS_SIZE ) {
		TaskCnt = TASKSTATUS_SIZE;
	}

	TaskCnt = uxTaskGetSystemState( Usage_TaskStatusArray, TaskCnt, &TaskStatusTime );

	// Note: uxTaskGetSystemState stores the task information in TaskStatusArray in an arbitrary sequence.
	//       Sort TaskStatusArray by the task number!
	for(i=0;i<TaskCnt;i++) {
		Usage_TaskStatusArraySort[i] = &Usage_TaskStatusArray[i];
	}
	qsort(Usage_TaskStatusArraySort,TaskCnt,sizeof(TaskStatus_t*),Usage_TaskStatusCompare);

	// Get time without idle:
	NonIdleCounter = 0;
	for( x = 0; x < TaskCnt; x++ ) 	{
		TS = Usage_TaskStatusArraySort[ x ];
		for( i = 0; i < TaskCnt; i++ ) {
			if( TS->xTaskNumber == Usage_TaskStatusArrayPrev[ i ].xTaskNumber ) {
				if( Usage_TaskStatusArrayPrev[ i ].ulRunTimeCounter == 0 ) {
					Ignore = 1;
				}
				if( TS->xHandle != xTaskGetIdleTaskHandle() ) {
					NonIdleCounter += TS->ulRunTimeCounter-Usage_TaskStatusArrayPrev[ i ].ulRunTimeCounter;
				}
			}
		}
	}

	TotalTimeDiff = TaskStatusTime - Usage_TaskStatusTimePrev;

	TotalTimeDiff /= 100UL;        // For percentage calculations.

	if( TaskStatusTime > 0 && Usage_TaskStatusTimePrev != 0 && !Ignore  ) {
		printf("CPU usage without idle: %d%%, free RTOS heap: %d, SOL: %d\n", (int) (NonIdleCounter/TotalTimeDiff), xPortGetFreeHeapSize(), (int) Usage_SOL );
		Usage_SOL++;
			for( x = 0; x < TaskCnt; x++ ) 	{
			TS = Usage_TaskStatusArraySort[ x ];
			DiffCounter = -1;
			for( i = 0; i < TaskCnt; i++ ) {
				if( TS->xTaskNumber == Usage_TaskStatusArrayPrev[ i ].xTaskNumber ) {
					DiffCounter = TS->ulRunTimeCounter-Usage_TaskStatusArrayPrev[ i ].ulRunTimeCounter;
					break;
				}
			}
			if( DiffCounter >= 0 ) {
				Usage = (DiffCounter) / TotalTimeDiff;
				if( Usage > 100 ) {
					Usage = 100;
				}
			}

			printf("  %2d: %-16s: CPU: %3u%%, free stack: %3u\n",
					(int) TS->xTaskNumber, TS->pcTaskName, (int) Usage,
					TS->usStackHighWaterMark );
		}
	}
	memcpy(Usage_TaskStatusArrayPrev,Usage_TaskStatusArray,sizeof(Usage_TaskStatusArrayPrev));
	Usage_TaskStatusTimePrev = TaskStatusTime;
}

void Usage_Init(void)
{
	// Use core tick timer (implemented in the data watchpoint and trace unit, available in STM32F4 only!)
	//DEMCR |= (1<<24);
    //DWT_CTRL |= (1<<0);

	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	DWT->CYCCNT = 0;
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}