commit 982056f704f05dc8581dbce7eaf597f9a54f9d0c
Author: Svenja <skenneweg@techfak.uni-bielefeld.de>
Date:   Wed Jul 29 17:16:45 2020 +0200

    Several Changes in the subscriber files:
    Splitted up the subscriber file (basesubscriber and other)
    Finished some subscriber functions (not tested yet)

diff --git a/src/urt_subscriber.c b/src/urt_subscriber.c
index ce23262..0d4b5b6 100644
--- a/src/urt_subscriber.c
+++ b/src/urt_subscriber.c
@@ -20,6 +20,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include <urtware.h>
+#include <stdio.h>
 
 /******************************************************************************/
 /* LOCAL DEFINITIONS                                                          */
@@ -41,11 +42,11 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* LOCAL FUNCTIONS                                                            */
 /******************************************************************************/
 
-void urtFetchMessage ()
+void urtFetchMessage (urt_message_t* message, urt_nrtsubscriber_t* subscriber, void* const payload, size_t bytes)
 {
-    //TODO: Update message pointer
-    //TODO: Copy message origin time
-    //TODO: Copy message payload
+  subscriber->base.lastMessage = message;
+  *subscriber->base.lastMessageTime = message->originTime;
+  memcpy(message->payload, payload, bytes);
 }
 
 urt_message_t* urtFindOldestMessage(urt_message_t* oldestMessage)
@@ -70,6 +71,17 @@ urt_message_t* urtFindLatestMessage(urt_message_t* latestMessage)
   }
 }
 
+void urtContributeMessages(urt_message_t* messages)
+{
+  urt_message_t* lastMessageContribute = messages;
+  while (lastMessageContribute->next)
+  {
+    lastMessageContribute = lastMessageContribute->next;
+  }
+  lastMessageContribute->next = topic->latestMessage->next;
+  topic->latestMessage->next = messages;
+}
+
 /******************************************************************************/
 /* EXPORTED FUNCTIONS                                                         */
 /******************************************************************************/
@@ -112,21 +124,15 @@ urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_top
   urtDebugAssert(subscriber);
   urtDebugAssert(topic);
 
-  if (!subscriber->base.topic)
-      return URT_STATUS_SUBSCRIBE_TOPICSET;
+  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;
+  if (messages) {
+    urtContributeMessages(messages);
   }
 
   subscriber->base.lastMessage = topic->latestMessage;
@@ -154,49 +160,47 @@ urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_top
  *          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)
+urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber, void* const payload, size_t bytes, urt_delay_t* latency)
 {   
   urtDebugAssert(subscriber);
 
-  if (!subscriber->base.topic)
-      return URT_STATUS_FETCH_NOTOPIC;
+  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)
-    {
+  urt_message_t* oldestMessage = subscriber->base.lastMessage;
+  if(oldestMessage->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;
+    oldestMessage = oldestMessage->next;
   }
-  else
-  {
-    messageTemp = urtFindOldestMessage(messageTemp->next);
+  else {
+    oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage();
+  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
+
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
 
-  if (calculatedLatency < subscriber->minLatency)
-  {
+  if (calculatedLatency < subscriber->minLatency) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency)
-  {
+  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 (latency) {
+      latency = calculatedLatency;
+    }
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
@@ -220,7 +224,7 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
  *          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) {
+urt_status_t urtNrtSubscriberFetchLatestMessage (urt_nrtsubscriber_t* subscriber, void* const payload, size_t bytes, urt_delay_t* latency) {
   urtDebugAssert(subscriber);
 
   if (!subscriber->base.topic)
@@ -235,25 +239,25 @@ urt_status_t urtNrtSubscriberFetchLatestMessage (urt_nrtsubscriber_t* subscriber
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage();
+  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - lastMessage->originTime;
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
 
-  if (calculatedLatency < subscriber->minLatency)
-  {
+  if (calculatedLatency < subscriber->minLatency) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency)
-  {
+  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 (latency) {
+      latency = calculatedLatency;
+    }
   }
 
   urtMutexUnlock(subscriber->base.topic->lock);
@@ -354,13 +358,7 @@ urt_status_t urtSrtSubscriberSubscribe(urt_srtsubscriber_t* subscriber, urt_topi
   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;
+    urtContributeMessages(messages);
   }
 
   subscriber->base.lastMessage = topic->latestMessage;
@@ -388,7 +386,7 @@ urt_status_t urtSrtSubscriberSubscribe(urt_srtsubscriber_t* subscriber, urt_topi
  *          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,
+urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, void* const payload,
                                               size_t bytes, urt_delay_t* latency)
 {
   urtDebugAssert(subscriber);
@@ -398,40 +396,40 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
 
   urtMutexLock(subscriber->base.topic->lock);
 
-  urt_message_t* messageTemp = subscriber->base.lastMessage;
-  if(messageTemp->originTime == subscriber->base.lastMessageTime)
+  urt_message_t* oldestMessage = subscriber->base.lastMessage;
+  if(oldestMessage->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;
+    oldestMessage = oldestMessage->next;
   }
   else
   {
-    messageTemp = urtFindOldestMessage(messageTemp->next);
+    oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage();
+  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
+
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
 
-  if (calculatedLatency < subscriber->minLatency)
-  {
+  if (calculatedLatency < subscriber->minLatency) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency)
-  {
+  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 (latency) {
+      latency = calculatedLatency;
+    }
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
@@ -455,13 +453,14 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
  *          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,
+urt_status_t urtSrtSubscriberFetchLatestMessage(urt_srtsubscriber_t* subscriber, void* const payload,
                                                 size_t bytes, urt_delay_t* latency)
 {
   urtDebugAssert(subscriber);
 
-  if (!subscriber->base.topic)
-      return URT_STATUS_FETCH_NOTOPIC;
+  if (!subscriber->base.topic) {
+    return URT_STATUS_FETCH_NOTOPIC;
+  }
 
   urtMutexLock(subscriber->base.topic->lock);
   urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);
@@ -472,25 +471,25 @@ urt_status_t urtSrtSubscriberFetchLatestMessage(urt_srtsubscriber_t* subscriber,
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage();
+  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - lastMessage->originTime;
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
 
-  if (calculatedLatency < subscriber->minLatency)
-  {
+  if (calculatedLatency < subscriber->minLatency) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency)
-  {
+  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 (latency) {
+      latency = calculatedLatency;
+    }
   }
 
   urtMutexUnlock(subscriber->base.topic->lock);
@@ -625,13 +624,7 @@ urt_status_t urtFrtSubscriberSubscribe(urt_frtsubscriber_t* subscriber, urt_topi
   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;
+    urtContributeMessages(messages);
   }
 
   subscriber->base.lastMessage = topic->latestMessage;
@@ -659,7 +652,7 @@ urt_status_t urtFrtSubscriberSubscribe(urt_frtsubscriber_t* subscriber, urt_topi
  *          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,
+urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, void* const payload,
                                               size_t bytes, urt_delay_t* latency)
 {
   urtDebugAssert(subscriber);
@@ -669,43 +662,53 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
 
   urtMutexLock(subscriber->base.topic->lock);
 
-  urt_message_t* messageTemp = subscriber->base.lastMessage;
-  if(messageTemp->originTime == subscriber->base.lastMessageTime)
+  urt_message_t* oldestMessage = subscriber->base.lastMessage;
+  if(oldestMessage->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;
+    oldestMessage = oldestMessage->next;
   }
   else
   {
-    messageTemp = urtFindOldestMessage(messageTemp->next);
+    oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage();
+  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
-#if(URT_CFG_PUBSUB_PROFILING == true)
+  if (URT_CFG_PUBSUB_PROFILING == true || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
+
+#if(URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == false)
   subscriber->base.sumLatencies += calculatedLatency;
+
+  if (calculatedLatency < subscriber->minLatency) {
+    subscriber->minLatency = calculatedLatency;
+  }
+  else if (calculatedLatency > subscriber->maxLatency) {
+    subscriber->maxLatency = 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)
-  {
+#if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)
+  if (calculatedLatency < subscriber->minLatency && calculatedLatency > subscriber->minLatency - subscriber->maxJitter) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency+subscriber->maxJitter)
-  {
+  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency + subscriber->maxJitter) {
     subscriber->maxLatency = calculatedLatency;
   }
-#endif /* URT_CFG_PUBSUB_PROFILING && URT_CFG_PUBSUB_QOS_JITTERCHECKS */
+  else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
+    urtMutexUnlock(subscriber->base.topic);
+    return URT_STATUS_JITTERVIOLATION;
+  }
+#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
 
-  bool temp = false;
-  if (temp/*optional latency output argument given*/)
-  {
-    latency = calculatedLatency;
+    if (latency) {
+      latency = calculatedLatency;
+    }
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
@@ -714,7 +717,7 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
   urtMutexUnlock(subscriber->base.topic->lock);
-  return URT_STATUS_OK; //TODO: or urt_status_jitterviolation
+  return URT_STATUS_OK;
 }
 
 /**
@@ -729,7 +732,7 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
  *          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,
+urt_status_t urtFrtSubscriberFetchLatestMessage(urt_frtsubscriber_t* subscriber, void* const payload,
                                                 size_t bytes, urt_delay_t* latency)
 {
   urtDebugAssert(subscriber);
@@ -746,28 +749,38 @@ urt_status_t urtFrtSubscriberFetchLatestMessage(urt_frtsubscriber_t* subscriber,
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage();
+  urtFetchMessage(lastMessage, subscriber, payload, bytes);
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
-#if(URT_CFG_PUBSUB_PROFILING == true)
+  if (URT_CFG_PUBSUB_PROFILING == true || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
+
+#if(URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == false)
   subscriber->base.sumLatencies += calculatedLatency;
+
+  if (calculatedLatency < subscriber->minLatency) {
+    subscriber->minLatency = calculatedLatency;
+  }
+  else if (calculatedLatency > subscriber->maxLatency) {
+    subscriber->maxLatency = 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)
-  {
+#if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)
+  if (calculatedLatency < subscriber->minLatency && calculatedLatency > subscriber->minLatency - subscriber->maxJitter) {
     subscriber->minLatency = calculatedLatency;
   }
-  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency+subscriber->maxJitter)
-  {
+  else if (calculatedLatency > subscriber->maxLatency && calculatedLatency < subscriber->maxLatency + subscriber->maxJitter) {
     subscriber->maxLatency = calculatedLatency;
   }
-#endif /* URT_CFG_PUBSUB_PROFILING && URT_CFG_PUBSUB_QOS_JITTERCHECKS */
+  else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
+    urtMutexUnlock(subscriber->base.topic);
+    return URT_STATUS_JITTERVIOLATION;
+  }
+#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
 
-  bool temp = false;
-  if (temp/*optional latency output argument given*/)
-  {
-    latency = calculatedLatency;
+    if (latency) {
+      latency = calculatedLatency;
+    }
   }
 
   urtMutexUnlock(subscriber->base.topic->lock);
@@ -812,26 +825,27 @@ 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 (!subscriber->base.topic) {
+    return URT_STATUS_UNSUBSCRIBE_NOTOPIC;
+  }
+
+#if (URT_CFG_PUBSUB_PROFILING == true)
+  urtMutexLock(topic->lock);
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+  urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);
+
 # if (URT_CFG_PUBSUB_PROFILING == true)
-    subscriber->base.topic->numSubscribers--;
+  subscriber->base.topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
-//Hier weiter
 
 # if (URT_CFG_PUBSUB_PROFILING == true)
-    urtMutexUnlock(topic->lock);
+  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;
+  subscriber->base.topic = NULL;
+  subscriber->base.lastMessage = NULL;
+  subscriber->base.lastMessageTime = 0;
+  return URT_STATUS_OK;
 }
 
 
@@ -928,13 +942,7 @@ urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topi
   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;
+    urtContributeMessages(messages);
   }
 
   subscriber->base.lastMessage = topic->latestMessage;
@@ -968,13 +976,14 @@ urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topi
  *          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,
+urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, void* const payload,
                                               size_t bytes, urt_delay_t* latency)
 {
   urtDebugAssert(subscriber);
 
-  if (!subscriber->base.topic)
-      return URT_STATUS_FETCH_NOTOPIC;
+  if (!subscriber->base.topic) {
+    return URT_STATUS_FETCH_NOTOPIC;
+  }
 
   urtMutexLock(subscriber->base.topic->lock);
   urt_message_t* messageTemp = subscriber->base.lastMessage;
@@ -985,14 +994,23 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
   }
   messageTemp = messageTemp->next;
 
-  uint64_t calculatedLatency = NULL; //TODO: Calculate message latency
-#if (URT_CFG_PUBSUB_PROFILING == true)
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - messageTemp->originTime;
+
+#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 (latency) {
+      latency = calculatedLatency;
+    }
   }
 
   subscriber->base.lastMessage->numHrtConsumersLeft--;
@@ -1006,38 +1024,40 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
   subscriber->base->numMessagesReceived++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-#if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || URT_CFG_PUBSUB_PROFILING == true)
-  if (temp /*latency is within allowed jitter range*/)
-  {
-    if (calculatedLatency < subscriber->minLatency)
-    {
-      subscriber->minLatency = calculatedLatency;
-    }
-    else if (calculatedLatency > subscriber->maxLatency)
-    {
-      subscriber->maxLatency = calculatedLatency;
-    }
-  }
-  else
-  {
 #if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)
-    urtMutexUnlock(subscriber->base.topic->lock);
+  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;
+  }
+  else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
+    urtMutexUnlock(subscriber->base.topic);
     urtCoreStopNodes(URT_STATUS_JITTERVIOLATION);
     return URT_STATUS_JITTERVIOLATION;
+  }
 #endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
+
+#if (URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == false)
+  if (calculatedLatency < subscriber->minLatency) {
+    subscriber->minLatency = calculatedLatency;
   }
-#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */
+  else if (calculatedLatency > subscriber->maxLatency) {
+    subscriber->maxLatency = calculatedLatency;
+  }
+#endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtFetchMessage();
+  urtFetchMessage(messageTemp, subscriber, payload, bytes);
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
   if (messageTemp->next->originTime < messageTemp->originTime)
   {
-    //TODO: update qos deadliner timer wrt. next message
+    //TODO: first reset?! (when ... set)
+    urtTimerSet(subscriber->qosDeadlineTimer, subscriber->deadlineOffset, urtCoreCallbackDefault, NULL);
   }
   else
   {
-    //TODO: reset qos deadline timer
+    urtTimerReset(subscriber->qosDeadlineTimer);
   }
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
 
@@ -1058,8 +1078,88 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
  *          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 urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber, void* payload,
-                                                size_t bytes, urt_delay_t* latency){return URT_STATUS_OK;}
+urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber, void* const payload,
+                                                size_t bytes, urt_delay_t* latency)
+{
+  if (!subscriber->base.topic) {
+    return URT_STATUS_FETCH_NOTOPIC;
+  }
+
+  urtMutexLock(subscriber->base.topic->lock);
+  urt_message_t* lastMessage = subscriber->base.lastMessage;
+  bool hrtZero = false;
+  while (lastMessage->next->originTime < lastMessage->originTime) {
+    lastMessage = lastMessage->next;
+    lastMessage->numHrtConsumersLeft--;
+    if (lastMessage->numHrtConsumersLeft == 0) {
+        hertZero = true;
+    }
+#if (URT_CFG_PUBSUB_PROFILING == true)
+    lastMessage->numConsumersLeft--;
+    subscriber->base.numMessagesReceived++;
+#endif /* URT_CFG_PUBSUB_PROFILING */
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
+    urtTimerReset(subscriber->qosDeadlineTimer);
+#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
+  }
+
+  if (hrtZero) {
+    urtCondvarSignal(subscriber->base.topic->hrtReleased);
+  }
+
+  if (lastMessage->originTime == subscriber->base.lastMessageTime) {
+    urtMutexUnlock(subscriber->base.topic);
+    return URT_STATUS_FETCH_NOMESSAGE;
+  }
+
+  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
+    uint64_t calculatedLatency = urtTimeNow() - messageTemp->originTime;
+
+#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 */
+
+    if (latency) {
+      latency = calculatedLatency;
+    }
+  }
+
+#if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)
+  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;
+  }
+  else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
+    urtMutexUnlock(subscriber->base.topic);
+    urtCoreStopNodes(URT_STATUS_JITTERVIOLATION);
+    return URT_STATUS_JITTERVIOLATION;
+  }
+#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
+
+#if (URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == false)
+  if (calculatedLatency < subscriber->minLatency) {
+    subscriber->minLatency = calculatedLatency;
+  }
+  else if (calculatedLatency > subscriber->maxLatency) {
+    subscriber->maxLatency = calculatedLatency;
+  }
+#endif /* URT_CFG_PUBSUB_PROFILING */
+
+  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
+  urtTimerReset(subscriber->qosDeadlineTimer);
+#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
+}
 
 /**
  * @brief  Unsubscribes from a subscriber.
