#include "icq_task.h"
#include "icq_manager.h"


typedef struct 
{
	icq_task_t 		cpus[CONFIGURE_MAXIMUM_PROCESSORS][CONFIGURE_MAXIMUM_TASKS];
	uint32_t		cpus_task_counter[CONFIGURE_MAXIMUM_PROCESSORS];
}icq_task_mapping_t;


icq_task_mapping_t tasks_mapping;

uint32_t icq_task_mapping_is_init = 0;

extern int icq_queue_post_message(icq_queue_t *icq, icq_message_t msg);

extern icq_message_t icq_queue_read_message(icq_queue_t *icq);

extern uint32_t icq_manager_register(uint32_t cpu, icq_task_t* task);



icq_task_t* icq_task_get_by_id(rtems_id task_id){

	icq_task_t* task;

	for(uint32_t i=0; i<CONFIGURE_MAXIMUM_PROCESSORS; i = i+1){
		
		for(uint32_t j=0; j<tasks_mapping.cpus_task_counter[i]; j = j+1){

			if(tasks_mapping.cpus[i][j].task_id == task_id){

				return &tasks_mapping.cpus[i][j];
				
			}
		
		}

	}

	return NULL;
}

icq_task_t* icq_task_get_by_name(char task_name[4]){

	icq_task_t* task;

	rtems_id task_id;
	rtems_status_code status;
	rtems_name name;


	name = rtems_build_name(task_name[0],task_name[1],task_name[2],task_name[3]);
	status = rtems_task_ident(
		name,
		RTEMS_SEARCH_ALL_NODES,
		&task_id
	);
	if (status == RTEMS_SUCCESSFUL) {
    	return icq_task_get_by_id(task_id);
  	}

	

	return NULL;
}

void icq_task_timer_handler(rtems_id timer_id, void *user_data) {
	
    rtems_id msg_queue_id;
    rtems_status_code status;

    icq_task_t* task =  (icq_task_t *)user_data;

    icq_message_t msg;

    memcpy(msg.sender_id, "timer", sizeof(msg.sender_id));
    msg.msg_number=-1;
    
    msg_queue_id = task->msg_queue_id;
	status = rtems_message_queue_send(msg_queue_id, (void*) &msg, sizeof(icq_message_t));
	if (status != RTEMS_SUCCESSFUL) {
		locked_printf("\n**rtems_message_queue_send not sucessfull**\n");
		exit(1);
	}

    status = rtems_timer_fire_after(timer_id, RTEMS_MILLISECONDS_TO_TICKS(task->period), icq_task_timer_handler, task);
    if (status != RTEMS_SUCCESSFUL) {
        locked_printf("Failed to restart timer: %s\n", rtems_status_text(status));
    }
}


icq_task_t* icq_task_map_to_cpu(icq_task_t *task, uint32_t cpu){

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

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

	}
	else if(task == NULL){

		locked_printf("task is NULL in icq_task_map_to_cpu\n");
		exit(1);

	}


	rtems_status_code  status;

	cpu_set_t          cpuset;

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

	status = rtems_task_set_affinity(task->task_id, sizeof(cpuset), &cpuset);
	if (status != RTEMS_SUCCESSFUL) {
    	locked_printf("Failed to set affiinty in rtems_task_set_affinity (%s)\n", rtems_status_text(status));
    	exit(1);
  	}

	uint32_t cpuTaskCounter = tasks_mapping.cpus_task_counter[cpu];

	task->cpu = cpu;

	tasks_mapping.cpus[cpu][cpuTaskCounter] = *task;

	tasks_mapping.cpus_task_counter[cpu] = tasks_mapping.cpus_task_counter[cpu] + 1;

	return &(tasks_mapping.cpus[cpu][cpuTaskCounter]);

}

icq_task_t* icq_task_init(char name[4], uint32_t period, uint32_t cpu, uint32_t receiver, void* func){

	rtems_id           task_id, icm_id, msg_queue_id, timer_id;
	rtems_status_code  status;
	rtems_name  	   task_name, msg_queue_name;

	icq_task_t 		   task;
	icq_task_t* 	   task_mapped;

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

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

	}
	else if(func == NULL){

		locked_printf("No function has been defined in icq_task_init (func parameter is NULL)\n");
		exit(1);

	}

	if(icq_task_mapping_is_init == 0){
		icq_task_mapping_is_init = 1;

		for(uint32_t i=0; i<CONFIGURE_MAXIMUM_PROCESSORS; i = i+1){
			tasks_mapping.cpus_task_counter[i] = 0;
		}
	}

	task_name = rtems_build_name(name[0],name[1],name[2],name[3]);
	status = rtems_task_ident(
		task_name,
		RTEMS_SEARCH_ALL_NODES,
		&task_id
	);
	if (status == RTEMS_SUCCESSFUL) {
    	locked_printf("You have tried to create a second \"%s\" object in icq_task_init\n", name);
    	exit(1);
  	}

	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("Failed to create task in icq_task_init (%s)\n", rtems_status_text(status));
    	exit(1);
  	}

	task.task_id = task_id;

	task_mapped = icq_task_map_to_cpu(&task, cpu);


	memcpy(task_mapped->name, name, sizeof(task_mapped->name));

	msg_queue_name = rtems_build_name(name[0],name[1],'M',name[3]);

	status = rtems_message_queue_create(
		msg_queue_name,
	    CONFIGURE_ICQ_INTERCOREQUEUE_SIZE*2,
	    sizeof(icq_message_t),
	    RTEMS_LOCAL|RTEMS_PRIORITY,
      	&msg_queue_id
	);
	if (status != RTEMS_SUCCESSFUL) {
    	locked_printf("Failed to create message_queue in icq_task_init (%s)\n", rtems_status_text(status));
    	exit(1);
  	}


	task_mapped->msg_queue_id = msg_queue_id;

	task_mapped->receiver_counter = 0;

	for(uint32_t i=0; i<CONFIGURE_ICQ_MAXIMUM_RECEIVERTASKS; i = i+1){
			task_mapped->receiver_tasks[i] = malloc(sizeof(icq_task_t));
			if(task_mapped->receiver_tasks[i] == NULL){
				locked_printf("Failed to allocate memory in icq_task_init\n");
				exit(1);
			}

			task_mapped->msg_sent[i] = 0;
	}	

	
	
	if(period>0){

		status = rtems_timer_create(rtems_build_name(name[0],name[1],'T',name[2]), &timer_id);
	    if (status != RTEMS_SUCCESSFUL) {
    		locked_printf("Failed to create timer in icq_task_init (%s)\n", rtems_status_text(status));
    		exit(1);
  		}

	    task_mapped->timer_id = timer_id;
	    task_mapped->period = period;
	}
	else{
		task_mapped->period = 0;
	}

	if(receiver==0){

#ifdef ICQ_SINGLE_QUEUE

			task_mapped->intercore_queue = NULL;

#endif

#ifdef ICQ_MULTI_QUEUE

		for(uint32_t k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k=k+1){

			task_mapped->intercore_queue[k] = 0;

		}
	
#endif

	}
	else{

#ifdef ICQ_SINGLE_QUEUE
		task_mapped->intercore_queue = icq_queue_init();

		icq_spinlock_init(&(task_mapped->intercore_queue->spinLock));

#endif

#ifdef ICQ_MULTI_QUEUE

		for(uint32_t k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k=k+1){

			task_mapped->intercore_queue[k] = 0;

		}
	
#endif

		status = rtems_task_ident(
			rtems_build_name('I','C','M',cpu+'0'),
			RTEMS_SEARCH_ALL_NODES,
			&icm_id
		);
		if(status == RTEMS_INVALID_NAME){
			icq_manager_init(CONFIGURE_ICQ_INTERCOREMANAGER_PERIOD, cpu);
		}

		icq_manager_register(cpu, task_mapped);

	}


	status = rtems_task_start( task_mapped->task_id, func, task_mapped->msg_queue_id );
	if (status != RTEMS_SUCCESSFUL) {
		locked_printf("Failed to start task in icq_task_init (%s)\n", rtems_status_text(status));
		exit(1);
	}

	task_mapped->task_report = icq_task_report_init();

	return task_mapped;
}


uint32_t icq_task_add_receiver(char sender_name[4], char receiver_name[4]){

	icq_task_t *sender_task, *receiver_task;

	sender_task = icq_task_get_by_name(sender_name);
	if (sender_task == NULL) {
		locked_printf("The task %s does not exist in icq_task_add_receiver\n", sender_name);
		exit(1);
	}

	receiver_task = icq_task_get_by_name(receiver_name);
	if (receiver_task == NULL) {
		locked_printf("The task %s does not exist in icq_task_add_receiver\n", receiver_name);
		exit(1);
	}


	if(sender_task->receiver_counter < CONFIGURE_ICQ_MAXIMUM_RECEIVERTASKS){
		sender_task->receiver_tasks[sender_task->receiver_counter] = receiver_task;
		sender_task->receiver_counter = sender_task->receiver_counter + 1;
	}

#ifdef ICQ_MULTI_QUEUE

	if(receiver_task->intercore_queue[sender_task->cpu] == NULL){

		receiver_task->intercore_queue[sender_task->cpu] = icq_queue_init();

		icq_spinlock_init(&(receiver_task->intercore_queue[sender_task->cpu]->spinLock));

	}

#endif

	return sender_task->receiver_counter;
}


uint32_t icq_task_send_message(char receiver_task_name[4]){

	icq_message_t msg;
	icq_task_t *sender_task, *receiver_task;

	rtems_status_code status;

	int status_post;

	char msg_sent_str[64];
	char msg_id[64];

	sender_task = icq_task_get_by_id(rtems_task_self());
	if (sender_task == NULL) {
		locked_printf("The id(%d) of the calling task does not correspond to one of the tasks of the communication system in icq_task_send_message\n", rtems_task_self());
		exit(1);
	}

	memcpy(msg.sender_id, sender_task->name, sizeof(msg.sender_id));
	
	for(uint32_t i=0; i<sender_task->receiver_counter; i = i+1){

		if(strcmp(sender_task->receiver_tasks[i]->name, receiver_task_name)==0){

#ifdef ICQ_SINGLE_QUEUE
			icq_queue_t* interCoreQueue = sender_task->receiver_tasks[i]->intercore_queue;
#endif

#ifdef ICQ_MULTI_QUEUE
			icq_queue_t* interCoreQueue = sender_task->receiver_tasks[i]->intercore_queue[sender_task->cpu];
#endif

			msg.msg_number = sender_task->msg_sent[i];

			status_post = icq_queue_post_message(interCoreQueue, msg);
			if(status_post == -1){
				locked_printf("InterCoreQueue of %s is full, the message is lost in icq_task_send_message\n", receiver_task_name);
			}

			sender_task->msg_sent[i] = sender_task->msg_sent[i] + 1;
		}

	}

	return 0;
}


void icq_task_process_intercore_queue(rtems_id task_id){

	icq_message_t 		msg;

	rtems_id 			msg_queue_id;
	rtems_status_code 	status;

	icq_task_t* task = icq_task_get_by_id(task_id);
	if (task == NULL) {
		locked_printf("The task %d does not exist in icq_task_process_intercore_queue\n", task_id);
		exit(1);
	}

#ifdef ICQ_SINGLE_QUEUE

	icq_spinlock_lock(task->intercore_queue->spinLock);

	uint32_t msg_counter = task->intercore_queue->msg_counter;

	for(uint32_t i=0; i<msg_counter; i = i+1){

		msg_queue_id = task->msg_queue_id;
		msg = icq_queue_read_message(task->intercore_queue);

		status = rtems_message_queue_send(msg_queue_id, (void*) &msg, sizeof(icq_message_t));
		if (status != RTEMS_SUCCESSFUL) {
			locked_printf("Failed to send message in icq_task_process_intercore_queue (%s)\n", rtems_status_text(status));
			exit(1);
		}

		task->intercore_queue->msg_received = task->intercore_queue->msg_received+1;
	}
	icq_spinlock_unlock(task->intercore_queue->spinLock);

#endif

#ifdef ICQ_MULTI_QUEUE
	for(uint32_t k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k=k+1){
		if(task->intercore_queue[k]!=0){
			icq_spinlock_lock(task->intercore_queue[k]->spinLock);
		}
	}

	for(uint32_t k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k=k+1){
		if(task->intercore_queue[k]!=0){
			uint32_t msg_counter = task->intercore_queue[k]->msg_counter;

			for(uint32_t i=0; i<msg_counter; i = i+1){

				msg_queue_id = task->msg_queue_id;
				msg = icq_queue_read_message(task->intercore_queue[k]);

				status = rtems_message_queue_send(msg_queue_id, (void*) &msg, sizeof(icq_message_t));
				if (status != RTEMS_SUCCESSFUL) {
					locked_printf("Failed to send message  in icq_task_process_intercore_queue (%s)\n",  rtems_status_text(status));
					exit(1);
				}

				task->intercore_queue[k]->msg_received = task->intercore_queue[k]->msg_received+1;
			}
		}
	}

	for(uint32_t k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k=k+1){
		if(task->intercore_queue[k]!=0){
			icq_spinlock_unlock(task->intercore_queue[k]->spinLock);
		}
	}

#endif

}


void icq_task_print_all_tasks_timing_execution_trace(){

	uint32_t k = 0;

	locked_printf("\n\t\t<<<--- TimingExecutionEntries --->>>");

	for(uint32_t i=0; i<CONFIGURE_MAXIMUM_PROCESSORS; i = i+1){
		
		for(uint32_t j=0; j<tasks_mapping.cpus_task_counter[i]; j = j+1){

			icq_task_report_print_timing_execution_trace(i, tasks_mapping.cpus[i][j].name, tasks_mapping.cpus[i][j].task_report);

		}

	}

	locked_printf("\n\t\t<<<--- trace ID --->>>");

	for(uint32_t i=0; i<CONFIGURE_MAXIMUM_PROCESSORS; i = i+1){
		
		for(uint32_t j=0; j<tasks_mapping.cpus_task_counter[i]; j = j+1){
#ifdef ICQ_SINGLE_QUEUE
			icq_task_report_print_timing_task_trace_id(i, tasks_mapping.cpus[i][j].name, tasks_mapping.cpus[i][j].intercore_queue, k);
#endif

#ifdef ICQ_MULTI_QUEUE
			for(k=0; k<CONFIGURE_MAXIMUM_PROCESSORS; k++){
				icq_task_report_print_timing_task_trace_id(i, tasks_mapping.cpus[i][j].name, tasks_mapping.cpus[i][j].intercore_queue[k], k);
			}
#endif

		}

	}


	locked_printf("\n\t\t<<<--- Stub --->>>");
	

}


void icq_task_ending_timer_handler(rtems_id timer_id, void *user_data) {
	
	rtems_status_code status;

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

		for(uint32_t j=0; j<tasks_mapping.cpus_task_counter[i]; j = j+1){

			rtems_timer_cancel(tasks_mapping.cpus[i][j].timer_id);
			if (status != RTEMS_SUCCESSFUL) {
				locked_printf("Failed to cancel timer in icq_task_ending_timer_handler (%s)\n", rtems_status_text(status));
				exit(1);
			}

			rtems_task_suspend(tasks_mapping.cpus[i][j].task_id);

		}

	}

	locked_printf("\nEnd of the Execution\n");

	icq_task_print_all_tasks_timing_execution_trace();

	rtems_task_exit();
}



void icq_task_launch_application(void){

	icq_task_t* 			task;
	
	rtems_status_code 		status;
	rtems_id 				timer_id;

	for(uint32_t i=0; i<CONFIGURE_MAXIMUM_PROCESSORS; i = i+1){
		
		for(uint32_t j=0; j<tasks_mapping.cpus_task_counter[i]; j = j+1){
			if(tasks_mapping.cpus[i][j].period>0){

				status = rtems_timer_fire_after(tasks_mapping.cpus[i][j].timer_id, RTEMS_MILLISECONDS_TO_TICKS(tasks_mapping.cpus[i][j].period), icq_task_timer_handler, &tasks_mapping.cpus[i][j]);
			    if (status != RTEMS_SUCCESSFUL) {
		    		locked_printf("Failed to fire timer in icq_task_launch_application (%s)\n", rtems_status_text(status));
		    		exit(1);
		  		}
		  	}

		}

	}


	status = rtems_timer_create(rtems_build_name('E','N','D','T'), &timer_id);
    if (status != RTEMS_SUCCESSFUL) {
		locked_printf("Failed to create timer ENDT in icq_task_launch_application (%s)\n", rtems_status_text(status));
		exit(1);
	}

	status = rtems_timer_fire_after(timer_id, RTEMS_MILLISECONDS_TO_TICKS(CONFIGURE_ICQ_EXECUTION_TIME), icq_task_ending_timer_handler, NULL);
    if (status != RTEMS_SUCCESSFUL) {
		locked_printf("Failed to fire timer ENDT in icq_task_launch_application (%s)\n", rtems_status_text(status));
		exit(1);
	}

	while(1);

}