int main(void)
{
  RT_TASK *task;
  RTIME now;
  int cnt=0;

  // make main thread LXRT soft realtime
  task = rt_task_init_schmod(nam2num("MYTASK"), 9, 0, 0, SCHED_FIFO, 0xF);
  mlockall(MCL_CURRENT | MCL_FUTURE);

  // start realtime timer and scheduler
  //rt_set_oneshot_mode();
  rt_set_periodic_mode();
  start_rt_timer(0);

  now = rt_get_time() + 10*PERIOD;
  rt_task_make_periodic(task, now, PERIOD);

  printf("Init mutex and cond.\n");
  mutex = rt_sem_init(nam2num("MUTEX"), 1);

  if (mutex==0)
    printf("Error init mutex\n");

  cond = rt_cond_init(nam2num("CONDITION"));

  if (cond==0)
    printf("Error init cond\n");

  thread0 = rt_thread_create(fun0, NULL, 10000);
  //thread1 = rt_thread_create(fun1, NULL, 20000);

  //rt_sleep(PERIOD);

  while (cnt < THRESHOLD) {
    rt_task_wait_period();
    rt_printk("main: Hello World %d!\n",cnt);
    rt_sem_wait(mutex); //now the mutex should have value 0

    if (instance_cnt==0) {
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("worker thread busy!\n");
    } else {
      instance_cnt++;
      rt_cond_signal(cond);
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("signaling worker thread to start!\n");
    }

    cnt++;
  }

  // wait for end of program
  printf("TYPE <ENTER> TO TERMINATE\n");
  getchar();

  // cleanup
  stop_rt_timer();
  return 0;
}

int init_module(void)
{
    RTIME tick_period;
    int err;

    printk("%s: Loading RX%d Test module...\n", test_name, rt_port);

    err = rt_spopen(rt_port, 
		    line_param[0], line_param[1], line_param[2], 
		    line_param[3], line_param[4], line_param[5]);
    if (err) return err;
	
    /* Register error handler */
    rt_spset_err_callback_fun(rt_port, error_handler);

    /* Start RX message handler */
    rt_task_init(&thread, rx_handler, 0, STACK_SIZE, 0, 0, 0);
    rt_task_resume(&thread);

    rt_set_oneshot_mode();
    if (period) {
	tick_period = start_rt_timer(nano2count(period));
	rt_task_make_periodic(&thread, rt_get_time(), tick_period);
    }
    return 0;
}

int main(void)
{
	struct sched_param mysched;

	mysched.sched_priority = sched_get_priority_max(SCHED_FIFO);
	if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
		puts(" ERROR IN SETTING THE SCHEDULER UP");
		perror("errno");
		exit(0);
 	}       
	rt_grow_and_lock_stack(20000);

#ifdef ONE_SHOT
	rt_set_oneshot_mode();
#endif
	start_rt_timer(nano2count(TICK_PERIOD));
	firing_time = rt_get_time();
	period = nano2count(1000000);
	prt = rt_init_timer();
	rt_insert_timer(prt, 1, firing_time, period, prh, 0, 1);
	rt_tasklet_use_fpu(prt, 1);
	seqt = rt_init_timer();
	rt_insert_timer(seqt, 0, firing_time, 0, seqh, 0, 1);
	while(!end) sleep(1);
	stop_rt_timer();
	rt_delete_timer(prt);
	rt_delete_timer(seqt);
	return 0;
}

static int my_init(void) {
    int i, ierr1, ierr2;
    rt_typed_sem_init(&semaphore1, 0, BIN_SEM);
    rt_typed_sem_init(&semaphore2, 0, BIN_SEM);

    rt_set_oneshot_mode();
    ierr1 = rt_task_init_cpuid(&tasks[0], task_body1, 0, STACK_SIZE, 1, 0, 0, 0);
    ierr2 = rt_task_init_cpuid(&tasks[1], task_body2, 0, STACK_SIZE, 0, 0, 0, 0);

    printk("[task 1] init return code %d by program %s\n", ierr1, __FILE__);
    printk("[task 2] init return code %d by program %s\n", ierr2, __FILE__);

    if (ierr1 == -1 || ierr2 == -1) {
        return -1;
    }

    start_rt_timer(nano2count(TICK_PERIOD));

    first_release = rt_get_time();
    for (i = 0 ; i < N_TASK ; i++) {
        rt_task_make_periodic(&tasks[i], first_release, PERIOD);
    }

    return 0;
}

void *signalNi(void *arg)
{
	RT_TASK *Task_2;

	unsigned long Task_2_name = nam2num("TSK_2") + i++;
	time_t aclock;
	time_t clockNow;
	int timeEx, begin_2;
	struct tm *newtime;
	int count = 0;

	Task_2 = rt_task_init(Task_2_name, 0, 0, 0);

//	if(!(Task_2 = rt_task_init_schmod(Task_2_name,1,0,0,SCHED_FIFO,1))) {
//		printf("CANNOT INIT HANDLER TASK > Task 2 <\n");
//		exit(1);
//	}
	rt_allow_nonroot_hrt();

        rt_make_hard_real_time();

	rt_task_make_periodic(Task_2, rt_get_time(), sampling * 18);
	rt_change_prio(Task_2, 1);


	begin_2 = begin;
	while (count < 20) {
		rt_sem_wait(rMutex);
		time(&aclock); // Pega tempo em segundos.
		newtime = localtime(&aclock);


		printf(" Signal 2 =======> %s", asctime(newtime));

		sleep(1);

		time(&aclock); // Pega tempo em segundos.
		newtime = localtime(&aclock);

		printf(" Signal 2 after Sleep =======> %s", asctime(newtime));

		timeEx = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
		if( (timeEx - begin_2) > 9 )
			printf(" Time Failure of the Signal 2\n");
		else printf(" Time Correct of the Signal 2\n");
		begin_2 = timeEx + (9 - (timeEx-begin)%9);

		rt_sem_signal(rMutex);

		rt_task_wait_period();
		count++;
	}

	rt_make_soft_real_time();
	rt_task_delete(Task_2);

	return 0;

}

int init_module(void)
{
	RTIME period;
	rt_task_init(&agentask, fun, 0, STACK_SIZE, 0, 0, 0);
	rt_set_oneshot_mode();
	period = start_rt_timer((int) nano2count(TICK_PERIOD));
	rt_task_make_periodic(&agentask, rt_get_time() + period, period);
	return 0;
}

void *thread_fun(void *arg)
{
	RTIME start_time, period;
	RTIME t0, t;
	SEM *sem;
	RT_TASK *mytask;
	unsigned long mytask_name;
	int mytask_indx, jit, maxj, maxjp, count;

	mytask_indx = *((int *)arg);
	mytask_name = taskname(mytask_indx);
	cpus_allowed = 1 - cpus_allowed; 
 	if (!(mytask = rt_task_init_schmod(mytask_name, 1, 0, 0, SCHED_FIFO, 1 << cpus_allowed))) {
		printf("CANNOT INIT TASK %lu\n", mytask_name);
		exit(1);
	}
	printf("THREAD INIT: index = %d, name = %lu, address = %p.\n", mytask_indx, mytask_name, mytask);
	mlockall(MCL_CURRENT | MCL_FUTURE);

 	if (!(mytask_indx%2)) {
		rt_make_hard_real_time();
	}
	rt_receive(0, (unsigned long *)((void *)&sem));

	period = nano2count(PERIOD);
	start_time = rt_get_time() + nano2count(10000000);
	rt_task_make_periodic(mytask, start_time + (mytask_indx + 1)*period, ntasks*period);

// start of task body
	{
		count = maxj = 0;
		t0 = rt_get_cpu_time_ns();
		while(count++ < LOOPS) {
			rt_task_wait_period();
			t = rt_get_cpu_time_ns();
			if ((jit = t - t0 - ntasks*(RTIME)PERIOD) < 0) {
				jit = -jit;
			}
			if (count > 1 && jit > maxj) {
				maxj = jit;
			}
			t0 = t;
//			rtai_print_to_screen("THREAD: index = %d, count %d\n", mytask_indx, count);
		}
		maxjp = (maxj + 499)/1000;
	}
// end of task body

	rt_sem_signal(sem);
 	if (!(mytask_indx%2)) {
		rt_make_soft_real_time();
	}

	rt_task_delete(mytask);
	printf("THREAD %lu ENDS, LOOPS: %d MAX JIT: %d (us)\n", mytask_name, count, maxjp);
	return 0;
}

static void *latency_fun(void *arg)
{
	struct sample { long min, max, avrg, jitters[2]; } samp;
	int diff;
	int skip;
	int average;
	int min_diff;
	int max_diff;
	int period;
	RT_TASK *chktsk;
	RTIME expected;

	min_diff = 1000000000;
	max_diff = -1000000000;
	if (!(Latency_Task = rt_thread_init(nam2num("PRETSK"), 0, 0, SCHED_FIFO, CPUMAP))) {
		printf("CANNOT INIT LATENCY TASK\n");
		exit(1);
	}

	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();
	rt_sem_wait_barrier(barrier);
	period = nano2count(TICK_TIME);
	expected = start + 3*period;
	rt_task_make_periodic(Latency_Task, expected, period);
	while (!end) {
		average = 0;
		for (skip = 0; skip < NAVRG && !end; skip++) {
			expected += period;
			END("HE\n");
			rt_task_wait_period();
			BEGIN("HB\n");
			diff = count2nano(rt_get_time() - expected);
			if (diff < min_diff) {
				min_diff = diff;
			}
			if (diff > max_diff) {
				max_diff = diff;
			}
			average += diff;
		}
		samp.min = min_diff;
		samp.max = max_diff;
		samp.avrg = average/NAVRG;
		samp.jitters[0] = fastjit;
		samp.jitters[1] = slowjit;
		if ((chktsk = rt_get_adr(nam2num("PRECHK")))) {
			rt_sendx_if(chktsk, &samp, sizeof(samp));
		}
	}
	rt_sem_wait_barrier(barrier);
	rt_make_soft_real_time();
	rt_thread_delete(Latency_Task);
	return 0;
}

static int
__latency_init(void)
{

	/* XXX check option ranges here */

	/* register a proc entry */
#ifdef CONFIG_PROC_FS
	create_proc_read_entry("rtai/latency_calibrate", /* name             */
	                       0,			 /* default mode     */
	                       NULL, 			 /* parent dir       */
			       proc_read, 		 /* function         */
			       NULL			 /* client data      */
	);
#endif

	rtf_create(DEBUG_FIFO, 16000);	/* create a fifo length: 16000 bytes */
	rt_linux_use_fpu(use_fpu);	/* declare if we use the FPU         */

	rt_task_init(			/* create our measuring task         */
			    &thread,	/* poiter to our RT_TASK             */
			    fun,	/* implementation of the task        */
			    0,		/* we could transfer data -> task    */
			    3000,	/* stack size                        */
			    0,		/* priority                          */
			    use_fpu,	/* do we use the FPU?                */
			    0		/* signal? XXX                       */
	);

	rt_set_runnable_on_cpus(	/* select on which CPUs the task is  */
		&thread,		/* allowed to run                    */
		RUN_ON_CPUS
	);

	/* Test if we have to start the timer                                */
	if (start_timer || (start_timer = !rt_is_hard_timer_running())) {
		if (timer_mode) {
			rt_set_periodic_mode();
		} else {
			rt_set_oneshot_mode();
		}
		rt_assign_irq_to_cpu(TIMER_8254_IRQ, TIMER_TO_CPU);
		period_counts = start_rt_timer(nano2count(period));
	} else {
		period_counts = nano2count(period);
	}

	loops = (1000000000*avrgtime)/period;

	/* Calculate the start time for the task. */
	/* We set this to "now plus 10 periods"   */
	expected = rt_get_time() + 10 * period_counts;
	rt_task_make_periodic(&thread, expected, period_counts);
	return 0;
}

int init_module(void)
{
	rt_task_init(&thread, fun, 0, 3000, 0, 0, 0);
	rt_set_oneshot_mode();
	period = start_rt_timer(nano2count(PERIOD));
	expected = rt_get_time() + 10*period;
	rt_task_make_periodic(&thread, expected, period);
//	rt_printk("\n\n*** 'LATENCY_8254 IN USE %d", LATENCY_8254);
	printk("\n*** Wait %d seconds for it ... ***\n\n", (int)(((long long)SKIP*(long long)PERIOD)/1000000000));
	return 0;
}

static void spv(long loops)
{
	int skip, average = 0;
	for (skip = 0; skip < loops; skip++) {
		expected += period;
		rt_task_wait_period();
		average += (int)count2nano(rt_get_time() - expected);
	}
	rtf_put(0, &average, sizeof(average));
	rt_task_suspend(0);
}

static void *rt_BaseRate(void *args)
{
	char name[7];
	int i;
	static RTIME t0;


	for (i = 0; i < MAX_NTARGETS; i++) {
		sprintf(name,"BRT%d",i);
		if (!rt_get_adr(nam2num(name))) break;
	}
	if (!(rt_BaseRateTask = rt_task_init_schmod(nam2num(name), *((int *)args), 0, 0, SCHED_FIFO, CpuMap))) {
		fprintf(stderr,"Cannot init rt_BaseRateTask.\n");
		return (void *)1;
	}

	sem_post(&err_sem);

	iopl(3);
	rt_task_use_fpu(rt_BaseRateTask, 1);

	MXmain();
	grow_and_lock_stack(stackinc);
	if (UseHRT) {
		rt_make_hard_real_time();
	}

	rt_rpc(rt_MainTask, 0, (void *)name);
	t0 = rt_get_cpu_time_ns();
	rt_task_make_periodic(rt_BaseRateTask, rt_get_time() + rt_BaseRateTick, rt_BaseRateTick);
	while (!endBaseRate) {
#ifdef TASKDURATION
		RTTSKper=rt_get_cpu_time_ns()-RTTSKinit;
#endif
		WaitTimingEvent(TimingEventArg);

		if (endBaseRate) break;
		APPLICATION_Process_Event(TIME_EV);

		SIM_TIME = (rt_get_cpu_time_ns() - t0)*1.0E-9;
#ifdef TASKDURATION
		RTTSKinit=rt_get_cpu_time_ns();
#endif

	}
	if (UseHRT) {
		rt_make_soft_real_time();
	}

	rt_task_delete(rt_BaseRateTask);

	return 0;
}

void parent_func(int arg) 
{
    int i;

    rt_printk("Starting parent task %d\n", arg);
    for (i = 0; i < NUM_CHILDREN; i++) {
    	rt_task_init(&child_task[i], child_func, i, STACK_SIZE, PRIORITY, 0, 0);
	rt_set_task_trap_handler(&child_task[i], 14, my_trap_handler);
	rt_task_make_periodic(&child_task[i], rt_get_time() + period, period*i);
    }
    rt_task_suspend(rt_whoami());
}

int init_module(void)
{
#ifdef ONE_SHOT
	rt_set_oneshot_mode();
#endif
	firing_time = rt_get_time() + nano2count(100000000);
	period = nano2count(TICK_PERIOD);
	start_rt_timer(period);
	rt_insert_timer(&prt, 1, firing_time, period, prh, 0xAAAAAAAA, 0);
	rt_insert_timer(&ost, 0, firing_time, nano2count(PERIODIC_BUDDY*TICK_PERIOD/2), osh, 0xBBBBBBBB, 0);
	return 0;
}

int main(void)
{
	RT_TASK *spktsk, *plrtsk;
	RTIME period;
	MBX *mbx;
	char data, temp;
	unsigned int msg, i;

//	ioperm(PORT_ADR, 1, 1);
	iopl(3);
        if (!(spktsk = rt_task_init_schmod(nam2num("SPKTSK"), 1, 0, 0, SCHED_FIFO, 0xF))) {
                printf("CANNOT INIT SPEAKER TASK\n");
                exit(1);
        }
        mbx = rt_mbx_init(nam2num("SNDMBX"), 4000);
	printf("\nSPEAKER TASK RUNNING\n");

	rt_set_oneshot_mode();
	start_rt_timer(0);
	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();

	period = nano2count(PERIOD);
	rt_task_make_periodic(spktsk, rt_get_time() + 5*period, period);

        for (i = 0; i < 100; i++) {
		plrtsk = rt_receive(0, &msg);
		rt_return(plrtsk, msg);
        }

	while(1) {
		if (!rt_mbx_receive_if(mbx, &data, 1)) {
			data = filter(data);
			temp = inb(PORT_ADR);            
			temp &= 0xfd;
			temp |= (data & 1) << 1;
			outb(temp, PORT_ADR);
		}
		rt_task_wait_period();
		if ((plrtsk = rt_receive_if(0, &msg))) {
			rt_return(plrtsk, msg);
			break;
		} 
	}

	rt_sleep(nano2count(100000000));
	rt_make_soft_real_time();
	rt_mbx_delete(mbx);
	stop_rt_timer();
	rt_task_delete(spktsk);
	printf("\nSPEAKER TASK STOPS\n");
	return 0;
}

int init_module(void)
{
	RTIME tick_period;
	rtf_create_using_bh(CMDF, 4, 0);
#ifdef ONE_SHOT
	rt_set_oneshot_mode();
#endif
	tick_period = start_rt_timer(nano2count(TICK_PERIOD));
	rt_task_init(&thread, intr_handler, 0, STACK_SIZE, 0, 0, 0);
	rt_task_make_periodic(&thread, rt_get_time() + 2*tick_period, tick_period);
	return 0;
}

void task_body(long int arg) {
     int loop = N_LOOP;

	rt_task_set_resume_end_times(-0, -Period[arg]*time_unit);

    while (loop--) {
        int i;
		RTIME begin, end;

		begin = rt_get_time();
        rt_printk("Start task %d\t  %llu ns\n", arg, count2nano(begin-first_release));
        for(i = 0; i < C[arg] ; i++) {
            run_for_1_time_unit();
        }
        end = rt_get_time();
        rt_printk("End task %d\t  %llu ns\n", arg, count2nano(end-first_release));
        rt_printk("Task length %d\t  %llu ns\n", arg, count2nano(end-begin));

		rt_task_set_resume_end_times(-Period[arg]*time_unit, -Period[arg]*time_unit);
    }
}

int create_tasks(void)
{
	int i;
	struct thread_param *arrayThreadParams[NTASKS];

	rt_set_periodic_mode();

	rt_hard_timer_tick_cpuid(CPU_ALLOWED);

	printf("************** Iniciando escalonamento **************\n");

	for (i = 0; i < NTASKS; i++)
	{
		arrayThreads[i] = (pthread_t *) malloc(sizeof(pthread_t));
		if (!arrayThreads[i])
		{
			printf("[ERRO] Não foi possivel criar a Thread da tarefa %d.\n\n", i);
			return(0);
		}
	}

	tick_period = start_rt_timer(nano2count(TICK_PERIOD));
	printf("TICK_PERIOD =======> %llu\n", tick_period);

	delay_timeline_sched = tick_period * 10;
	timeline_sched = rt_get_time() + delay_timeline_sched;

	for (i = 0; i < NTASKS; i++) {
		if((arrayThreadParams[i] = malloc(sizeof(*arrayThreadParams[i]))) == NULL)
		{
			printf("[ERRO] Não foi possivel criar os parametros da tarefa %d.\n\n", i);
			return (-1);
		}
		arrayThreadParams[i]->idTask = i;

		// Inicializando as tarefas...
		if(pthread_create(arrayThreads[i], 0, init_task, (void *)arrayThreadParams[i]))
		{
			printf("[ERRO] Não foi possível inicializar a Thread da tarefa %d.\n", i);
			return(0);
		}
	}

	while(!getchar()); // Aguardo o usuario apertar alguma tecla para finalizar o escalonamento...

	for (i = 0; i < NTASKS; i++) {
		pthread_cancel((pthread_t) *arrayThreads[i]);
		free(arrayThreadParams[i]);
	}

	return 0;
}

void seqh(unsigned long data)
{
	switch (opcod) {
		case 0: {
			rt_printk("- SEQUENCER FIRED FOR A FAST POLLING\n");
			rt_insert_timer(seqt, 10, rt_get_time() + nano2count(500000), nano2count(500000), seqh, 0, 1);
			opcod = 1;
		}
		break;
		case 1: {
			if (poloops++ < LOOPS) {
				if (poloops == 1) {
					rt_printk("- SEQUENCER BEGINS FAST POLLING\n");
				}
			} else {
				rt_printk("- SEQUENCER FAST POLLING ENDED, TIMED FOR A LATER FINAL OPERATION\n");
				rt_set_timer_firing_time(seqt, rt_get_time() + nano2count(50000000));
				opcod = 2;
			}
		}
		break;
		case 2: {
			rt_printk("- SEQUENCER EXECUTES FINAL OPERATION, ... THEN\n  SLOWLY POLLS WAITING FOR THE END OF THE PERIODIC COMPUTATIONAL TASKLET\n");
				opcod = 3;
		}
		break;
		case 3: {
			if (endp) {
				rt_printk("- PERIODIC COMPUTATIONAL TASKLET GONE, SEQUENCER ENDS IT ALL\n");
				rt_remove_timer(seqt);
				end = 1;
			} else {
//				rt_printk("\nSEQUENCER WAITING FOR PERIODIC TIMER END");
			}
		}
		break;
	}
}

void actcode(int arg) {
  RTIME t, t_old;

    while (1) {
        rt_sem_wait(&sensDone); // wait for sensor acquisition
        t = rt_get_time();
        printk("[act_task] time: %llu ms\n", count2nano(t - now));
        /* controller code */
        //ctrlcode(currentAngle, currentPosition);
    	//setDA_raw(1, (int) command_u16);
        //ctrlcode(0.1, 1.0);
        /* end of controller code */
        t_old = t;
    }
}