commit 65dc89cb810a1abae669052800f2fc714a0309ae
Author: skenneweg <skenneweg@techfak.uni-bielefeld.de>
Date:   Mon Jul 27 20:54:32 2020 +0200

    Work in the subscriber functions:
    Mainly the fetchLatest/FetchNext

diff --git a/src/urt_subscriber.c b/src/urt_subscriber.c
index 79f3a31..ce23262 100644
--- a/src/urt_subscriber.c
+++ b/src/urt_subscriber.c
@@ -41,6 +41,35 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* 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                                                         */
 /******************************************************************************/
@@ -126,8 +155,56 @@ urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_top
  *          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;
 }
 
@@ -144,6 +221,42 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
  *          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;
 }
 
@@ -276,7 +389,59 @@ urt_status_t urtSrtSubscriberSubscribe(urt_srtsubscriber_t* subscriber, urt_topi
  *          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){return URT_STATUS_OK;}
+                                              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.
@@ -291,7 +456,46 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
  *          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){return URT_STATUS_OK;}
+                                                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.
@@ -456,7 +660,62 @@ urt_status_t urtFrtSubscriberSubscribe(urt_frtsubscriber_t* subscriber, urt_topi
  *          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){return URT_STATUS_OK;}
+                                              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.
@@ -471,7 +730,49 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
  *          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){return URT_STATUS_OK;}
+                                                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.
@@ -668,7 +969,81 @@ urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topi
  *          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){return URT_STATUS_OK;}
+                                              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)
+  subscriber->base.lastMessage->numConsumersLeft--;
+  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);
+    urtCoreStopNodes(URT_STATUS_JITTERVIOLATION);
+    return URT_STATUS_JITTERVIOLATION;
+#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
+  }
+#endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */
+
+  urtFetchMessage();
+
+#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
+  if (messageTemp->next->originTime < messageTemp->originTime)
+  {
+    //TODO: update qos deadliner timer wrt. next message
+  }
+  else
+  {
+    //TODO: reset qos deadline timer
+  }
+#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
+
+  urtMutexUnlock(subscriber->base.topic->lock);
+  return URT_STATUS_OK;
+}
 
 
 /**
