AMiRo-OS

/*

µRtWare is a lightweight publish/subscribe middleware for real-time

applications. It was developed as part of the software habitat for the

Autonomous Mini Robot [1] (AMiRo) but can be used for other purposes as well.

Copyright (C) 2018..2020  Thomas Schöpping et al.

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <urtware.h>

/******************************************************************************/

/* LOCAL DEFINITIONS                                                          */

/******************************************************************************/

/******************************************************************************/

/* EXPORTED VARIABLES                                                         */

/******************************************************************************/

/******************************************************************************/

/* LOCAL TYPES                                                                */

/******************************************************************************/

/******************************************************************************/

/* LOCAL VARIABLES                                                            */

/******************************************************************************/

/******************************************************************************/

/* LOCAL FUNCTIONS                                                            */

/******************************************************************************/

void urtFetchMessage ()

{

    //TODO: Update message pointer

    //TODO: Copy message origin time

    //TODO: Copy message payload

}

urt_message_t* urtFindOldestMessage(urt_message_t* oldestMessage)

{

  while (oldestMessage->next->originTime < oldestMessage->originTime)

  {

    oldestMessage = oldestMessage->next;

  }

  return oldestMessage;

}

urt_message_t* urtFindLatestMessage(urt_message_t* latestMessage)

{

  urt_message_t* lastMessage = subscriber->base.lastMessage;

  while (lastMessage->next->originTime < lastMessage->originTime)

  {

    lastMessage = lastMessage->next;

#if (URT_CFG_PUBSUB_PROFILING == true)

    subscriber->base.lastMessage->numConsumersLeft--;

    subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  }

}

/******************************************************************************/

/* EXPORTED FUNCTIONS                                                         */

/******************************************************************************/

/**

 * @brief   Initialize the nrt Subscriber.

 *

 * @param[in] subscriber  The NRT subscriber to initialize. Must not be NULL.

 */

void urtNrtSubscriberInit (urt_nrtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  subscriber->base.topic = NULL;

  urtEventListenerInit(subscriber->base.evtListener);

  subscriber->base.lastMessage = NULL;

  subscriber->base.lastMessageTime = 0;

#if (URT_CFG_PUBSUB_PROFILING == true)

    subscriber->minLatency = URT_DELAY_INFINITE;

    subscriber->maxLatency = URT_DELAY_IMMEDIATE;

#endif /* URT_CFG_PUBSUB_PROFILING */

  return;

}

/**

 * @brief  Subscribes the subscriber to a topic.

 *

 * @param[in] subscriber  The NRT subscriber which shall subscribe to a topic. Must not be NULL.

 * @param[in] topic  The topic to subscribe to. Must not be NULL.

 * @param[in] messages  NULL terminated list of messages to contribute to the topic.

 *                      Messages must not be associated to another topic.

 *                      Once a message has been contributed, it cannot be removed later.

 *                      May be NULL(no messages to contribute).

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_SUBSCRIBE_TOPICSET if the subscriber is already associated to a topic.

 */

urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_topic_t* topic, urt_message_t* messages)

{

  urtDebugAssert(subscriber);

  urtDebugAssert(topic);

  if (!subscriber->base.topic)

      return URT_STATUS_SUBSCRIBE_TOPICSET;

  subscriber->base.topic = topic;

  urtMutexLock(topic->lock);

  if (messages)

  {

    urt_message_t* lastMessageContribute = messages;

    while (lastMessageContribute->next)

    {

        lastMessageContribute = lastMessageContribute->next;

    }

    lastMessageContribute->next = topic->latestMessage->next;

    topic->latestMessage->next = messages;

  }

  subscriber->base.lastMessage = topic->latestMessage;

  subscriber->base.lastMessageTime = topic->latestMessage->originTime;

  urtEventRegister(topic->evtSource, subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);

#if (URT_CFG_PUBSUB_PROFILING == true)

    topic->numHrtSubscribers--;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the next message.

 *

 * @param[in] subscriber  The NRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Retruns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber, void* payload, size_t bytes, urt_delay_t* latency)

{   

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* messageTemp = subscriber->base.lastMessage;

  if(messageTemp->originTime == subscriber->base.lastMessageTime)

  {

    if (subscriber->base.lastMessage->next->originTime > subscriber->base.lastMessageTime)

    {

      urtMutexUnlock(subscriber->base.topic->lock);

      return URT_STATUS_FETCH_NOMESSAGE;

    }

    messageTemp = messageTemp->next;

  }

  else

  {

    messageTemp = urtFindOldestMessage(messageTemp->next);

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

  if (calculatedLatency < subscriber->minLatency)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.lastMessage->numConsumersLeft--;

  subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief Fetches the latest message.

 *

 * @param[in] subscriber  The NRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtNrtSubscriberFetchLatestMessage (urt_nrtsubscriber_t* subscriber, void* payload, size_t bytes, urt_delay_t* latency) {

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);

  if (lastMessage->originTime == subscriber->base.lastMessageTime)

  {

    urtMutexUnlock(subscriber->base.topic->lock);

    return URT_STATUS_FETCH_NOMESSAGE;

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

  if (calculatedLatency < subscriber->minLatency)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Unsubscribes from a subscriber.

 *

 * @param[in] subscriber  The NRT subscriber to be unsubscribed. Must not be NULL.

 *

 * @return  Returns URT_STATUS_OK on sucess.

 *          Returns URT_STATUS_UNSUBSCRIBE_NOTOPIC if the subscriber is not associated to a topic.

 */

urt_status_t urtNrtSubscriberUnsubscribe (urt_nrtsubscriber_t* subscriber)

{

  if (subscriber->base.topic)

  {

# if(URT_CFG_PUBSUB_PROFILING == true)

      urtMutexLock(topic->lock);

      subscriber->base.topic->numSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);

# if(URT_CFG_PUBSUB_PROFILING == true)

      urtMutexUnlock(topic->lock);

      subscriber->base.topic = NULL;

      subscriber->base.lastMessage = NULL;

      subscriber->base.lastMessageTime = 0;

#endif /* URT_CFG_PUBSUB_PROFILING */

    return URT_STATUS_OK;

  }

  return URT_STATUS_UNSUBSCRIBE_NOTOPIC;

}

/**

 * @brief  Initialize the srt Subscriber.

 *

 * @param[in] subscriber  The SRT subscriber to initialize. Must not be NULL.

 */

void urtSrtSubscriberInit(urt_srtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  subscriber->base.topic = NULL;

  urtEventListenerInit(subscriber->base.evtListener);

  subscriber->base.lastMessage = NULL;

  subscriber->base.lastMessageTime = 0;

  #if (URT_CFG_PUBSUB_PROFILING)

    subscriber->base.sumLatencies = 0;

    subscriber->base.numMessagesReceived = 0;

    subscriber->usefulnesscb = NULL;

    subscriber->cbparams = NULL;

    subscriber->minLatency = URT_DELAY_INFINITE;

    subscriber->maxLatency = URT_DELAY_IMMEDIATE;

  #endif /* URT_CFG_PUBSUB_PROFILING */

  return;

}

/**

 * @brief  Subscribes the subscriber to a topic.

 *

 * @param[in] subscriber  The SRT subscriber which shall subscribe to a topic. Must not be NULL.

 * @param[in] topic  The topic to subscribe to. Must not be NULL.

 * @param[in] message  NULL terminated list of messages to contribute to the topic.

 *                     Messages must not be associated to another topic.

 *                     Once a message has been contributed, it cannot be removed later.

 *                     May be NULL (no messages to contribute)

 * @param[in] usefulnesscb  Pointer to a function to calculate usefulness of a message. Must not be NULL.

 * @param[in] cbparams  Optional parameters for the usefulness callback.

 *                      May be NULL if the callback expects no parameters.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_SUBSCRIBE_TOPICSET if the subscriber is already associated to a topic.

 */

urt_status_t urtSrtSubscriberSubscribe(urt_srtsubscriber_t* subscriber, urt_topic_t* topic,

                                       urt_message_t* messages, urt_srtusefulnessfunc_t* usefulnesscb, void* cbparams)

{

  urtDebugAssert(subscriber);

  urtDebugAssert(topic);

  if (subscriber->base.topic)

  {

    return URT_STATUS_SUBSCRIBE_TOPICSET;

  }

  subscriber->base.topic = topic;

  subscriber->usefulnesscb = usefulnesscb;

  subscriber->cbparams = cbparams;

# if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies = 0;

  subscriber->base.numMessagesReceived = 0;

  subscriber->minLatency = URT_DELAY_INFINITE;

  subscriber->maxLatency = URT_DELAY_IMMEDIATE;

# endif  /* URT_CFG_PUBSUB_PROFILING */

  urtMutexLock(topic->lock);

  if (messages)

  {

    urt_message_t* lastMessageContribute = messages;

    while (lastMessageContribute->next)

    {

        lastMessageContribute = lastMessageContribute->next;

    }

    lastMessageContribute->next = topic->latestMessage->next;

    topic->latestMessage->next = messages;

  }

  subscriber->base.lastMessage = topic->latestMessage;

  subscriber->base.lastMessageTime = topic->latestMessage->originTime;

  urtEventRegister(topic->evtSource, subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);

# if (URT_CFG_PUBSUB_PROFILING == true)

    topic->numHrtSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the next message.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, void* payload,

                                              size_t bytes, urt_delay_t* latency)

{

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* messageTemp = subscriber->base.lastMessage;

  if(messageTemp->originTime == subscriber->base.lastMessageTime)

  {

    if (subscriber->base.lastMessage->next->originTime > subscriber->base.lastMessageTime)

    {

      urtMutexUnlock(subscriber->base.topic->lock);

      return URT_STATUS_FETCH_NOMESSAGE;

    }

    messageTemp = messageTemp->next;

  }

  else

  {

    messageTemp = urtFindOldestMessage(messageTemp->next);

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

  if (calculatedLatency < subscriber->minLatency)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.lastMessage->numConsumersLeft--;

  subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the latest message.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtSrtSubscriberFetchLatestMessage(urt_srtsubscriber_t* subscriber, void* payload,

                                                size_t bytes, urt_delay_t* latency)

{

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);

  if (lastMessage->originTime == subscriber->base.lastMessageTime)

  {

    urtMutexUnlock(subscriber->base.topic->lock);

    return URT_STATUS_FETCH_NOMESSAGE;

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

  if (calculatedLatency < subscriber->minLatency)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Calculates the usefulness of the subscriber.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] latency  Latency (of a message) as argument to calculate usefulness.

 *

 * @return  Returns the usefulness as a value within [0,1].

 */

float urtSrtSubscriberCalculateUsefulness(urt_srtsubscriber_t* subscriber, urt_delay_t latency)

{

  urtDebugAssert(subscriber);

  return subscriber->usefulnesscb(latency);

}

/**

 * @brief  Unsubscribes from a subscriber.

 *

 * @param[in] subscriber  The NRT subscriber to be unsubscribed. Must not be NULL.

 *

 * @return  Returns URT_STATUS_OK on sucess.

 *          Returns URT_STATUS_UNSUBSCRIBE_NOTOPIC if the subscriber is not associated to a topic.

 */

urt_status_t urtSrtSubscriberUnsubscribe(urt_srtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  if (subscriber->base.topic)

  {

# if (URT_CFG_PUBSUB_PROFILING == true)

    urtMutexLock(topic->lock);

    subscriber->base.topic->numSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);

# if (URT_CFG_PUBSUB_PROFILING == true)

      urtMutexUnlock(topic->lock);

# endif /* URT_CFG_PUBSUB_PROFILING */

    subscriber->base.topic = NULL;

    subscriber->base.lastMessage = NULL;

    subscriber->base.lastMessageTime = 0;

    return URT_STATUS_OK;

  }

  return URT_STATUS_UNSUBSCRIBE_NOTOPIC;

}

/**

 * @brief  Initialize the FRT Subscriber.

 *

 * @param[in] subscriber  The SRT subscriber to initialize. Must not be NULL.

 */

void urtFrtSubscriberInit(urt_frtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  subscriber->base.topic = NULL;

  urtEventListenerInit(subscriber->base.evtListener);

  subscriber->base.lastMessage = NULL;

  subscriber->base.lastMessageTime = 0;

  #if (URT_CFG_PUBSUB_PROFILING)

    subscriber->base.sumLatencies = 0;

    subscriber->base.numMessagesReceived = 0;

  #endif /* URT_CFG_PUBSUB_PROFILING */

  #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS)

    subscriber->deadlineOffset = 0;

  #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

  #if (URT_CFG_PUBSUB_QOS_JITTERCHECKS)

    subscriber->maxJitter = 0;

  #endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */

  #if (URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING)

    subscriber->minLatency = URT_DELAY_INFINITE;

    subscriber->maxLatency = URT_DELAY_IMMEDIATE;

  #endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */

  return;

}

/**

 * @brief  Subscribes the subscriber to a topic.

 *

 * @param[in] subscriber  The NRT subscriber which shall subscribe to a topic. Must not be NULL.

 * @param[in] topic  The topic to subscribe to. Must not be NULL.

 * @param[in] messages  NULL terminated list of messages to contribute to the topic.

 *                      Messages must not be associated to another topic.

 *                      Once a message has been contributed, it cannot be removed later.

 *                      May be NULL(no messages to contribute).

 * @param[in] deadline  Maximum latency to consume messages. A value of 0 indicates that latency is of no concern.

 * @param[in] jitter  Maximum allowed jitter (difference between maximum and minimum latency) when consuming messages.

 *                    A value of 0 indicates that jitter is of no concern.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_SUBSCRIBE_TOPICSET if the subscriber is already associated to a topic.

 */

urt_status_t urtFrtSubscriberSubscribe(urt_frtsubscriber_t* subscriber, urt_topic_t* topic,

                                       urt_message_t* messages, urt_delay_t deadline, urt_delay_t jitter)

{

  urtDebugAssert(subscriber);

  urtDebugAssert(topic);

  if (subscriber->base.topic)

  {

    return URT_STATUS_SUBSCRIBE_TOPICSET;

  }

  subscriber->base.topic = topic;

# if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies = 0;

  subscriber->base.numMessagesReceived = 0;

# endif /* URT_CFG_PUBSUB_PROFILING */

# if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)

  subscriber->deadlineOffset = deadline;

# endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)

  subscriber->maxJitter =jitter;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || URT_CFG_PUBSUB_PROFILING == true)

  subscriber->minLatency = URT_DELAY_INFINITE;

  subscriber->maxLatency = URT_DELAY_IMMEDIATE;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */

  urtMutexLock(topic->lock);

  if (messages)

  {

    urt_message_t* lastMessageContribute = messages;

    while (lastMessageContribute->next)

    {

      lastMessageContribute = lastMessageContribute->next;

    }

    lastMessageContribute->next = topic->latestMessage->next;

    topic->latestMessage->next = messages;

  }

  subscriber->base.lastMessage = topic->latestMessage;

  subscriber->base.lastMessageTime = topic->latestMessage->originTime;

  urtEventRegister(topic->evtSource, subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);

# if (URT_CFG_PUBSUB_PROFILING == true)

  topic->numHrtSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the next message.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, void* payload,

                                              size_t bytes, urt_delay_t* latency)

{

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* messageTemp = subscriber->base.lastMessage;

  if(messageTemp->originTime == subscriber->base.lastMessageTime)

  {

    if (subscriber->base.lastMessage->next->originTime > subscriber->base.lastMessageTime)

    {

      urtMutexUnlock(subscriber->base.topic->lock);

      return URT_STATUS_FETCH_NOMESSAGE;

    }

    messageTemp = messageTemp->next;

  }

  else

  {

    messageTemp = urtFindOldestMessage(messageTemp->next);

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

#endif /* URT_CFG_PUBSUB_PROFILING */

#if (URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == true) //TODO: Both must be true? otherwise jitter does not exist

  if (calculatedLatency < subscriber->minLatency && calculatedLatency > subscriber->minLatency-subscriber->maxJitter)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency+subscriber->maxJitter)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING && URT_CFG_PUBSUB_QOS_JITTERCHECKS */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.lastMessage->numConsumersLeft--;

  subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK; //TODO: or urt_status_jitterviolation

}

/**

 * @brief  Fetches the latest message.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtFrtSubscriberFetchLatestMessage(urt_frtsubscriber_t* subscriber, void* payload,

                                                size_t bytes, urt_delay_t* latency)

{

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);

  if (lastMessage->originTime == subscriber->base.lastMessageTime)

  {

    urtMutexUnlock(subscriber->base.topic->lock);

    return URT_STATUS_FETCH_NOMESSAGE;

  }

  urtFetchMessage();

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

#endif /* URT_CFG_PUBSUB_PROFILING */

#if (URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == true) //TODO: Both must be true? otherwise jitter does not exist

  if (calculatedLatency < subscriber->minLatency && calculatedLatency > subscriber->minLatency-subscriber->maxJitter)

  {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency+subscriber->maxJitter)

  {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING && URT_CFG_PUBSUB_QOS_JITTERCHECKS */

  bool temp = false;

  if (temp/*optional latency output argument given*/)

  {

    latency = calculatedLatency;

  }

  urtMutexUnlock(subscriber->base.topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Calculates the validity from the subscriber.

 *

 * @param[in] subscriber  The FRT subscriber to calculate a validity for. Must not be NULL.

 * @param[in] latency  Latency (of a message) as argument to calculate validity.

 *

 * @return  Returns a boolean indicator whether the latency is fine.

 */

bool urtFrtSubscriberCalculateValidity(urt_frtsubscriber_t* subscriber, urt_delay_t latency)

{

#if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || URT_CFG_PUBSUB_PROFILING == true)

  if (latency > subscriber->minLatency && latency < subscriber->maxLatency)

    return true;

#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)

  if (latency < subscriber->minLatency && subscriber->maxLatency-subscriber->maxJitter < latency)

      return true;

  if (latency > subscriber->maxLatency && subscriber->minLatency+subscriber->maxJitter > latency)

      return true;

# endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS && URT_CFG_PUBSUB_QOS_JITTERCHECKS */

  return false;

}

/**

 * @brief  Unsubscribes from a subscriber.

 *

 * @param[in] subscriber  The FRT subscriber to be unsubscribed. Must not be NULL.

 *

 * @return  Returns URT_STATUS_OK on sucess.

 *          Returns URT_STATUS_UNSUBSCRIBE_NOTOPIC if the subscriber is not associated to a topic.

 */

urt_status_t urtFrtSubscriberUnsubscribe(urt_frtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  if (subscriber->base.topic)

  {

    urtMutexLock(topic->lock);

    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);

    //TODO: decrement topic's HRT counter

# if (URT_CFG_PUBSUB_PROFILING == true)

    subscriber->base.topic->numSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

//Hier weiter

# if (URT_CFG_PUBSUB_PROFILING == true)

    urtMutexUnlock(topic->lock);

# endif /* URT_CFG_PUBSUB_PROFILING */

    subscriber->base.topic = NULL;

    subscriber->base.lastMessage = NULL;

    subscriber->base.lastMessageTime = 0;

    return URT_STATUS_OK;

  }

  return URT_STATUS_UNSUBSCRIBE_NOTOPIC;

}

/**

 * @brief  Initialize the HRT Subscriber.

 *

 * @param[in] subscriber  The HRT subscriber to initialize. Must not be NULL.

 */

void urtHrtSubscriberInit(urt_hrtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  subscriber->base.topic = NULL;

  urtEventListenerInit(subscriber->base.evtListener);

  subscriber->base.lastMessage = NULL;

  subscriber->base.lastMessageTime = 0;

# if (URT_CFG_PUBSUB_PROFILING)

    subscriber->base.sumLatencies = 0;

    subscriber->base.numMessagesReceived = 0;

# endif /* URT_CFG_PUBSUB_PROFILING */

  subscriber->next = NULL;

# if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS)

    subscriber->deadlineOffset = 0;

    urtTimerInit(subscriber->qodDeadlineTimer);

# endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

# if (URT_CFG_PUBSUB_QOS_RATECHECKS)

    subscriber->expectedRate = 0;

# endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS)

    subscriber->maxJitter = 0;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING)

    subscriber->minLatency = URT_DELAY_INFINITE;

    subscriber->maxLatency = URT_DELAY_IMMEDIATE;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */

  return;

}

/**

 * @brief  Subscribes the subscriber to a topic.

 *

 * @param[in] subscriber  The HRT subscriber which shall subscribe to a topic. Must not be NULL.

 * @param[in] topic  The topic to subscribe to. Must not be NULL.

 * @param[in] messages  NULL terminated list of messages to contribute to the topic.

 *                      Messages must not be associated to another topic.

 *                      Once a message has been contributed, it cannot be removed later.

 *                      May be NULL(no messages to contribute).

 * @param[in] deadline  Maximum latency to consume messages. A value of 0 indicates that latency is of no concern.

 * @param[in] rate  Expected minimum rate of new messages (= maximum time between consecutive messages).

 *                  A value of 0 indicates, that rate is of no concern.

 * @param[in] jitter  Maximum allowed jitter (difference between maximum and minimum latency) when consuming messages.

 *                    A value of 0 indicates that jitter is of no concern.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_SUBSCRIBE_TOPICSET if the subscriber is already associated to a topic.

 */

urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topic_t* topic,

                                       urt_message_t* messages, urt_delay_t deadline, urt_delay_t rate, urt_delay_t jitter)

{

  urtDebugAssert(subscriber);

  urtDebugAssert(topic);

  if (subscriber->base.topic)

  {

    return URT_STATUS_SUBSCRIBE_TOPICSET;

  }

  subscriber->base.topic = topic;

# if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies = 0;

  subscriber->base.numMessagesReceived = 0;

# endif /* URT_CFG_PUBSUB_PROFILING */

# if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)

  subscriber->deadlineOffset = deadline;

# endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)

  subscriber->maxJitter =jitter;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */

# if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || URT_CFG_PUBSUB_PROFILING == true)

  subscriber->minLatency = URT_DELAY_INFINITE;

  subscriber->maxLatency = URT_DELAY_IMMEDIATE;

# endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */

# if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)

  subscriber->expectedRate = rate;

# endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */

  urtMutexLock(topic->lock);

  if (messages)

  {

    urt_message_t* lastMessageContribute = messages;

    while (lastMessageContribute->next)

    {

      lastMessageContribute = lastMessageContribute->next;

    }

    lastMessageContribute->next = topic->latestMessage->next;

    topic->latestMessage->next = messages;

  }

  subscriber->base.lastMessage = topic->latestMessage;

  subscriber->base.lastMessageTime = topic->latestMessage->originTime;

  urtEventRegister(topic->evtSource, subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);

# if(URT_CFG_PUBSUB_QOS_RATECHECKS == true)

  //TODO: Implement

# endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */

  topic->numHrtSubscribers--;

# if (URT_CFG_PUBSUB_PROFILING == true)

  topic->numSubscribers--;

# endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the next message.

 *

 * @param[in] subscriber  The SRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Returns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, void* payload,

                                              size_t bytes, urt_delay_t* latency)

{

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic)

      return URT_STATUS_FETCH_NOTOPIC;

  urtMutexLock(subscriber->base.topic->lock);

  urt_message_t* messageTemp = subscriber->base.lastMessage;

  if (messageTemp->next->originTime > messageTemp.originTime)

  {

    urtMutexUnlock(subscriber->base.topic->lock);

    return URT_STATUS_FETCH_NOMESSAGE;

  }

  messageTemp = messageTemp->next;

  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency

#if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

#endif /* URT_CFG_PUBSUB_PROFILING */

  bool temp = false;

  if (temp /* optional latency output argument given */)

  {

    latency = calculatedLatency

  }

  subscriber->base.lastMessage->numHrtConsumersLeft--;

  if (subscriber->base.lastMessage->numHrtConsumersLeft != 0)

  {

    urtCondvarSignal(subscriber->base.topic->hrtReleased);

  }

#if (URT_CFG_PUBSUB_PROFILING == true)