/**
 * @brief Receives a message with absolute timeout.
 *
 * rt_mbx_receive_until receives a message of @e msg_size bytes from
 * the mailbox @e mbx. The caller will be blocked until all bytes of
 * the message arrive, timeout expires or an error occurs.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg points to a buffer provided by the caller.
 *
 * @param msg_size corresponds to the size of the message received.
 *
 * @param time is an absolute value of the timeout.
 *
 * @return On success, 0 is returned.
 * On failure a value is returned as described below:
 * - the number of bytes not received: an error is occured
 *   in the queueing of all receiving tasks or the timeout has expired.
 * - @b EINVAL: mbx points to an invalid mailbox.
 *
 * See also: notes under rt_mbx_received_timed().
 */
RTAI_SYSCALL_MODE int _rt_mbx_receive_until(MBX *mbx, void *msg, int msg_size, RTIME time, int space)
{
	RT_TASK *rt_current = RT_CURRENT;
	int retval;

	CHK_MBX_MAGIC;
	if ((retval = rt_sem_wait_until(&mbx->rcvsem, time)) > 1)
	{
		return MBX_RET(msg_size, retval);
	}
	while (msg_size)
	{
		if ((retval = mbx_wait_until(mbx, &mbx->avbs, time, rt_current)))
		{
			rt_sem_signal(&mbx->rcvsem);
			retval = MBX_RET(msg_size, retval);
			rt_wakeup_pollers(&mbx->poll_recv, retval);
			return retval;
		}
		msg_size = mbxget(mbx, (char **)(&msg), msg_size, space);
		mbx_signal(mbx);
	}
	rt_sem_signal(&mbx->rcvsem);
	rt_wakeup_pollers(&mbx->poll_send, 0);
	return 0;
}

void CommandChrono_Get(char *command)
{
	switch (Chronostatus) {
		case stoppedInitial:
			if (buffered) { 
				buffered = FALSE;
			} else {
				rt_sem_wait(&notEmpty);
			}
			*command  = buffer;
			Chronostatus = running;
			rt_sem_signal(&notFull);
			break;
		case running:
			if (rt_sem_wait_if(&notEmpty) > 0) {
				*command = buffer;
	 			if (*command == 'E') {
					Chronostatus = stoppedFinal;
				}
				rt_sem_signal(&notFull);
			} else {
				*command = 'C';
			}
			break;
		case stoppedFinal:
			if (rt_sem_wait_timed(&notEmpty, fiveSeconds) >= 0) {
				buffered = TRUE;
			}
			*command = 'R';
			Chronostatus = stoppedInitial;
			break;
	}
}

/**
 * @brief Sends a message unconditionally.
 *
 * rt_mbx_send sends a message @e msg of @e msg_size bytes to the
 * mailbox @e mbx. The caller will be blocked until the whole message
 * is copied into the mailbox or an error occurs. Even if the message
 * can be sent in a single shot, the sending task can be blocked if
 * there is a task of higher priority waiting to receive from the
 * mailbox.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg corresponds to the message to be sent.
 *
 * @param msg_size is the size of the message.
 *
 * @return On success, 0 is returned.
 * On failure a value is returned as described below:
 * - the number of bytes not received: an error is occured
 *   in the queueing of all sending tasks.
 * - @b EINVAL: mbx points to an invalid mailbox.
 */
RTAI_SYSCALL_MODE int _rt_mbx_send(MBX *mbx, void *msg, int msg_size, int space)
{
	RT_TASK *rt_current = RT_CURRENT;
	int retval;

	CHK_MBX_MAGIC;
	if ((retval = rt_sem_wait(&mbx->sndsem)) > 1)
	{
		return MBX_RET(msg_size, retval);
	}
	while (msg_size)
	{
		if ((retval = mbx_wait(mbx, &mbx->frbs, rt_current)))
		{
			rt_sem_signal(&mbx->sndsem);
			retval = MBX_RET(msg_size, retval);
			rt_wakeup_pollers(&mbx->poll_recv, retval);
			return retval;
		}
		msg_size = mbxput(mbx, (char **)(&msg), msg_size, space);
		mbx_signal(mbx);
	}
	rt_sem_signal(&mbx->sndsem);
	rt_wakeup_pollers(&mbx->poll_recv, 0);
	return 0;
}

void taskh_func(long tid)
{
	RTIME time;
	unsigned int msg = 0, wait;
	rt_send(&taskm, msg);
	rt_send(&taskl, msg);
	while (1) {
		rt_receive(&taskm, &msg);
		time = rt_get_time_ns();
		if (MUTEX_LOCK(&mutex) <= 1) {
			if ((wait = (int)(rt_get_time_ns() - time)) > 250000) {
				rt_printk("PRIORITY INVERSION, WAITED FOR %d us\n", wait/1000);
			} else {
				rt_printk("NO PRIORITY INVERSION, WAITED FOR %d us\n", wait/1000);
			}
			if (SemType) {
				MUTEX_LOCK(&mutex);
				MUTEX_LOCK(&mutex);
				rt_busy_sleep(100000);
				MUTEX_LOCK(&mutex);
			}
			rt_busy_sleep(100000);
			if (SemType) {
				rt_sem_signal(&mutex);
				rt_busy_sleep(100000);
				rt_sem_signal(&mutex);
				rt_sem_signal(&mutex);
			}
			rt_sem_signal(&mutex);
		} else {
			rt_task_suspend(0);
		}
	}
}

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;
}

static int _send(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
{
	unsigned long flags;
	RT_MSG *msg_ptr;
	void *p;

	if (msg_size > mq->fastsize) {
		if (!(p = rt_malloc(msg_size))) {
			rt_sem_signal(&mq->freslots);
			rt_sem_signal(&mq->senders);
			return -ENOMEM;
		}
	} else {
		p = NULL;
	}
	flags = rt_spin_lock_irqsave(&mq->lock);
	msg_ptr = mq->slots[mq->slot++];
	rt_spin_unlock_irqrestore(flags, &mq->lock);
	msg_ptr->hdr.size = msg_size;
	msg_ptr->hdr.priority = msgpri;
	msg_ptr->hdr.malloc = p;
	msg_ptr->hdr.broadcast = 0;
	if (space) {
		memcpy(p ? p : msg_ptr->msg, msg, msg_size);
	} else {
		rt_copy_from_user(p ? p : msg_ptr->msg, msg, msg_size);
	}
	flags = rt_spin_lock_irqsave(&mq->lock);
	enq_msg(mq, &msg_ptr->hdr);
	rt_spin_unlock_irqrestore(flags, &mq->lock);
	rt_sem_signal(&mq->received);
	rt_sem_signal(&mq->senders);
	return 0;
}

static void ClockChrono_Read(long t)
{
	char ch;
	int run = 0;

	while(1) {
		cpu_used[hard_cpu_id()]++;
		rt_sem_wait(&keybrd_sem);
		rtf_get(Keyboard, &ch, 1);
		ch = toupper(ch);
		switch(ch) {
			case 'T': case 'R': case 'H': case 'M': case 'S':
				CommandClock_Put(ch);
				break;
			case 'C': case 'I': case 'E':
				CommandChrono_Put(ch);
				break;
			case 'N':
				hide = ~hide;
				break;
			case 'P':
				pause = TRUE;
				rt_fractionated_sleep(nano2count(FIVE_SECONDS));
				pause = FALSE;
				break;
			case 'K': case 'D':
				run |= ch;
				if (run == ('K' | 'D')) {
					rt_sem_signal(&sync);
					rt_sem_signal(&sync);
				}
				break;
		}
	}
}

void CommandChrono_Put(char command)
{
	rt_sem_wait(&notFull);
	if ((Chronostatus == running) != (command == 'C')) {
		buffer = command;
		rt_sem_signal(&notEmpty);
	} else {
		rt_sem_signal(&notFull);
	}
}

void CommandClock_Put(char command)
{
	rt_sem_wait(notFull);
	if (((Clockstatus == running) == (command == 'T')) || command == 'F') {
		buffer = command;
		rt_sem_signal(notEmpty);
	} else {
		rt_sem_signal(notFull);
	}
}

static int _receive(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
{
	int size;
	RT_MSG *msg_ptr;
	void *p;

	size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
	if (space)
	{
		memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
		if (msgpri)
		{
			*msgpri = msg_ptr->hdr.priority;
		}
	}
	else
	{
		rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
		if (msgpri)
		{
			rt_put_user(msg_ptr->hdr.priority, msgpri);
		}
	}

	if (msg_ptr->hdr.broadcast)
	{
		if (!--msg_ptr->hdr.broadcast)
		{
			rt_sem_wait_barrier(&mq->broadcast);
			goto relslot;
		}
		else
		{
			rt_sem_signal(&mq->received);
			rt_sem_signal(&mq->receivers);
			rt_sem_wait_barrier(&mq->broadcast);
		}
	}
	else
	{
		unsigned long flags;
relslot:	flags = rt_spin_lock_irqsave(&mq->lock);
		mq->firstmsg = msg_ptr->hdr.next;
		mq->slots[--mq->slot] = msg_ptr;
		rt_spin_unlock_irqrestore(flags, &mq->lock);
		rt_sem_signal(&mq->freslots);
		rt_sem_signal(&mq->receivers);
		if (p)
		{
			rt_free(p);
		}
	}
	return msg_size - size;
}

void Display_PutHour(MenageHmsh_tChain11 chain)
{
	rt_sem_wait(&mutex);
	hour = chain;
	hour.chain[0] = 'h';
	if (HourBufferstatus == empty) {
		HourBufferstatus = full;
		rt_sem_signal(&notEmpty);
	}
	rt_sem_signal(&mutex);
}

void Display_PutTimes(MenageHmsh_tChain11 chain)
{
	rt_sem_wait(&mutex);
	times = chain;
	times.chain[0] = 't';
	if (TimesBufferstatus == empty) {
		TimesBufferstatus = full;
		rt_sem_signal(&notEmpty);
	}
	rt_sem_signal(&mutex);
}

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

	struct sched_param mysched;

	mysched.sched_priority = 99;

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

	mlockall(MCL_CURRENT | MCL_FUTURE);

	sending_task = rt_task_init(nam2num("STSK"), 0, 0, 0);
	shmsem   = rt_get_adr(nam2num("SHSM"));
	agentsem = rt_get_adr(nam2num("AGSM"));
	shm = rtai_malloc(nam2num("MEM"), 1);
	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;
}

/*
 * initialization task
 */
static void start_task_code(int notused)
{
	int i;
	char buf[9];
	buf[8] = 0;
  /* create the sync semaphore */
	rt_sem_init(&sync_sem, 0);
  /* create the priority-test semaphore */
	rt_sem_init(&prio_sem, 0);
  /* pass the semaphore to the first task */
	rt_sem_signal(&sems[0]);
  /* wait for each task to send the sync semaphore */
	for (i = 0; i < NUM_TASKS; ++i) {
		rt_sem_wait(&sync_sem);
	}
	rt_printk(sync_str);
  /* post the priority-test semaphore -- the tasks should then run */
  /* in priority order */
	for (i = 0; i < NUM_TASKS; ++i) {
		rt_sem_signal(&prio_sem);
	}
	rt_printk("\n");
	for (i = 0; i < NUM_TASKS; ++i) {
		rt_sem_wait(&sync_sem);
	}
	rt_printk(sync_str);

  /* now, test message queues */
	TAKE_PRINT; 
	rt_printk("testing message queues\n");
	GIVE_PRINT;
	for (i = 0; i < NUM_TASKS; ++i) {
		if (rt_mbx_send(&mbx_in, strs[i], 8)) {
			rt_printk("rt_mbx_send() failed\n");
		}
	}
	for (i = 0; i < NUM_TASKS; ++i) {
		rt_mbx_receive(&mbx_out, buf, 8);
		TAKE_PRINT; 
		rt_printk("\nreceived from mbx_out: %s", buf); 
		GIVE_PRINT;
	}
	rt_printk("\n");
	for (i = 0; i < NUM_TASKS; ++i) {
		rt_sem_signal(&sync_sem);
	}
	TAKE_PRINT; 
	rt_printk("\ninit task complete\n"); 
	GIVE_PRINT;

  /* nothing more for this task to do */
}

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

    while (loop--) {
        rt_printk("T1\n");
        rt_sem_signal(&semaphore2);
        rt_sem_wait(&semaphore1);
        rt_printk("T1 after T2\n");
        rt_sem_signal(&semaphore2);

        rt_task_wait_period();
    }
}

static void sched_task(long t) {
	int i, k;
	unsigned long msg;

	change = 0;
	t = rdtsc();
	for (i = 0; i < loops; i++) {
		for (k = 0; k < ntasks; k++) {
			rt_task_resume(thread + k);
		}
	}
	t = rdtsc() - t;
	rt_printk("\n\nFOR %d TASKS: ", ntasks);
	rt_printk("TIME %d (ms), SUSP/RES SWITCHES %d, ", (int)llimd(t, 1000, CPU_FREQ), 2*ntasks*loops);
	rt_printk("SWITCH TIME%s %d (ns)\n", use_fpu ? " (INCLUDING FULL FP SUPPORT)":"",
	       (int)llimd(llimd(t, 1000000000, CPU_FREQ), 1, 2*ntasks*loops));

	change = 1;
	for (k = 0; k < ntasks; k++) {
		rt_task_resume(thread + k);
	}
	t = rdtsc();
	for (i = 0; i < loops; i++) {
		for (k = 0; k < ntasks; k++) {
			rt_sem_signal(&sem);
		}
	}
	t = rdtsc() - t;
	rt_printk("\nFOR %d TASKS: ", ntasks);
	rt_printk("TIME %d (ms), SEM SIG/WAIT SWITCHES %d, ", (int)llimd(t, 1000, CPU_FREQ), 2*ntasks*loops);
	rt_printk("SWITCH TIME%s %d (ns)\n", use_fpu ? " (INCLUDING FULL FP SUPPORT)":"",
	       (int)llimd(llimd(t, 1000000000, CPU_FREQ), 1, 2*ntasks*loops));

	change = 2;
	for (k = 0; k < ntasks; k++) {
		rt_sem_signal(&sem);
	}
	t = rdtsc();
	for (i = 0; i < loops; i++) {
		for (k = 0; k < ntasks; k++) {
			rt_rpc(thread + k, 0, &msg);
		}
	}
	t = rdtsc() - t;
	rt_printk("\nFOR %d TASKS: ", ntasks);
	rt_printk("TIME %d (ms), RPC/RCV-RET SWITCHES %d, ", (int)llimd(t, 1000, CPU_FREQ), 2*ntasks*loops);
	rt_printk("SWITCH TIME%s %d (ns)\n\n", use_fpu ? " (INCLUDING FULL FP SUPPORT)":"",
	       (int)llimd(llimd(t, 1000000000, CPU_FREQ), 1, 2*ntasks*loops));
}

RTAI_SYSCALL_MODE int _rt_msg_broadcast_if(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
{
	if (rt_sem_wait_if(&mq->senders) <= 0) {
		return 0;
	}
	if (mq->received.count >= 0) {
		rt_sem_signal(&mq->senders);
		return 0;
	}
	if (rt_sem_wait_if(&mq->freslots) <= 0) {
		rt_sem_signal(&mq->senders);
		return 0;
	}
	return _broadcast(mq, msg, msg_size, msgpri, -(mq->received.count + mq->receivers.count), space);
}

/**
 * @brief Receives bytes as many as possible, without blocking the
 * calling task.
 *
 * rt_mbx_receive_wp receives at most @e msg_size of bytes of message
 * from the mailbox @e mbx then returns immediately.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg points to a buffer provided by the caller.
 *
 * @param msg_size corresponds to the size of the message to be received.
 *
 * @return On success, the number of bytes not received is returned. On
 * failure a negative value is returned as described below:
 * - @b EINVAL: mbx points to not a valid mailbox.
 */
RTAI_SYSCALL_MODE int _rt_mbx_receive_wp(MBX *mbx, void *msg, int msg_size, int space)
{
	unsigned long flags;
	RT_TASK *rt_current = RT_CURRENT;
	int size = msg_size;

	CHK_MBX_MAGIC;
	flags = rt_global_save_flags_and_cli();
	if (mbx->rcvsem.count > 0 && mbx->avbs) {
		mbx->rcvsem.count = 0;
		if (mbx->rcvsem.type > 0) {
			mbx->rcvsem.owndby = rt_current;
			enqueue_resqel(&mbx->rcvsem.resq, rt_current);
		}
		rt_global_restore_flags(flags);
		msg_size = mbxget(mbx, (char **)(&msg), msg_size, space);
		mbx_signal(mbx);
		rt_sem_signal(&mbx->rcvsem);
	} else {
		rt_global_restore_flags(flags);
	}
	if (msg_size < size) {
		rt_wakeup_pollers(&mbx->poll_send, 0);
	}
	return msg_size;
}

static void rt_timer_tick(void)
{
	cpu_used[NR_RT_CPUS]++;
	if (passed++ < 5) {
		t0 = rdtsc();
	} else {
		t = rdtsc();
		if ((jit = t - t0 - imuldiv(rt_times.periodic_tick, CPU_FREQ, FREQ_8254)) < 0) {
			jit = -jit;
		}
		if (jit > maxj) {
			maxj = jit;
		}
		t0 = t;
	}

	rt_times.tick_time = rt_times.intr_time;
	rt_times.intr_time = rt_times.tick_time + rt_times.periodic_tick;
	rt_set_timer_delay(0);
	if (rt_times.tick_time >= rt_times.linux_time) {
		rt_times.linux_time += rt_times.linux_tick;
		rt_pend_linux_irq(TIMER_8254_IRQ);
	} 
	if (Mode) {
		rt_sem_signal(&sem);
	} else {
		rt_task_resume(&thread);
	}
}

/**
 * @brief Sends a message overwriting what already in the buffer
 * if there is no place for the message.
 *
 * rt_mbx_ovrwr_send sends the message @e msg of @e msg_size bytes
 * to the mailbox @e mbx overwriting what already in the mailbox
 * buffer if there is no place for the message. Useful for logging
 * purposes. It returns immediately and the caller is never blocked.
 *
 * @return On success, 0 is returned. On failure a negative value
 * is returned as described below:
 * - @b EINVAL: @e mbx points to an invalid mailbox.
 */
RTAI_SYSCALL_MODE int _rt_mbx_ovrwr_send(MBX *mbx, void *msg, int msg_size, int space)
{
	unsigned long flags;
	RT_TASK *rt_current = RT_CURRENT;

	CHK_MBX_MAGIC;

	flags = rt_global_save_flags_and_cli();
	if (mbx->sndsem.count > 0)
	{
		mbx->sndsem.count = 0;
		if (mbx->sndsem.type > 0)
		{
			mbx->sndsem.owndby = rt_current;
			enqueue_resqel(&mbx->sndsem.resq, rt_current);
		}
		rt_global_restore_flags(flags);
		msg_size = mbxovrwrput(mbx, (char **)(&msg), msg_size, space);
		mbx_signal(mbx);
		rt_sem_signal(&mbx->sndsem);
	}
	else
	{
		rt_global_restore_flags(flags);
	}
	return msg_size;
}