#include "interCoreManager.h"


/* Global variables */
interCoreManager_t interCoreManagers[CONFIGURE_MAXIMUM_PROCESSORS];


rtems_task processInterCoreQueues(
  rtems_task_argument argument
)
{
	uint32_t cpu_self = rtems_scheduler_get_processor();

	interCoreManager_t icm = interCoreManagers[cpu_self];

  while(1){

  	locked_printf("interCoreTaskCounter = %d\n", icm.interCoreTaskCounter);

    rtems_task_wake_after(RTEMS_MILLISECONDS_TO_TICKS(argument));
  }
}

void init_interCoreManager(int period, int cpu){

	rtems_id           task_id;
	rtems_status_code  status;

	cpu_set_t          cpuset;

	interCoreManager_t icm;

	status = rtems_task_create(
		rtems_build_name( 'I','C','M',cpu),
		1,
		RTEMS_MINIMUM_STACK_SIZE,
		RTEMS_DEFAULT_MODES,
		RTEMS_DEFAULT_ATTRIBUTES,
		&task_id
	);
	if(status!=RTEMS_SUCCESSFUL){
		locked_printf("\n**rtems_task_create not sucessfull**\n");
		exit(1);
	}

	CPU_ZERO(&cpuset);
	CPU_SET(cpu, &cpuset);

	status = rtems_task_set_affinity(task_id, sizeof(cpuset), &cpuset);
		if(status!=RTEMS_SUCCESSFUL){
		locked_printf("\n**rtems_task_set_affinity not sucessfull**\n");
		exit(1);
	}

	status = rtems_task_start( task_id, processInterCoreQueues, period );
		if(status!=RTEMS_SUCCESSFUL){
		locked_printf("\n**rtems_task_start not sucessfull %d**\n", RTEMS_INVALID_ID);
		exit(1);
	}

	icm.id = task_id;
	icm.interCoreTaskCounter = 7;

	interCoreManagers[cpu] = icm;
}


uint32_t registerTask(uint32_t cpu, rtems_name task_name){

	interCoreManager_t icm = interCoreManagers[cpu];

	icm.interCoreTask[icm.interCoreTaskCounter] = task_name;

	icm.interCoreTaskCounter = icm.interCoreTaskCounter + 1;

	return icm.interCoreTaskCounter;
}