//-----------------------------------------------------------------------------
void child_func(int arg) {

int i;
size_t n = 0;
char inBuf[50];
uint priority = 0;
int nMsgs = 0;
struct mq_attr my_attrs ={0,0,0,0};
mode_t my_mode = 0;
int my_oflags = (O_RDONLY | O_NONBLOCK);
mqd_t rx_q = INVALID_PQUEUE;


  rt_printk("Starting child task %d\n", arg);
  for(i = 0; i < 3; i++) {

    rt_printk("child task %d, loop count %d\n", arg, i);

  }
  //Open the queue for reading
  rx_q = mq_open("my_queue", my_oflags, my_mode, &my_attrs);
  if (rx_q <= 0) {
    rt_printk("ERROR: child cannot open my_queue\n");
  } else {

    //Get the message(s) off the pQueue
    n = mq_getattr(rx_q, &my_attrs);
    nMsgs = my_attrs.mq_curmsgs;
    rt_printk("There are %d messages on the queue\n", nMsgs);

    while(nMsgs-- > 0) {
      n = mq_receive(rx_q, inBuf, sizeof(inBuf), &priority);
      inBuf[n] = '\0';

      //Report what happened
      rt_printk("Child got >%s<, %d bytes at priority %d\n", inBuf,n, priority);
    }
  }

  mq_close(rx_q);
  mq_unlink("my_queue");
  free_z_apps(rt_whoami());
  rt_task_delete(rt_whoami());

}

int main(void)
{
	RT_TASK *mytask;
	MBX *Keyboard, *Screen;
	unsigned int i;
	char d = 'd';
	char chain[12];
	char displine[40] = "CLOCK-> 00:00:00  CHRONO-> 00:00:00";
	
	signal(SIGINT, endme);
 	if (!(mytask = rt_task_init(nam2num("SCRTSK"), 20, 0, 0))) {
		printf("CANNOT INIT SCREEN TASK\n");
		exit(1);
	}

 	if (!(Keyboard = rt_get_adr(nam2num("KEYBRD")))) {
		printf("CANNOT FIND KEYBOARD MAILBOX\n");
		exit(1);
	}

	if (rt_mbx_send(Keyboard, &d, 1) > 0 ) {
		fprintf(stderr, "Can't send initial command to RT-task\n");
		exit(1);
	}

 	if (!(Screen = rt_get_adr(nam2num("SCREEN")))) {
		printf("CANNOT FIND SCREEN MAILBOX\n");
		exit(1);
	}

	while(1) {
		rt_mbx_receive(Screen, chain, 12);
		if (chain[0] == 't') {
			for (i = 0; i < 11; i++) {
				displine[i+27] = chain[i+1];
			}
		} else if (chain[0] == 'h') {
			for (i = 0; i < 8; i++) {
				displine[i+8] = chain[i+1];
			}
		}
		printf("%s\n", displine);
	}
	rt_task_delete(mytask);
}

void *ThreadImplLxrt33::runThread(void *arg)
{
  ThreadImplLxrt33 *self = static_cast<ThreadImplLxrt33*>(arg);

  self->m_rt_task = rt_task_init(getpid() + pthread_self_rt(), abs(self->m_priority),
                                 DEFAULT_STACK_SIZE, 0);
  if (self->m_rt_task == NULL)
  {
    PRINTF("ERROR: Cannot initialize LXRT task %lu!\n", self->m_thread_id);
    PRINTF("       Probably another thread with the same name already exists.\n");
  }
  else
  {
    rt_task_use_fpu(self->m_rt_task, 1);

    if (self->m_priority < 0)
    {
      rt_make_hard_real_time();
      if (!rt_is_hard_real_time(rt_buddy()))
      {
        PRINTF("ERROR: Setting thread %lu to hard real-time failed!\n", self->m_thread_id);
      }
      else
      {
        // Everything worked as expected, so no message here.
      }
    }
    else
    {
      // This is a soft realtime thread, so nothing additional has to
      // be done here.
    }

    self->m_thread->runThread();

    rt_make_soft_real_time();

    // TODO: Check if this is correct. The RTAI 3.5 and 3.8
    // implementations leave this to a call to join().
    rt_task_delete(self->m_rt_task);
    self->m_rt_task = NULL;
  }

  return NULL;
}

static void cleanup(void)
{
    int ret;

    if (verbose)
	printf("Cleaning up...\n");

    usleep(100000);

    if (s >= 0) {
	ret = rt_dev_close(s);
	s = -1;
	if (ret) {
	    fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret));
	}
	rt_task_delete(&rt_task_desc);
    }
}

/****
 * Initialize the realtime txqueue wakeup manager and its mailbox.
 */
int rt_wakeup_mgr_init (struct rtnet_mgr *mgr) {
	int ret = 0;

	if ( (ret=rt_mbx_init (&(mgr->mbx), sizeof(struct rtnet_msg))) )
		return ret;
	if ( (ret=rt_task_init(&(mgr->task), &rt_wakeup_manager_task, (int)mgr, 4096, RTNET_RT_WAKEUP_PRIORITY, 0, 0)) )
		goto rt_mbox_err;
	if ( (ret=rt_task_resume(&(mgr->task))) )
		goto rt_task_err;

	return (ret);

 rt_task_err:
	rt_task_delete(&(mgr->task));
 rt_mbox_err:
	rt_mbx_delete(&(mgr->mbx));
	return (ret);
}

void cleanup_module(void)
{
	int cpuid;
	rt_reset_irq_to_sym_mode(0);
	CLEAR_8254_TSC_EMULATION;
	rt_free_timer();
	rtf_destroy(CMDF);
	if (Mode) {
		rt_sem_delete(&sem);
	}
	rt_task_delete(&thread);
	printk("\n\nCPU USE SUMMARY\n");
	for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
		printk("# %d -> %d\n", cpuid, cpu_used[cpuid]);
	}
	printk("# ints -> %d\n",  cpu_used[NR_RT_CPUS]);
	printk("END OF CPU USE SUMMARY\n\n");
}

void task2(void)
{
	int i;

	rt_thread_init(nam2num("TASK2"), 2, 0, SCHED_FIFO, 0x1);
	rt_grow_and_lock_stack(STACK_SIZE - 10000);
#ifdef MAKE_HARD
	MAKE_HARD();
#endif	
	rt_printk("TASK2 TID = %d.\n\n", rt_gettid());

	rt_printk("TESTING FAILING WAIT IF ......");
	for (i = 0; i < LOOPS; i++) {
		if (rt_sem_wait_if(sem2) > 0) {
			break;
		}
	}
	rt_printk(" %s OK.\n", i == LOOPS ? "" : "NOT");

	rt_printk("TESTING SUCCEEDING WAIT IF ...");
	rt_sem_signal(sem2);
	for (i = 0; i < LOOPS; i++) {
		if (rt_sem_wait_if(sem2) == 1) {
			rt_sem_signal(sem2);
		} else {
			break;
		}
	}
	rt_printk(" %s OK.\n", i == LOOPS ? "" : "NOT");

	rt_printk("TESTING WAIT/SIGNAL ..........");
	rt_sem_wait(sem2);
	for (i = 0; i < LOOPS; i++) {
		rt_sem_signal(sem1);
		if (rt_sem_wait(sem2) == 0) {
			break;
		}
	}
	rt_printk(" %s OK.\n", i == LOOPS ? "" : "NOT");

        rt_task_delete(NULL);
	rt_printk("\nTASK2 EXITING : ");
	return;
}

int main(int argc, char *argv[])
{
        RT_TASK *task;
        struct sockaddr_in addr;
	int i, srvport;

        if (!(task = rt_task_init(nam2num("STRTSK"), 0, 0, 0))) {
                printf("CANNOT INIT START_TASK TASK\n");
                exit(1);
        }

        comnode = tasknode = 0;
        for (i = 0; i < argc; i++) {
                if (strstr(argv[i], "ComNode=")) {
                	inet_aton(argv[i] + 8, &addr.sin_addr);
	                comnode = addr.sin_addr.s_addr;
                        argv[i] = 0;
                        continue;
                }
                if (strstr(argv[i], "TaskNode=")) {
                	inet_aton(argv[i] + 9, &addr.sin_addr);
	                tasknode = addr.sin_addr.s_addr;
                        argv[i] = 0;
                        continue;
                }
        }
        if (!comnode) {
                inet_aton("127.0.0.1", &addr.sin_addr);
                comnode = addr.sin_addr.s_addr;
        }
        if (!tasknode) {
                inet_aton("127.0.0.1", &addr.sin_addr);
                tasknode = addr.sin_addr.s_addr;
        }
	init_module();
	rt_thread_join(thread);
        while ((srvport = rt_request_port(comnode)) <= 0) {
                msleep(100);
        }
        RT_sem_signal(comnode, srvport, end_sem);
        rt_release_port(comnode, srvport);
        rt_task_delete(task);
	exit(0);
}

static void mfun(int t)
{
	char tname[6] = "MFUN", mname[6] = "RMBX";
	RT_TASK *mytask;
	int smbx, rmbx, msg[MAXSIZ + 1], mtype, i;

	randu();
	tname[4] = mname[4] = t + '0';
	tname[5] = mname[5] = 0;
	mytask = rt_thread_init(nam2num(tname), t + 2, 0, SCHED_FIFO, 0xF);
	smbx = rt_msgget(nam2num("SMSG"), 0);
	rmbx = rt_msgget(nam2num(mname), 0);
	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();

	msg[0] = t;
	rt_sem_wait_barrier(barrier);
	while (end < t) {
		msg[MAXSIZ] = 0;
		for (i = 1; i < MAXSIZ; i++) {
			msg[MAXSIZ] += (msg[i] = MAXSIZ*randu()); 
		}
		if (rt_msgsnd_nu(smbx, 1, msg, sizeof(msg), 0)) {
			rt_printk("SEND FAILED, TASK: %d\n", t);
			goto prem;
		}
		msg[0] = msg[1] = 0;
		if (rt_msgrcv_nu(rmbx, &mtype, msg, sizeof(msg), 1, 0) < 0) {
			rt_printk("RECEIVE FAILED, TASK: %d\n", t);
			goto prem;
		}
		if (msg[0] != t || msg[1] != 0xFFFFFFFF) {
			rt_printk("WRONG REPLY TO TASK: %d.\n", t);
                        goto prem;
		}
		cnt[t]++;
//		rt_printk("TASK: %d, OK (%d).\n", t, cnt[t]);
		rt_sleep(nano2count(SLEEP_TIME));
	}
prem: 
	rt_make_soft_real_time();
	rt_task_delete(mytask);
	printf("TASK %d ENDS.\n", t);
}

void cleanup_module(void)
{
	while (!cleanup) {
		current->state = TASK_INTERRUPTIBLE;
		schedule_timeout(HZ/10);
	}
	stop_rt_timer();
	rt_task_delete(&t1);
	rt_task_delete(&t2);
	rt_task_delete(&t3);
	rt_task_delete(&t4);
	rt_task_delete(&t5);
	rt_task_delete(&t6);
	rt_task_delete(&t7);
	rt_tbx_delete(&bx);
	rt_printk("\n\nEND\n\n");
}

int main(void)
{
	int i;
        RT_TASK *task;

        if (!(task = rt_task_init(nam2num("CMNODE"), 0, 0, 0))) {
                printf("CANNOT INIT COMNODE TASK\n");
                exit(1);
        }

	init_module();
	for (i = 0; i < 7; i++) {
		rt_sem_wait(end_sem);
	}
	cleanup_module();
	rt_task_delete(task);

	exit(0);
}

static void __exit rtnetproxy_cleanup_module(void)
{

    /* Unregister the fallback at rtnet */
    rt_ip_register_fallback(0);

    /* free the rtai srq */
    rt_free_srq(rtnetproxy_srq);

    rt_task_suspend(&rtnetproxy_thread);
    rt_task_delete(&rtnetproxy_thread);

    /* Free the ringbuffers... */
    {
        struct sk_buff *del_skb;  /* standard skb */
        while ((del_skb = read_from_ringbuffer(&ring_skb_rtnet_kernel)) != 0)
        {
            dev_kfree_skb(del_skb);
        }
        while ((del_skb = read_from_ringbuffer(&ring_skb_kernel_rtnet)) != 0)
        {
            dev_kfree_skb(del_skb);
        }
    }
    {
        struct rtskb *del; /* rtnet skb */
        while ((del=read_from_ringbuffer(&ring_rtskb_kernel_rtnet))!=0)
        {
            kfree_rtskb(del); // Although this is kernel mode, freeing should work...
        }
        while ((del=read_from_ringbuffer(&ring_rtskb_rtnet_kernel))!=0)
        {
            kfree_rtskb(del); // Although this is kernel mode, freeing should work...
        }
    }

    /* Unregister the net device: */
    unregister_netdev(&dev_rtnetproxy);
    kfree(dev_rtnetproxy.priv);

    memset(&dev_rtnetproxy, 0, sizeof(dev_rtnetproxy));
    dev_rtnetproxy.init = rtnetproxy_init;
}

static void *Task6(void *arg)
{   
	RT_TASK *t6;
	int status;
	MSG buf;
    
	t6 = rt_task_init_schmod(nam2num("T6"), BASE_PRIO + 5, 0, 0, SCHED_FIFO, 0xF);
	printf("\nTask6 (%p) starting\n", t6);
	mlockall(MCL_CURRENT | MCL_FUTURE);

	while (!endall) {
		memset((char*)&buf, 0, sizeof(buf));    
		status = rt_tbx_receive(bx, (char*)&buf, sizeof(buf)); 
		printf("\nTask6 received msgnr %u from task %u, status=%d\n", buf.progressive, buf.sending_task, status);
	}
	rt_task_delete(t6);
	printf("\nTask6 ends itself\n");
	return 0;
}

void cleanup_module(void)
{
    if (start_timer)
        stop_rt_timer();

    while (close_rt(sock) == -EAGAIN) {
        set_current_state(TASK_INTERRUPTIBLE);
        schedule_timeout(1*HZ); /* wait a second */
    }

    /* rt_task_delete     */
    rt_task_delete(&rt_task);

    /* destroy the fifo   */
    rtf_destroy(PRINT);

    printk("packets sent:\t\t%10lu\npackets received:\t%10lu\npacketloss:\t\t"
           "%10lu%%\n", sent, rcvd, 100-((100*rcvd)/sent));
}

int main(void)
{
	RT_TASK *sending_task ;
	SEM *shmsem, *agentsem;
	int i, *shm, shm_size, count;
	unsigned long chksum;

	sending_task = rt_task_init_schmod(nam2num("STSK"), 0, 0, 0, SCHED_FIFO, 0xF);
	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();
	shmsem   = rt_get_adr(nam2num("SHSM"));
	agentsem = rt_get_adr(nam2num("AGSM"));
	shm = rt_shm_alloc(nam2num("MEM"), 0, 0);
	shm_size = shm[0];
	count = COUNT;
	while(count--) {
		printf("SENDING TASK WAIT ON SHMSEM\n");
		rt_sem_wait(shmsem);
		printf("SENDING TASK SIGNALLED ON SHMSEM\n");
			if (!(shm[0] = ((float)rand()/(float)RAND_MAX)*shm_size) || shm[0] > shm_size) {
				shm[0] = shm_size;
			}
			chksum = 0;
			for (i = 1; i <= shm[0]; i++) {
				shm[i] = rand();
				chksum += shm[i];
			}
			shm[shm[0] + 1] = chksum;
			printf("STSK: %d CHECKSUM = %lx\n", count, chksum);
		printf("SENDING TASK SIGNAL AGENTSEM\n");
		rt_sem_signal(agentsem);
	}
	printf("SENDING TASK LAST WAIT ON SHMSEM\n");
	rt_sem_wait(shmsem);
	printf("SENDING TASK SIGNALLED ON SHMSEM\n");
	shm[0] = 0;
	printf("SENDING TASK LAST SIGNAL TO AGENTSEM\n");
	rt_sem_signal(agentsem);
	printf("SENDING TASK DELETES ITSELF\n");
	rt_task_delete(sending_task);
	printf("END SENDING TASK\n");
	return 0;
}

void task1(void)
{
	rt_thread_init(nam2num("TASK1"), 1, 0, SCHED_FIFO, 0x1);
	rt_grow_and_lock_stack(STACK_SIZE - 10000);
#ifdef MAKE_HARD
	MAKE_HARD();
#endif	
	rt_make_hard_real_time();
	rt_printk("TASK1 TID = %d : ", rt_gettid());

	while (!end) {
		rt_sem_wait(sem1);
		rt_sem_signal(sem2);
	}

        rt_task_delete(NULL);
	rt_printk("TASK1 EXITING.\n");
	return;
}

void cleanup_module(void)
{
        /* DO NOT stop timer. RTmac needs the timer! */
	//stop_rt_timer(); 

        /* rt_task_delete     */ 
        rt_printk("rt_task_delete() = %d\n",rt_task_delete(&rt_task));

        /* close th rt-socket */
        closing = 1;
        rt_printk("begin close\n");
  	rt_printk("  rt_socket_close() = %d\n",rt_socket_close(sock));
  	rt_printk("end close\n");

	rt_sem_delete(&tx_sem);

	/* destroy the fifo   */
	rtf_destroy(PRINT);
}

/* alarm_handler 
** is triggered once 10s is passed to inform player 
** wait time has expired - issue start
*/
void alarm_handler(void *arg) 
{
     int sock = *(int *)&arg;
     int echolen;
    
     /* start the countdown */
     rt_task_set_periodic(NULL, TM_NOW, 8000000000ULL);
     rt_task_wait_period(NULL); 

     if(!issuedStart)
       rt_task_delete(NULL); //not need two players connected

     rt_mutex_acquire(&autoStart, TM_INFINITE);
     issuedStart = true;
     rt_mutex_release(&autoStart);
     AIMode = true;
      
     printf("FORCED START client playing with AI!\n");
     /* below we emulate the add, start */
     addPong();     
     echolen = strlen(pongData[0]);
     if(send(sock, pongData[0],echolen, 0) != echolen)
     	rt_err("send()");
     echolen = strlen(pongData[1]);
     if(send(sock, pongData[1],echolen, 0) != echolen) 
     	rt_err("send()");

     start(); 
     echolen = strlen(gameData);
     if(send(sock, gameData, echolen, 0) != echolen)
     	 rt_err("send()");

     echolen = strlen(ballData);
     if(send(sock, ballData, echolen, 0) != echolen)
	 rt_err("send()");

     if(rt_task_resume(&recvThread[0]))
	rt_err("rt_task_resume()");
     if(rt_task_resume(&ballThread))	
	 rt_err("rt_task_resume()");

}

void cleanup_module(void)
{
    rt_shutdown_irq(parirq);
    rt_free_global_irq(parirq);

    outb(0, PAR_CONTROL);

    /* Important: First close the socket! */
    while (rt_socket_close(sock) == -EAGAIN) {
        printk("rt_server: Socket busy - waiting...\n");
        set_current_state(TASK_INTERRUPTIBLE);
        schedule_timeout(1*HZ); /* wait a second */
    }

    rt_printk("rt_task_delete() = %d\n",rt_task_delete(&rt_task));

    rt_sem_delete(&tx_sem);

    rtf_destroy(PRINT_FIFO);
}

void loop(void *arg){
	Matrix outputMatrix=empty_matrix(1,1);
	RT_TASK pwm_task;

	lpt_init();

	rt_task_spawn(&pwm_task,NULL,0,gs->task_prio,0,&pwm,NULL);

	while (running) {
		read_inputs();
		
		//outputMatrix=periodic_function(io.input_result,io.input_num);

		write_outputs(outputMatrix);
	}

	rt_task_delete(&pwm_task);

	lpt_close();
}