#include "task.h"


task_t init_task(char name[4], int period, int cpu, int receiver, rtems_name msgTo[], void* func){

	rtems_id           task_id, icm_id;
	rtems_status_code  status;
	rtems_name  	   task_name;

	cpu_set_t          cpuset;

	task_t  		   task;

	interCoreManager_t icm;

	task_name = rtems_build_name(name[0],name[1],name[2],name[3]);

	status = rtems_task_create(
		task_name,
		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, func, period );
		if(status!=RTEMS_SUCCESSFUL){
		locked_printf("\n**rtems_task_start not sucessfull %d**\n", RTEMS_INVALID_ID);
		exit(1);
	}

	task.id = task_id;
	task.msgTo = msgTo;

	if(receiver==0){

		task.icq = NULL;

	}
	else{

		interCoreQueue_t icq;
		task.icq = &icq;


		status = rtems_task_ident(
			rtems_build_name('I','C','M',cpu),
			RTEMS_SEARCH_ALL_NODES,
			&icm_id
		);
		if(status == RTEMS_INVALID_NAME){

			init_interCoreManager(CONFIGURE_ICQ_INTERCOREMANAGER_PERIOD, cpu);

		}

		status = registerTask(cpu, task_name);
		locked_printf("status = %d\n", status);
	}

	return task;
}