#include "icq_manager.h"
#include "icq_task_report.h"


/* Global variables */
icq_manager_t* icq_manager_tasks[CONFIGURE_MAXIMUM_PROCESSORS];

uint32_t icq_manager_is_init =0;

extern void icq_task_process_intercore_queue(rtems_id taskid);


rtems_task icq_manager_process_intercorequeues(
  rtems_task_argument argument
)
{
	rtems_status_code 		status;
	rtems_id 							msg_queue_id = argument;

	uint32_t 							cpu_self = rtems_scheduler_get_processor();

	icq_manager_t* 				icm_self = icq_manager_tasks[cpu_self];

  icq_message_t   			msg;
  size_t          			msg_size;



  do{

    status =rtems_message_queue_receive(msg_queue_id, &msg, &msg_size, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
    if(status!=RTEMS_SUCCESSFUL){
    	locked_printf("Failed to receive message in icq_manager_process_intercorequeues (%s)\n", rtems_status_text(status));
    	exit(1);
    }
    icq_task_report_add_event(icm_self->task->task_report, EXECUTING);

    icq_task_report_add_event(icm_self->task->task_report, MESSAGE_PROCESSING_START);
  	for(uint32_t i=0; i<icm_self->intercore_task_counter; i++){

  		icq_task_t *task = icm_self->intercore_tasks[i];

  		icq_task_process_intercore_queue(task->task_id);

  	}
  	icq_task_report_add_event(icm_self->task->task_report, MESSAGE_PROCESSING_END);


  	icq_task_report_add_event(icm_self->task->task_report, STOPPED);

  }while(1);
  
}


uint32_t icq_manager_register(uint32_t cpu, icq_task_t* task){

	uint32_t counter = icq_manager_tasks[cpu]->intercore_task_counter;

	if(counter<CONFIGURE_ICQ_MAXIMUM_INTERCORETASKS){

		icq_manager_tasks[cpu]->intercore_tasks[counter] = task;

		icq_manager_tasks[cpu]->intercore_task_counter = ++counter;

	}

	return counter;
}


void icq_manager_init(uint32_t period, uint32_t cpu){

	
	rtems_status_code  status;
	rtems_name 				 icm_task_name, msg_queue_name;

	cpu_set_t          cpu_set;

	char icm_name[4];
	char cpu_str; 



	if(cpu < 0 || cpu >= CONFIGURE_MAXIMUM_PROCESSORS){

		locked_printf("CPU%d does not exist, your system has %d CPU (0 to %d) in icq_manager_init\n", cpu, CONFIGURE_MAXIMUM_PROCESSORS, CONFIGURE_MAXIMUM_PROCESSORS-1);
		exit(1);

	}

	if(icq_manager_is_init==0){

		icq_manager_is_init=1;

		for(uint32_t i=0;i<CONFIGURE_MAXIMUM_PROCESSORS;i++){

			icq_manager_tasks[i] = malloc(sizeof(icq_manager_t));
			if(icq_manager_tasks[i]==NULL){
				locked_printf("Failed to allocate memory in icq_manager_init\n");
				exit(1);
			}
		
		}
	
	}

	icm_name[0] = 'I';
	icm_name[1] = 'C';
	icm_name[2] = 'M';
  cpu_str = cpu + '0'; 
  icm_name[3] = cpu_str;

	icq_task_t *icm_task = icq_task_init(icm_name, period, cpu,  0, &icq_manager_process_intercorequeues);


	icq_manager_tasks[cpu]->task = icm_task;

	icq_manager_tasks[cpu]->intercore_task_counter = 0;


}