commit fb72e91b1fcb99de893710031303ab33dc230d0d
Author: skenneweg <skenneweg@techfak.uni-bielefeld.de>
Date:   Mon Aug 3 19:34:05 2020 +0200

    Nearly finished the publisher.
    Fixed errors, warnings in publisher, subscriber, topic, ...

diff --git a/doc/activitydiagrams/subscriber/hrtsubscriber/plantuml.jar b/doc/activitydiagrams/subscriber/hrtsubscriber/plantuml.jar
deleted file mode 100644
index 2e54b95..0000000
Binary files a/doc/activitydiagrams/subscriber/hrtsubscriber/plantuml.jar and /dev/null differ
diff --git a/inc/urt_core.h b/inc/urt_core.h
index 2cc241a..c3c3352 100644
--- a/inc/urt_core.h
+++ b/inc/urt_core.h
@@ -74,7 +74,7 @@ extern "C" {
 #endif /* URT_CFG_RPC_ENABLED */
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
-  urt_osTimerCallback_t urtCoreCallbackDefault(void* params)
+  urt_osTimerCallback_t urtCoreCallbackDefault(void* params);
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
 
 #if defined(__cplusplus)
diff --git a/inc/urt_node.h b/inc/urt_node.h
index 23d1c62..dcb8afb 100644
--- a/inc/urt_node.h
+++ b/inc/urt_node.h
@@ -41,19 +41,13 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /******************************************************************************/
 
 /*
- * forward declarations
+ * forward declaration
  */
 typedef struct urt_node urt_node_t;
 
 /**
  * @brief
  *
- * @param[in]   node   .
- * @param[in]   cause  .
- * @param[in]   arg    .
- *
- * @return    .
- *
  */
 typedef void (*urt_nodeShutdownCallback_t)(urt_node_t* node, urt_status_t cause, void* arg);
 
@@ -61,23 +55,12 @@ typedef void (*urt_nodeShutdownCallback_t)(urt_node_t* node, urt_status_t cause,
 /**
  * @brief
  *
- * @param[in]   node  .
- * @param[in]   arg   .
- *
- * @return    .
- *
  */
 typedef urt_osEventMask_t (*urt_nodeSetupCallback_t)(urt_node_t* node, void* arg);
 
 
 /**
  * @brief
- *
- * @param[in]   node  .
- * @param[in]   events    .
- * @param[in]   arg    .
- *
- * @return    .
  */
 typedef urt_osEventMask_t (*urt_nodeLoopCallback_t)(urt_node_t* node, urt_osEventMask_t events, void* arg);
 
diff --git a/inc/urt_primitives.h b/inc/urt_primitives.h
index 31fa0a9..1c54874 100644
--- a/inc/urt_primitives.h
+++ b/inc/urt_primitives.h
@@ -23,7 +23,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define URT_PRIMITIVES_H
 
 #include <urtware.h>
-
 #include <stdint.h>
 
 /*============================================================================*/
diff --git a/src/urt_core.c b/src/urt_core.c
index a3acacf..0a061dc 100644
--- a/src/urt_core.c
+++ b/src/urt_core.c
@@ -298,7 +298,7 @@ urt_topic_t* urtCoreGetTopic(urt_topicid_t id)
 #endif /* URT_CFG_PUBSUB_ENABLED */
 
 
-# if (URT_CFG_RPC_ENABLED)
+#if (URT_CFG_RPC_ENABLED == true)
 /**
  * @brief   Prepend service to core's list of services.
  *
@@ -325,18 +325,18 @@ urt_status_t urtCoreAddService(urt_service_t* service)
  * @return Returns a pointer to the requested service. Returns NULL if no service matches the given ID.
  */
 urt_service_t* urtCoreGetService(urt_serviceid_t id) {return urt_service_t;}
-# endif /* URT_CFG_RPC_ENABLED */
+#endif /* URT_CFG_RPC_ENABLED */
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
 urt_osTimerCallback_t urtCoreCallbackDefault(void* params)
 {
-  urtMutexLock(&core._lock);
-  while (params) {
-    urtPrintf("Danger");
-    urtThreadSleep(1);
-  }
-  urtMutexUnlock(&core._lock);
-  return
+urtMutexLock(&core._lock);
+while (params) {
+  urtPrintf("Danger");
+  urtThreadSleep(1);
+}
+urtMutexUnlock(&core._lock);
+return;
 }
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
 
diff --git a/src/urt_message.c b/src/urt_message.c
index 74a5e15..665e74a 100644
--- a/src/urt_message.c
+++ b/src/urt_message.c
@@ -53,12 +53,12 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 void urtMessageInit(urt_message_t* message, void* payload)
 {
-  urt_message_t.next = NULL;
-  urt_message_t.payload = payload;
-  urt_message_t.originTime = 0;
-  urt_message_t.numHrtConsumersLeft = 0;
+  message->next = NULL;
+  message->payload = payload;
+  message->originTime = 0;
+  message->numHrtConsumersLeft = 0;
   #if (URT_CFG_PUBSUB_PROFILING)
-    urt_message_t.numConsumersLeft = 0;
+    message->numConsumersLeft = 0;
   #endif /* URT_CFG_PUBSUB_PROFILING */
 
   return;
diff --git a/src/urt_publisher.c b/src/urt_publisher.c
index 6b44295..f6ca759 100644
--- a/src/urt_publisher.c
+++ b/src/urt_publisher.c
@@ -60,19 +60,20 @@ void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_messag
   urtDebugAssert(publisher);
   urtDebugAssert(topic);
 
-  urt_publisher_t.topic = topic;
+  publisher->topic = topic;
 #if (URT_CFG_PUBSUB_PROFILING == true)
-  urt_publisher_t.publishAttempts = 0;
-  urt_publisher_t.publishFails = 0;
+  publisher->publishAttempts = 0;
+  publisher->publishFails = 0;
 #endif /* URT_CFG_PUBSUB_PROFILING */
   if (messages != NULL) {
-    urtMutexLock(topic->lock);
+    urtMutexLock(&topic->lock);
     urt_message_t* lastMessage = messages;
     while (lastMessage->next != NULL) {
       lastMessage = lastMessage->next;
     }
-    //TODO: setnextpointer of last message to contribute to the message after the topic's latest message
+    lastMessage->next = topic->latestMessage->next;
     topic->latestMessage->next = messages;
+    urtMutexUnlock(&topic->lock);
   }
 
   return;
@@ -92,55 +93,56 @@ void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_messag
  */
 urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout)
 {
-  urtMutexLock(publisher->topic->lock);
-  urt_message_t* message = publisher->topic->next->latestMessage;
-  urt_osTime_t messageTime =  message->originTime;
+  urtMutexLock(&publisher->topic->lock);
+  urt_message_t* latestMessage = publisher->topic->next->latestMessage;
+  urt_osTime_t messageTime =  latestMessage->originTime;
 #if (URT_CFG_PUBSUB_PROFILING == true)
   publisher->publishAttempts++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  while (message->numHrtConsumersLeft > 0) {
-    urt_osCondvarWaitStatus_t timeout = urtCondvarWait(publisher->topic->hrtReleased, publisher->topic->lock, URT_DELAY_INFINITE);
+  while (latestMessage->numHrtConsumersLeft > 0) {
+    //TODO: urt_osCondvarWaitStatus_t timeout = urtCondvarWait(&publisher->topic->hrtReleased, &publisher->topic->lock, URT_DELAY_INFINITE);
     if (timeout == URT_CONDVAR_WAITSTATUS_TIMEOUT) {
 #if (URT_CFG_PUBSUB_PROFILING == true)
       publisher->publishFails++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
-      urtMutexUnlock(publisher->topic->lock);
+      urtMutexUnlock(&publisher->topic->lock);
       return URT_STATUS_PUBLISH_TIMEOUT;
     }
 
-    if (messageTime != message->originTime) {
-      messageTime = message->originTime;
-      message = message->next;
-      while (message->originTime < messageTime) {
-        message = message->next;
+    if (messageTime != latestMessage->originTime) {
+      messageTime = latestMessage->originTime;
+      latestMessage = latestMessage->next;
+      while (latestMessage->originTime < messageTime) {
+        latestMessage = latestMessage->next;
       }
     }
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
-  if (message->numHrtConsumersLeft > 0) { //TODO: never true because of while?
+  if (latestMessage->numConsumersLeft > 0) {
     publisher->topic->numMessagesDiscarded++;
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  publisher->topic->latestMessage = message; //TODO: Iterate topic's pointer to this message?
-  memcpy(message->payload, payload, bytes); //TODO: copy payload to message?
-  //TODO: Set origin time of message
-  publisher->topic->numHrtSubscribers = message->numHrtConsumersLeft;
+  publisher->topic->latestMessage = latestMessage;
+  memcpy(latestMessage->payload, payload, bytes);
+  latestMessage->originTime = t;
+  publisher->topic->numHrtSubscribers = latestMessage->numHrtConsumersLeft;
 #if (URT_CFG_PUBSUB_PROFILING == true)
-  publisher->topic->numSubscribers = message->numHrtConsumersLeft; //TODO: Set number of comsumers?
+  publisher->topic->numSubscribers = latestMessage->numConsumersLeft;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
 #if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)
-  publisher->topic->qosRateTimer =  NULL; //TODO: Set QoS rate timer wrt. most critical HRT Subscriber
+  publisher->topic->qosRateTimer =  publisher->topic->hrtSubscribers;
+  //TODO: Set QoS rate timer (@topic) wrt. most critical HRT Subscriber (here different types?)
 #endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
   urt_hrtsubscriber_t* hrtSubscriber = publisher->topic->hrtSubscribers;
   while (hrtSubscriber != NULL) {
     if (!urtTimerIsArmed(hrtSubscriber->qosDeadlineTimer)) {
-      *hrtSubscriber->qosDeadlineTimer = message->originTime;
+      *hrtSubscriber->qosDeadlineTimer = latestMessage->originTime;
     }
     hrtSubscriber = hrtSubscriber->next;
   }
@@ -150,7 +152,7 @@ urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size
   publisher->topic->numMessagesPublished++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  //TODO: Fire event
-  urtMutexUnlock(publisher->topic->lock);
+  urtEventSourceBroadcast(&publisher->topic->evtSource, URT_EVENTFLAG_PROCEED);
+  urtMutexUnlock(&publisher->topic->lock);
   return URT_STATUS_OK;
 }
diff --git a/src/urt_subscriber.c b/src/urt_subscriber.c
index 0d4b5b6..152bd43 100644
--- a/src/urt_subscriber.c
+++ b/src/urt_subscriber.c
@@ -42,17 +42,9 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* LOCAL FUNCTIONS                                                            */
 /******************************************************************************/
 
-void urtFetchMessage (urt_message_t* message, urt_nrtsubscriber_t* subscriber, void* const payload, size_t bytes)
-{
-  subscriber->base.lastMessage = message;
-  *subscriber->base.lastMessageTime = message->originTime;
-  memcpy(message->payload, payload, bytes);
-}
-
 urt_message_t* urtFindOldestMessage(urt_message_t* oldestMessage)
 {
-  while (oldestMessage->next->originTime < oldestMessage->originTime)
-  {
+  while (oldestMessage->next->originTime < oldestMessage->originTime) {
     oldestMessage = oldestMessage->next;
   }
   return oldestMessage;
@@ -60,22 +52,21 @@ urt_message_t* urtFindOldestMessage(urt_message_t* oldestMessage)
 
 urt_message_t* urtFindLatestMessage(urt_message_t* latestMessage)
 {
-  urt_message_t* lastMessage = subscriber->base.lastMessage;
-  while (lastMessage->next->originTime < lastMessage->originTime)
-  {
+  urt_message_t* lastMessage = latestMessage;
+  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 */
   }
+  return latestMessage;
 }
 
-void urtContributeMessages(urt_message_t* messages)
+void urtContributeMessages(urt_message_t* messages, urt_topic_t* topic)
 {
   urt_message_t* lastMessageContribute = messages;
-  while (lastMessageContribute->next)
-  {
+  while (lastMessageContribute->next) {
     lastMessageContribute = lastMessageContribute->next;
   }
   lastMessageContribute->next = topic->latestMessage->next;
@@ -129,22 +120,22 @@ urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_top
   }
 
   subscriber->base.topic = topic;
-  urtMutexLock(topic->lock);
+  urtMutexLock(&topic->lock);
 
   if (messages) {
-    urtContributeMessages(messages);
+    urtContributeMessages(messages, topic);
   }
 
   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);
+  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);
+  urtMutexUnlock(&topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -168,12 +159,12 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
     return URT_STATUS_FETCH_NOTOPIC;
   }
 
-  urtMutexLock(subscriber->base.topic->lock);
+  urtMutexLock(&subscriber->base.topic->lock);
 
   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);
+      urtMutexUnlock(&subscriber->base.topic->lock);
       return URT_STATUS_FETCH_NOMESSAGE;
     }
     oldestMessage = oldestMessage->next;
@@ -182,7 +173,9 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
     oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
+  subscriber->base.lastMessage = oldestMessage;
+  subscriber->base.lastMessageTime = oldestMessage->originTime;
+  memcpy(oldestMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || latency) {
     uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
@@ -199,7 +192,7 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
     if (latency) {
-      latency = calculatedLatency;
+      *latency = calculatedLatency;
     }
   }
 
@@ -208,7 +201,7 @@ urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber,
   subscriber->base->numMessagesReceived++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -230,16 +223,18 @@ urt_status_t urtNrtSubscriberFetchLatestMessage (urt_nrtsubscriber_t* subscriber
   if (!subscriber->base.topic)
       return URT_STATUS_FETCH_NOTOPIC;
 
-  urtMutexLock(subscriber->base.topic->lock);
+  urtMutexLock(&subscriber->base.topic->lock);
   urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);
 
   if (lastMessage->originTime == subscriber->base.lastMessageTime)
   {
-    urtMutexUnlock(subscriber->base.topic->lock);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+  subscriber->base.lastMessage = lastMessage;
+  subscriber->base.lastMessageTime = lastMessage->originTime;
+  memcpy(lastMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || latency) {
     uint64_t calculatedLatency = urtTimeNow() - lastMessage->originTime;
@@ -256,11 +251,11 @@ urt_status_t urtNrtSubscriberFetchLatestMessage (urt_nrtsubscriber_t* subscriber
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
     if (latency) {
-      latency = calculatedLatency;
+      *latency = calculatedLatency;
     }
   }
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -277,12 +272,12 @@ urt_status_t urtNrtSubscriberUnsubscribe (urt_nrtsubscriber_t* subscriber)
   if (subscriber->base.topic)
   {
 # if(URT_CFG_PUBSUB_PROFILING == true)
-      urtMutexLock(topic->lock);
+      urtMutexLock(&topic->lock);
       subscriber->base.topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
-    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);
+    urtEventUnregister(&subscriber->base.topic->evtSource, &subscriber->base.evtListener);
 # if(URT_CFG_PUBSUB_PROFILING == true)
-      urtMutexUnlock(topic->lock);
+      urtMutexUnlock(&topic->lock);
       subscriber->base.topic = NULL;
       subscriber->base.lastMessage = NULL;
       subscriber->base.lastMessageTime = 0;
@@ -307,14 +302,14 @@ void urtSrtSubscriberInit(urt_srtsubscriber_t* subscriber)
   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 */
+#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;
 }
 
@@ -335,42 +330,40 @@ void urtSrtSubscriberInit(urt_srtsubscriber_t* subscriber)
  *          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)
+                                       urt_message_t* messages, urt_usefulness_f* usefulnesscb,void* cbparams)
 {
   urtDebugAssert(subscriber);
   urtDebugAssert(topic);
 
-  if (subscriber->base.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)
+#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 */
+#endif  /* URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexLock(topic->lock);
-  if (messages)
-  {
-    urtContributeMessages(messages);
+  urtMutexLock(&topic->lock);
+  if (messages) {
+    urtContributeMessages(messages, topic);
   }
 
   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);
+  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);
+  urtMutexUnlock(&topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -391,27 +384,27 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
 {
   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);
+  urtMutexLock(&subscriber->base.topic->lock);
 
   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);
+  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;
     }
     oldestMessage = oldestMessage->next;
   }
-  else
-  {
+  else{
     oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
+  subscriber->base.lastMessage = oldestMessage;
+  subscriber->base.lastMessageTime = oldestMessage->originTime;
+  memcpy(oldestMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || latency) {
     uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
@@ -428,7 +421,7 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
     if (latency) {
-      latency = calculatedLatency;
+      *latency = calculatedLatency;
     }
   }
 
@@ -437,7 +430,7 @@ urt_status_t urtSrtSubscriberFetchNextMessage(urt_srtsubscriber_t* subscriber, v
   subscriber->base->numMessagesReceived++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -462,16 +455,18 @@ urt_status_t urtSrtSubscriberFetchLatestMessage(urt_srtsubscriber_t* subscriber,
     return URT_STATUS_FETCH_NOTOPIC;
   }
 
-  urtMutexLock(subscriber->base.topic->lock);
+  urtMutexLock(&subscriber->base.topic->lock);
   urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);
 
   if (lastMessage->originTime == subscriber->base.lastMessageTime)
   {
-    urtMutexUnlock(subscriber->base.topic->lock);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+  subscriber->base.lastMessage = lastMessage;
+  subscriber->base.lastMessageTime = lastMessage->originTime;
+  memcpy(lastMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || latency) {
     uint64_t calculatedLatency = urtTimeNow() - lastMessage->originTime;
@@ -487,12 +482,12 @@ urt_status_t urtSrtSubscriberFetchLatestMessage(urt_srtsubscriber_t* subscriber,
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-    if (latency) {
-      latency = calculatedLatency;
+    if (latency != NULL) {
+      *latency = calculatedLatency;
     }
   }
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -508,7 +503,7 @@ float urtSrtSubscriberCalculateUsefulness(urt_srtsubscriber_t* subscriber, urt_d
 {
   urtDebugAssert(subscriber);
 
-  return subscriber->usefulnesscb(latency);
+  return (*subscriber->usefulnesscb)(latency, subscriber->cbparams);
 }
 
 /**
@@ -526,12 +521,12 @@ urt_status_t urtSrtSubscriberUnsubscribe(urt_srtsubscriber_t* subscriber)
   if (subscriber->base.topic)
   {
 # if (URT_CFG_PUBSUB_PROFILING == true)
-    urtMutexLock(topic->lock);
+    urtMutexLock(&topic->lock);
     subscriber->base.topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
-    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);
+    urtEventUnregister(&subscriber->base.topic->evtSource, &subscriber->base.evtListener);
 # 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;
@@ -621,22 +616,22 @@ urt_status_t urtFrtSubscriberSubscribe(urt_frtsubscriber_t* subscriber, urt_topi
   subscriber->maxLatency = URT_DELAY_IMMEDIATE;
 # endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS || URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexLock(topic->lock);
+  urtMutexLock(&topic->lock);
   if (messages)
   {
-    urtContributeMessages(messages);
+    urtContributeMessages(messages, topic);
   }
 
   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);
+  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);
+  urtMutexUnlock(&topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -660,14 +655,14 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
   if (!subscriber->base.topic)
       return URT_STATUS_FETCH_NOTOPIC;
 
-  urtMutexLock(subscriber->base.topic->lock);
+  urtMutexLock(&subscriber->base.topic->lock);
 
   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);
+      urtMutexUnlock(&subscriber->base.topic->lock);
       return URT_STATUS_FETCH_NOMESSAGE;
     }
     oldestMessage = oldestMessage->next;
@@ -677,7 +672,9 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
     oldestMessage = urtFindOldestMessage(oldestMessage->next);
   }
 
-  urtFetchMessage(oldestMessage, subscriber, payload, bytes);
+  subscriber->base.lastMessage = oldestMessage;
+  subscriber->base.lastMessageTime = oldestMessage->originTime;
+  memcpy(oldestMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || latency) {
     uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
@@ -701,13 +698,13 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
     subscriber->maxLatency = calculatedLatency;
   }
   else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
-    urtMutexUnlock(subscriber->base.topic);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_JITTERVIOLATION;
   }
 #endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
 
     if (latency) {
-      latency = calculatedLatency;
+      *latency = calculatedLatency;
     }
   }
 
@@ -716,7 +713,7 @@ urt_status_t urtFrtSubscriberFetchNextMessage(urt_frtsubscriber_t* subscriber, v
   subscriber->base->numMessagesReceived++;
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -740,19 +737,21 @@ urt_status_t urtFrtSubscriberFetchLatestMessage(urt_frtsubscriber_t* subscriber,
   if (!subscriber->base.topic)
       return URT_STATUS_FETCH_NOTOPIC;
 
-  urtMutexLock(subscriber->base.topic->lock);
+  urtMutexLock(&subscriber->base.topic->lock);
   urt_message_t* lastMessage = urtFindLatestMessage(subscriber->base.lastMessage);
 
   if (lastMessage->originTime == subscriber->base.lastMessageTime)
   {
-    urtMutexUnlock(subscriber->base.topic->lock);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+    subscriber->base.lastMessage = lastMessage;
+    subscriber->base.lastMessageTime = lastMessage->originTime;
+    memcpy(lastMessage->payload, payload, bytes);
 
   if (URT_CFG_PUBSUB_PROFILING == true || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true || latency) {
-    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;
+    uint64_t calculatedLatency = urtTimeNow() - lastMessage->originTime;
 
 #if(URT_CFG_PUBSUB_PROFILING == true && URT_CFG_PUBSUB_QOS_JITTERCHECKS == false)
   subscriber->base.sumLatencies += calculatedLatency;
@@ -773,17 +772,17 @@ urt_status_t urtFrtSubscriberFetchLatestMessage(urt_frtsubscriber_t* subscriber,
     subscriber->maxLatency = calculatedLatency;
   }
   else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
-    urtMutexUnlock(subscriber->base.topic);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_JITTERVIOLATION;
   }
 #endif /* URT_CFG_PUBSUB_QOS_JITTERCHECKS */
 
     if (latency) {
-      latency = calculatedLatency;
+      *latency = calculatedLatency;
     }
   }
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -830,17 +829,17 @@ urt_status_t urtFrtSubscriberUnsubscribe(urt_frtsubscriber_t* subscriber)
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
-  urtMutexLock(topic->lock);
+  urtMutexLock(&topic->lock);
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);
+  urtEventUnregister(&subscriber->base.topic->evtSource, &subscriber->base.evtListener);
 
 # if (URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
 
 # 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;
@@ -939,16 +938,16 @@ urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topi
   subscriber->expectedRate = rate;
 # endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
 
-  urtMutexLock(topic->lock);
+  urtMutexLock(&topic->lock);
   if (messages)
   {
-    urtContributeMessages(messages);
+    urtContributeMessages(messages, topic);
   }
 
   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);
+  urtEventRegister(&topic->evtSource, &subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);
 
 # if(URT_CFG_PUBSUB_QOS_RATECHECKS == true)
   //TODO: Implement
@@ -959,7 +958,7 @@ urt_status_t urtHrtSubscriberSubscribe(urt_hrtsubscriber_t* subscriber, urt_topi
   topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtMutexUnlock(topic->lock);
+  urtMutexUnlock(&topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -985,17 +984,19 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
     return URT_STATUS_FETCH_NOTOPIC;
   }
 
-  urtMutexLock(subscriber->base.topic->lock);
-  urt_message_t* messageTemp = subscriber->base.lastMessage;
-  if (messageTemp->next->originTime > messageTemp.originTime)
+  urtMutexLock(&subscriber->base.topic->lock);
+  urt_message_t* lastMessage = subscriber->base.lastMessage;
+  if (lastMessage->next->originTime > lastMessage->originTime)
   {
-    urtMutexUnlock(subscriber->base.topic->lock);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_FETCH_NOMESSAGE;
   }
-  messageTemp = messageTemp->next;
+  lastMessage = lastMessage->next;
 
-  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
-    uint64_t calculatedLatency = urtTimeNow() - messageTemp->originTime;
+  uint64_t calculatedLatency;
+  if (URT_CFG_PUBSUB_PROFILING == true || latency || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true) {
+    calculatedLatency = urtTimeNow() - lastMessage->originTime;
+  }
 
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
@@ -1008,15 +1009,14 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-    if (latency) {
-      latency = calculatedLatency;
-    }
+  if (latency) {
+    *latency = calculatedLatency;
   }
 
   subscriber->base.lastMessage->numHrtConsumersLeft--;
   if (subscriber->base.lastMessage->numHrtConsumersLeft != 0)
   {
-    urtCondvarSignal(subscriber->base.topic->hrtReleased);
+    urtCondvarSignal(&subscriber->base.topic->hrtReleased);
   }
 
 #if (URT_CFG_PUBSUB_PROFILING == true)
@@ -1032,7 +1032,7 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
     subscriber->maxLatency = calculatedLatency;
   }
   else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
-    urtMutexUnlock(subscriber->base.topic);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     urtCoreStopNodes(URT_STATUS_JITTERVIOLATION);
     return URT_STATUS_JITTERVIOLATION;
   }
@@ -1047,10 +1047,12 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtFetchMessage(messageTemp, subscriber, payload, bytes);
+    subscriber->base.lastMessage = lastMessage;
+    subscriber->base.lastMessageTime = lastMessage->originTime;
+    memcpy(lastMessage->payload, payload, bytes);
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
-  if (messageTemp->next->originTime < messageTemp->originTime)
+  if (lastMessage->next->originTime < lastMessage->originTime)
   {
     //TODO: first reset?! (when ... set)
     urtTimerSet(subscriber->qosDeadlineTimer, subscriber->deadlineOffset, urtCoreCallbackDefault, NULL);
@@ -1061,7 +1063,7 @@ urt_status_t urtHrtSubscriberFetchNextMessage(urt_hrtsubscriber_t* subscriber, v
   }
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
 
-  urtMutexUnlock(subscriber->base.topic->lock);
+  urtMutexUnlock(&subscriber->base.topic->lock);
   return URT_STATUS_OK;
 }
 
@@ -1085,14 +1087,14 @@ urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber,
     return URT_STATUS_FETCH_NOTOPIC;
   }
 
-  urtMutexLock(subscriber->base.topic->lock);
+  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;
+        hrtZero = true;
     }
 #if (URT_CFG_PUBSUB_PROFILING == true)
     lastMessage->numConsumersLeft--;
@@ -1104,16 +1106,18 @@ urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber,
   }
 
   if (hrtZero) {
-    urtCondvarSignal(subscriber->base.topic->hrtReleased);
+    urtCondvarSignal(&subscriber->base.topic->hrtReleased);
   }
 
   if (lastMessage->originTime == subscriber->base.lastMessageTime) {
-    urtMutexUnlock(subscriber->base.topic);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     return URT_STATUS_FETCH_NOMESSAGE;
   }
 
-  if (URT_CFG_PUBSUB_PROFILING == true || latency) {
-    uint64_t calculatedLatency = urtTimeNow() - messageTemp->originTime;
+  uint64_t calculatedLatency;
+  if (URT_CFG_PUBSUB_PROFILING == true || latency || URT_CFG_PUBSUB_QOS_JITTERCHECKS == true) {
+     calculatedLatency = urtTimeNow() - lastMessage->originTime;
+  }
 
 #if(URT_CFG_PUBSUB_PROFILING == true)
   subscriber->base.sumLatencies += calculatedLatency;
@@ -1126,9 +1130,8 @@ urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber,
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-    if (latency) {
-      latency = calculatedLatency;
-    }
+  if (latency) {
+    *latency = calculatedLatency;
   }
 
 #if (URT_CFG_PUBSUB_QOS_JITTERCHECKS == true)
@@ -1139,7 +1142,7 @@ urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber,
     subscriber->maxLatency = calculatedLatency;
   }
   else if (calculatedLatency > subscriber->maxLatency || calculatedLatency < subscriber->minLatency) {
-    urtMutexUnlock(subscriber->base.topic);
+    urtMutexUnlock(&subscriber->base.topic->lock);
     urtCoreStopNodes(URT_STATUS_JITTERVIOLATION);
     return URT_STATUS_JITTERVIOLATION;
   }
@@ -1154,11 +1157,16 @@ urt_status_t urtHrtSubscriberFetchLatestMessage(urt_hrtsubscriber_t* subscriber,
   }
 #endif /* URT_CFG_PUBSUB_PROFILING */
 
-  urtFetchMessage(lastMessage, subscriber, payload, bytes);
+    subscriber->base.lastMessage = lastMessage;
+    subscriber->base.lastMessageTime = lastMessage->originTime;
+    memcpy(lastMessage->payload, payload, bytes);
 
 #if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
   urtTimerReset(subscriber->qosDeadlineTimer);
 #endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
+
+  urtMutexUnlock(&subscriber->base.topic->lock);
+  return URT_STATUS_OK;
 }
 
 /**
@@ -1173,46 +1181,41 @@ urt_status_t urtHrtSubscriberUnsubscribe(urt_hrtsubscriber_t* subscriber)
 {
   urtDebugAssert(subscriber);
 
-  if (subscriber->base.topic)
-  {
-    urtMutexLock(topic->lock);
-    urtEventUnregister(subscriber->base.topic->evtSource, subscriber->base.evtListener);
-    subscriber->base.topic->numHrtSubscribers--;
+  if(!subscriber->base.topic) {
+    return URT_STATUS_UNSUBSCRIBE_NOTOPIC;
+  }
+
+  urtMutexLock(&subscriber->base.topic->lock);
+  urtEventUnregister(&subscriber->base.topic->evtSource, &subscriber->base.evtListener);
+  subscriber->base.topic->numHrtSubscribers--;
 # if (URT_CFG_PUBSUB_PROFILING == true)
-    subscriber->base.topic->numSubscribers--;
+  subscriber->base.topic->numSubscribers--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
 # if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)
-    //TODO: remove self from topics lsit of HRT subscribers
-    //TODO: ...
+  //TODO: remove self from topics list of HRT subscribers, ...
 # endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
 
-    urt_message_t* messageTemp = subscriber->base.lastMessage;
-    bool hrtZero = false;
-    while (messageTemp->next->originTime < messageTemp->originTime)
-    {
-        messageTemp = messageTemp->next;
-        messageTemp->numHrtConsumersLeft--;
-        if (messageTemp->numHrtConsumersLeft == 0)
-        {
-            hrtZero = true;
-        }
+  urt_message_t* messageTemp = subscriber->base.lastMessage;
+  bool hrtZero = false;
+  while (messageTemp->next->originTime < messageTemp->originTime){
+    messageTemp = messageTemp->next;
+    messageTemp->numHrtConsumersLeft--;
+    if (messageTemp->numHrtConsumersLeft == 0) {
+      hrtZero = true;
+    }
 # if(URT_CFG_PUBSUB_PROFILING == true)
-        messageTemp->numConsumersLeft--;
+    messageTemp->numConsumersLeft--;
 # endif /* URT_CFG_PUBSUB_PROFILING */
-    }
-    if (hrtZero)
-    {
-      urtCondvarSignal(subscriber->base.topic->hrtReleased);
-    }
-
-    urtMutexUnlock(topic->lock);
-    subscriber->base.topic = NULL;
-    subscriber->base.lastMessage = NULL;
-    subscriber->base.lastMessageTime = 0;
-    return URT_STATUS_OK;
+  }
+  if (hrtZero){
+    urtCondvarSignal(&subscriber->base.topic->hrtReleased);
   }
 
-  return URT_STATUS_UNSUBSCRIBE_NOTOPIC;
+  urtMutexUnlock(&subscriber->base.topic->lock);
+  subscriber->base.topic = NULL;
+  subscriber->base.lastMessage = NULL;
+  subscriber->base.lastMessageTime = 0;
+  return URT_STATUS_OK;
 }
 
 
diff --git a/src/urt_topic.c b/src/urt_topic.c
index e262adf..dce4228 100644
--- a/src/urt_topic.c
+++ b/src/urt_topic.c
@@ -60,44 +60,44 @@ urt_status_t urtTopicInit(urt_topic_t* topic, urt_topicid_t id, urt_message_t* m
   urtDebugAssert(topic);
   urtDebugAssert(mandatoryMessage);
 
-  urt_topic_t.next = NULL;
-  urt_topic_t.id = id;
-  urtMutexInit(topic->lock);
-  urtEventSourceInit(topic->evtSource);
-  urt_topic_t.numHrtSubscribers = 0;
-  urtCondvarInit(topic->hrtReleased);
+  topic->next = NULL;
+  topic->id = id;
+  urtMutexInit(&topic->lock);
+  urtEventSourceInit(&topic->evtSource);
+  topic->numHrtSubscribers = 0;
+  urtCondvarInit(&topic->hrtReleased);
   topic->mandatoryMessage = *mandatoryMessage;
   topic->latestMessage = &topic->mandatoryMessage;
   #if (URT_CFG_PUBSUB_QOS_RATECHECKS)
-    urt_topic_t.hrtSubscribers = nullptr;
+    topic->hrtSubscribers = nullptr;
     //add later: timer init;
   #endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
   #if (URT_CFG_PUBSUB_PROFILING)
-    urt_topic_t.numMessagesPublished = 0;
-    urt_topic_t.numMessagesDiscarded = 0;
-    urt_topic_t.numSubscribers = 0;
+    topic->numMessagesPublished = 0;
+    topic->numMessagesDiscarded = 0;
+    topic->numSubscribers = 0;
   #endif /* URT_CFG_PUBSUB_PROFILING */
   topic->latestMessage->next = topic->latestMessage;
-  urtMutexLock(topic->lock);
+  urtMutexLock(&topic->lock);
   urt_topic_t* topicTemp = topic;
   while (topicTemp != NULL && topicTemp->id < id)
       topicTemp = topicTemp->next;
 
   if (topicTemp == NULL)
   {
-    //TODO: Append self to core's list of topic
-    urtMutexUnlock(topic->lock);
+    urtCoreAddTopic(topic);
+    urtMutexUnlock(&topic->lock);
     return URT_STATUS_OK;
   }
   else if (topicTemp->id > id)
   {
     topicTemp->next = topic;
-    urtMutexUnlock(topic->lock);
+    urtMutexUnlock(&topic->lock);
     return URT_STATUS_OK;
   }
   else
   {
-    urtMutexUnlock(topic->lock);
+    urtMutexUnlock(&topic->lock);
     return URT_STATUS_TOPIC_DUPLICATE;
   }
 }
diff --git a/urtware.h b/urtware.h
index 99466e2..ef5e93a 100644
--- a/urtware.h
+++ b/urtware.h
@@ -95,6 +95,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /*
  * TODO: Add further µRtWare includes here (e.g. urt_core.h).
  */
+//Necessary for memcpy
+#include <stdio.h>
+#include <string.h>
+
 #include <apps_urtosal.h>
 #include <urt_message.h>
 #include <urt_topic.h>
