/**
 * @brief   Signals all the Event Listeners registered on the specified Event
 *          Source.
 * @details This function variants ORs the specified event flags to all the
 *          threads registered on the @p event_source_t in addition to the
 *          event flags specified by the threads themselves in the
 *          @p event_listener_t objects.
 *
 * @param[in] esp       pointer to the @p event_source_t structure
 * @param[in] flags     the flags set to be added to the listener flags mask
 *
 * @api
 */
void chEvtBroadcastFlags(event_source_t *esp, eventflags_t flags) {

  chSysLock();
  chEvtBroadcastFlagsI(esp, flags);
  chSchRescheduleS();
  chSysUnlock();
}

bool Middleware::advertise(RemotePublisher * pub) {
	Node * node;
	LocalSubscriber * waiting_subscriber;

	if (_remote_publishers == NULL) {
		_remote_publishers = pub;
	} else {
		pub->next(_remote_publishers);
		_remote_publishers = pub;
	}

	node = _nodes;

	while (node) {
		waiting_subscriber = node->queue();

		while (waiting_subscriber != NULL) {
			if (compareTopics(pub->topic(),
					waiting_subscriber->topic())) {
				node->unqueueSubscriber(waiting_subscriber);
				node->remoteSubscribe(pub, waiting_subscriber);
			}
			waiting_subscriber = (LocalSubscriber *) waiting_subscriber->next();
		}

		node = node->next();
	}

	chSysLock();
	chSchRescheduleS();
	chSysUnlock();

	return true;
}

/**
 * @brief   Posts an high priority message into a mailbox.
 * @details The invoking thread waits until a empty slot in the mailbox becomes
 *          available or the specified time runs out.
 *
 * @param[in] mbp       the pointer to an initialized @p mailbox_t object
 * @param[in] msg       the message to be posted on the mailbox
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval MSG_OK       if a message has been correctly posted.
 * @retval MSG_RESET    if the mailbox has been reset.
 * @retval MSG_TIMEOUT  if the operation has timed out.
 *
 * @sclass
 */
msg_t chMBPostAheadTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
  msg_t rdymsg;

  chDbgCheckClassS();
  chDbgCheck(mbp != NULL);

  do {
    /* If the mailbox is in reset state then returns immediately.*/
    if (mbp->reset) {
      return MSG_RESET;
    }

    /* Is there a free message slot in queue? if so then post.*/
    if (chMBGetFreeCountI(mbp) > (size_t)0) {
      if (--mbp->rdptr < mbp->buffer) {
        mbp->rdptr = mbp->top - 1;
      }
      *mbp->rdptr = msg;
      mbp->cnt++;

      /* If there is a reader waiting then makes it ready.*/
      chThdDequeueNextI(&mbp->qr, MSG_OK);
      chSchRescheduleS();

      return MSG_OK;
    }

    /* No space in the queue, waiting for a slot to become available.*/
    rdymsg = chThdEnqueueTimeoutS(&mbp->qw, timeout);
  } while (rdymsg == MSG_OK);

  return rdymsg;
}

/**
 * @brief   Releases an object into a guarded memory pool.
 * @pre     The guarded memory pool must already be initialized.
 * @pre     The freed object must be of the right size for the specified
 *          guarded memory pool.
 * @pre     The added object must be properly aligned.
 *
 * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
 * @param[in] objp      the pointer to the object to be released
 *
 * @api
 */
void chGuardedPoolFree(guarded_memory_pool_t *gmp, void *objp) {

  chSysLock();
  chGuardedPoolFreeI(gmp, objp);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Resets a @p mailbox_t object.
 * @details All the waiting threads are resumed with status @p MSG_RESET and
 *          the queued messages are lost.
 * @post    The mailbox is in reset state, all operations will fail and
 *          return @p MSG_RESET until the mailbox is enabled again using
 *          @p chMBResumeX().
 *
 * @param[in] mbp       the pointer to an initialized @p mailbox_t object
 *
 * @api
 */
void chMBReset(mailbox_t *mbp) {

  chSysLock();
  chMBResetI(mbp);
  chSchRescheduleS();
  chSysUnlock();
}

void gfxSemSignal(gfxSem *psem) {
	chSysLock();
	if (gfxSemCounterI(psem) < psem->limit)
		chSemSignalI(&psem->sem);
	chSchRescheduleS();
	chSysUnlock();
}

/**
 * @brief   Unlocks all mutexes owned by the invoking thread.
 * @post    The stack of owned mutexes is emptied and all the found
 *          mutexes are unlocked.
 * @note    This function is <b>MUCH MORE</b> efficient than releasing the
 *          mutexes one by one and not just because the call overhead,
 *          this function does not have any overhead related to the priority
 *          inheritance mechanism.
 *
 * @api
 */
void chMtxUnlockAll(void) {
  thread_t *ctp = currp;

  chSysLock();
  if (ctp->p_mtxlist != NULL) {
    do {
      mutex_t *mp = ctp->p_mtxlist;
      ctp->p_mtxlist = mp->m_next;
      if (chMtxQueueNotEmptyS(mp)) {
#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE
        mp->m_cnt = (cnt_t)1;
#endif
        thread_t *tp = queue_fifo_remove(&mp->m_queue);
        mp->m_owner = tp;
        mp->m_next = tp->p_mtxlist;
        tp->p_mtxlist = mp;
        (void) chSchReadyI(tp);
      }
      else {
#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE
        mp->m_cnt = (cnt_t)0;
#endif
        mp->m_owner = NULL;
      }
    } while (ctp->p_mtxlist != NULL);
    ctp->p_prio = ctp->p_realprio;
    chSchRescheduleS();
  }
  chSysUnlock();
}

/**
 * @brief   Signals all threads that are waiting on the condition variable.
 *
 * @param[in] cp        pointer to the @p CondVar structure
 *
 * @api
 */
void chCondBroadcast(CondVar *cp) {

  chSysLock();
  chCondBroadcastI(cp);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Retrieves a message from a mailbox.
 * @details The invoking thread waits until a message is posted in the mailbox
 *          or the specified time runs out.
 *
 * @param[in] mbp       the pointer to an initialized @p mailbox_t object
 * @param[out] msgp     pointer to a message variable for the received message
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval MSG_OK       if a message has been correctly fetched.
 * @retval MSG_RESET    if the mailbox has been reset.
 * @retval MSG_TIMEOUT  if the operation has timed out.
 *
 * @sclass
 */
msg_t chMBFetchTimeoutS(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout) {
  msg_t rdymsg;

  chDbgCheckClassS();
  chDbgCheck((mbp != NULL) && (msgp != NULL));

  do {
    /* If the mailbox is in reset state then returns immediately.*/
    if (mbp->reset) {
      return MSG_RESET;
    }

    /* Is there a message in queue? if so then fetch.*/
    if (chMBGetUsedCountI(mbp) > (size_t)0) {
      *msgp = *mbp->rdptr++;
      if (mbp->rdptr >= mbp->top) {
        mbp->rdptr = mbp->buffer;
      }
      mbp->cnt--;

      /* If there is a writer waiting then makes it ready.*/
      chThdDequeueNextI(&mbp->qw, MSG_OK);
      chSchRescheduleS();

      return MSG_OK;
    }

    /* No message in the queue, waiting for a message to become available.*/
    rdymsg = chThdEnqueueTimeoutS(&mbp->qr, timeout);
  } while (rdymsg == MSG_OK);

  return rdymsg;
}

/**
 * @brief   Signals all the Event Listeners registered on the specified Event
 *          Source.
 * @details This function variants ORs the specified event flags to all the
 *          threads registered on the @p EventSource in addition to the event
 *          flags specified by the threads themselves in the
 *          @p EventListener objects.
 *
 * @param[in] esp       pointer to the @p EventSource structure
 * @param[in] flags     the flags set to be added to the listener flags mask
 *
 * @api
 */
void chEvtBroadcastFlags(EventSource *esp, flagsmask_t flags) {

  chSysLock();
  chEvtBroadcastFlagsI(esp, flags);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Performs a signal operation on a semaphore.
 * @post    This function does not reschedule so a call to a rescheduling
 *          function must be performed before unlocking the kernel. Note that
 *          interrupt handlers always reschedule on exit so an explicit
 *          reschedule must not be performed in ISRs.
 *
 * @param[in] sp    pointer to a @p semaphore_t structure
 *
 * @api
 */
void chSemSignal(semaphore_t *sp) {

  chSysLock();
  chSemSignalI(sp);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 *
 * @param[in] sps       pointer to a @p semaphore_t structure to be signaled
 * @param[in] spw       pointer to a @p semaphore_t structure to wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval MSG_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval MSG_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL));
  chDbgAssert(((sps->s_cnt >= (cnt_t)0) && queue_isempty(&sps->s_queue)) ||
              ((sps->s_cnt < (cnt_t)0) && queue_notempty(&sps->s_queue)),
              "inconsistent semaphore");
  chDbgAssert(((spw->s_cnt >= (cnt_t)0) && queue_isempty(&spw->s_queue)) ||
              ((spw->s_cnt < (cnt_t)0) && queue_notempty(&spw->s_queue)),
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= (cnt_t)0) {
    chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = MSG_OK;
  }
  if (--spw->s_cnt < (cnt_t)0) {
    thread_t *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtsemp = spw;
    chSchGoSleepS(CH_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = MSG_OK;
  }
  chSysUnlock();

  return msg;
}

/**
 * @brief   Adds a set of event flags directly to the specified @p thread_t.
 *
 * @param[in] tp        the thread to be signaled
 * @param[in] mask      the event flags set to be ORed
 *
 * @api
 */
void chEvtSignal(thread_t *tp, eventmask_t mask) {

  chSysLock();
  chEvtSignalI(tp, mask);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Signals all the Event Listeners registered on the specified Event
 *          Source.
 * @details This function variants ORs the specified event flags to all the
 *          threads registered on the @p EventSource in addition to the event
 *          flags specified by the threads themselves in the
 *          @p EventListener objects.
 *
 * @param[in] esp       pointer to the @p EventSource structure
 * @param[in] mask      the event flags set to be ORed
 *
 * @api
 */
void chEvtBroadcastFlags(EventSource *esp, eventmask_t mask) {

  chSysLock();
  chEvtBroadcastFlagsI(esp, mask);
  chSchRescheduleS();
  chSysUnlock();
}

THD_FUNCTION(test_support, arg) {
#if CH_CFG_USE_EVENTS == TRUE
  thread_t *tp = (thread_t *)arg;
#else
  (void)arg;
#endif

  /* Initializing global resources.*/
  chSemObjectInit(&gsem1, 0);
  chSemObjectInit(&gsem2, 0);

  while (true) {
    chSysLock();
    if (chSemGetCounterI(&gsem1) < 0)
      chSemSignalI(&gsem1);
    chSemResetI(&gsem2, 0);
    chThdResumeI(&gtr1, MSG_OK);
#if CH_CFG_USE_EVENTS == TRUE
    chEvtSignalI(tp, 0x55);
#endif
    chSchRescheduleS();
    chSysUnlock();

    chThdSleepMilliseconds(250);
  }
}

/**
 * @brief   Signals all threads that are waiting on the condition variable.
 *
 * @param[in] cp        pointer to the @p condition_variable_t structure
 *
 * @api
 */
void chCondBroadcast(condition_variable_t *cp) {

  chSysLock();
  chCondBroadcastI(cp);
  chSchRescheduleS();
  chSysUnlock();
}

static void test_005_007_execute(void) {

  /* [5.7.1] Starting the five threads with increasing priority, the
     threads will queue on the condition variable.*/
  test_set_step(1);
  {
    tprio_t prio = chThdGetPriorityX();
    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread6, "E");
    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread6, "D");
    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread6, "C");
    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prio+4, thread6, "B");
    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prio+5, thread6, "A");
  }

  /* [5.7.2] Atomically signaling the condition variable five times
     then waiting for the threads to terminate in priority order, the
     order is tested.*/
  test_set_step(2);
  {
    chSysLock();
    chCondSignalI(&c1);
    chCondSignalI(&c1);
    chCondSignalI(&c1);
    chCondSignalI(&c1);
    chCondSignalI(&c1);
    chSchRescheduleS();
    chSysUnlock();
    test_wait_threads();
    test_assert_sequence("ABCDE", "invalid sequence");
  }
}

/**
 * @brief   Performs a reset operation on the semaphore.
 * @post    After invoking this function all the threads waiting on the
 *          semaphore, if any, are released and the semaphore counter is set
 *          to the specified, non negative, value.
 * @post    This function does not reschedule so a call to a rescheduling
 *          function must be performed before unlocking the kernel. Note that
 *          interrupt handlers always reschedule on exit so an explicit
 *          reschedule must not be performed in ISRs.
 *
 * @param[in] sp        pointer to a @p semaphore_t structure
 * @param[in] n         the new value of the semaphore counter. The value must
 *                      be non-negative.
 *
 * @api
 */
void chSemReset(semaphore_t *sp, cnt_t n) {

  chSysLock();
  chSemResetI(sp, n);
  chSchRescheduleS();
  chSysUnlock();
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 * @pre     The configuration option @p CH_USE_SEMSW must be enabled in order
 *          to use this function.
 *
 * @param[in] sps       pointer to a @p Semaphore structure to be signaled
 * @param[in] spw       pointer to a @p Semaphore structure to be wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval RDY_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval RDY_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait");

  chDbgAssert(((sps->s_cnt >= 0) && isempty(&sps->s_queue)) ||
              ((sps->s_cnt < 0) && notempty(&sps->s_queue)),
              "chSemSignalWait(), #1",
              "inconsistent semaphore");

  chDbgAssert(((spw->s_cnt >= 0) && isempty(&spw->s_queue)) ||
              ((spw->s_cnt < 0) && notempty(&spw->s_queue)),
              "chSemSignalWait(), #2",
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= 0)
    chSchReadyI(fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
  if (--spw->s_cnt < 0) {
    Thread *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtobjp = spw;
    chSchGoSleepS(THD_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = RDY_OK;
  }
  chSysUnlock();
  return msg;
}

static THD_FUNCTION(thread1, p) {

  chSysLock();
  chThdResumeI(&tr1, MSG_OK);
  chSchRescheduleS();
  chSysUnlock();
  test_emit_token(*(char *)p);
}