From 89549fa7ac060a2e24ddd61e06a667d1d67a6241 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Fri, 23 Jul 2021 16:49:26 +0100
Subject: [PATCH] Config triggered SAEMs

---
 src/config.c     |  7 +++++++
 src/facilities.c |  4 ++--
 src/sa.c         | 17 +++++++++++------
 3 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/config.c b/src/config.c
index 5eded91..98c4b8c 100644
--- a/src/config.c
+++ b/src/config.c
@@ -367,6 +367,13 @@ int facilities_config(void* facilities_s) {
     ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep);
     zmq_close(management_socket);
 
+    if (config->facilities.saem.activate) {
+        facilities->bulletin.to_provide_len = 1;
+        facilities->bulletin.to_provide[0] = calloc(1, sizeof(announcement_t));
+        facilities->bulletin.to_provide[0]->endpoint.port = 7777;
+        facilities->bulletin.to_provide[0]->its_aid = config->facilities.saem.service_to_advertise;
+    }
+
     facilities->vehicle.length = config->facilities.vehicle.length;
     facilities->vehicle.width = config->facilities.vehicle.width;
     facilities->vehicle.role = config->facilities.vehicle.role;
diff --git a/src/facilities.c b/src/facilities.c
index 1397c68..e83dbb2 100644
--- a/src/facilities.c
+++ b/src/facilities.c
@@ -512,6 +512,7 @@ int main() {
     facilities.tx_queue = queue_init();
     facilities.den = calloc(1, sizeof(den_t));
     facilities.infrastructure = calloc(1, sizeof(infrastructure_t));
+    bulletin_init(&facilities.bulletin);
 
     time_t t;
     srand((unsigned) time(&t));
@@ -537,8 +538,7 @@ int main() {
         pthread_create(&facilities.cp_service, NULL, cp_service, (void*) &facilities);
 
     // SA
-    //bulletin_init(&facilities.bulletin);
-    //pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
+    pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
 
     uint8_t buffer[PACKET_MAX_LEN];
     syslog_info("[facilities] listening");
diff --git a/src/sa.c b/src/sa.c
index ce99af4..d64e016 100644
--- a/src/sa.c
+++ b/src/sa.c
@@ -31,6 +31,7 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
         return SAEM_INVALID_HEADER_VERSION;
     }
 
+    pthread_mutex_lock(&bulletin->lock);
 
     if (saem->sam.body.serviceInfos) {
         for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
@@ -38,11 +39,8 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
 
             uint16_t its_aid = si->serviceID;
 
-            if (!its_aid) {
-                continue;
-            }
-
             int index = -1;
+
             for (int a = 0; a < bulletin->to_consume_len; ++a) {
                 /* Check existence through stationID and itsAid */
                 if (saem->header.stationID == bulletin->to_consume[a]->station_id && 
@@ -91,6 +89,8 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
         }
     }
 
+    pthread_mutex_unlock(&bulletin->lock);
+
     return rv;
 }
 
@@ -102,6 +102,11 @@ void bulletin_init(bulletin_t* bulletin) {
     for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) {
         bulletin->to_consume[i] = calloc(1, sizeof(announcement_t));
     }
+
+    bulletin->to_provide_len = 1;
+    bulletin->to_provide[0] = calloc(1, sizeof(announcement_t));
+    bulletin->to_provide[0]->endpoint.port = 7777;
+
 }
 
 int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
@@ -110,7 +115,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
     SAEM_t* saem = calloc(1, sizeof(SAEM_t));
 
     /* header */
-    saem->header.protocolVersion = 2;
+    saem->header.protocolVersion = 1;
     saem->header.messageID = messageID_saem;
 
     pthread_mutex_lock(&facilities->id.lock);
@@ -255,7 +260,7 @@ void *sa_service(void *fc) {
         }
         pthread_mutex_unlock(&bulletin->lock);
 
-        sleep(1);
+        usleep(100000);
     }
 
     ASN_STRUCT_FREE(asn_DEF_TransportDataRequest, tdr);