From f93d83438b94796f9c02b59f7e2ce6febedcd98d Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Tue, 15 Feb 2022 18:14:56 +0000
Subject: [PATCH] DCM neighbour mgmt effortsNeighbour mgmt efforts

---
 README.md        | 29 +++++++++++++++++++++++++++++
 src/dcm.c        | 36 ++++++++++++++++++++++++++++++++++--
 src/dcm.h        | 16 ++++++++++++++++
 src/facilities.c |  1 +
 4 files changed, 80 insertions(+), 2 deletions(-)
 create mode 100644 README.md

diff --git a/README.md b/README.md
new file mode 100644
index 0000000..25e569b
--- /dev/null
+++ b/README.md
@@ -0,0 +1,29 @@
+## IT2S ITS-S Facilities
+
+IT2S implementation of the ETSI ITS Facilities layer.
+
+### Implemented services and messages
+
+- CAM
+- DENM
+- IVIM
+- CPM
+- SAEM (modded)
+- TPM (IT2S - Tolling Payments)
+- DCM (IT2S - Maneuver Coordination)
+
+### Compilation
+
+Install with CMake:
+
+```
+mkdir build
+cd build
+cmake ..
+make
+```
+
+### Usage
+
+Run the compiled `it2s-itss-facilities` executable.
+Needs IT2S ITS-S Transport, Security, Management and the Applications services to also be running.
diff --git a/src/dcm.c b/src/dcm.c
index a0c9721..fa88ecc 100644
--- a/src/dcm.c
+++ b/src/dcm.c
@@ -317,6 +317,7 @@ cleanup:
 
 int dcm_check(void* fc, DCM_t* dcm) {
     facilities_t* facilities = (facilities_t*) fc;
+    coordination_s* coordination = &facilities->coordination;
 
     int rv = 0;
 
@@ -358,13 +359,31 @@ int dcm_check(void* fc, DCM_t* dcm) {
     bool intersecting = false;
     int index = 0;
 
+    pthread_mutex_lock(&coordination->lock);
+
+    // Find neighbours
+    int ni = -1;
+    for (int i = 0; i < coordination->neighbours_len; ++i) {
+        if (coordination->neighbours->station_id == dcm->header.stationID) {
+            ni = i;
+            break;
+        }
+    }
+
+    // Add it if not found
+    if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) {
+        coordination->neighbours[coordination->neighbours_len].station_id = dcm->header.stationID;
+        ni = coordination->neighbours_len;
+        ++coordination->neighbours_len;
+    }
+
     if (dcm->dcm.negotiation) {
         switch (dcm->dcm.negotiation->present) {
             case CoordinationNegotiation_PR_request:
-                dcm_check_handle_request(facilities, dcm);
+               dcm_check_handle_request(facilities, dcm);
                break; 
             case CoordinationNegotiation_PR_reply:
-                dcm_check_handle_reply(facilities, dcm);
+               dcm_check_handle_reply(facilities, dcm);
                break; 
             default:
                break;
@@ -380,10 +399,13 @@ int dcm_check(void* fc, DCM_t* dcm) {
         if (intersecting) {
             syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms", 
                             dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
+            coordination->neighbours[ni].intersecting = true;
+            coordination->neighbours[ni].t_iid = now;
             dcm_check_intersection_detected(facilities, dcm);
         }
     }
 
+    pthread_mutex_unlock(&coordination->lock);
 
     return rv;
 }
@@ -450,6 +472,7 @@ cleanup:
 void* dc_service(void* fc) {
 
     facilities_t* facilities = (facilities_t*) fc;
+    coordination_s* coordination = (coordination_s*) &facilities->coordination;
 
     uint8_t dcm[512];
 
@@ -494,9 +517,18 @@ void* dc_service(void* fc) {
 
         queue_send(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
         
+        pthread_mutex_lock(&coordination->lock);
+
+        pthread_mutex_unlock(&coordination->lock);
+
+
         usleep(SLEEP_TIME_MS * 1000);
     }
 
 
     return NULL;
 }
+
+void coordination_init(coordination_s* coordination) {
+    pthread_mutex_init(&coordination->lock, NULL);
+}
diff --git a/src/dcm.h b/src/dcm.h
index ba613bc..cb2c0ef 100644
--- a/src/dcm.h
+++ b/src/dcm.h
@@ -3,10 +3,26 @@
 #include <stdbool.h>
 #include <dcm/DCM.h>
 
+#define MC_MAX_NEIGHBOURS 32
+#define MC_RESOLUTION_TIMEOUT 1000
+
+typedef struct {
+    uint64_t station_id;
+
+    bool intersecting;
+    uint64_t t_iid; /* initial intersection detection */
+} mc_neighbour_s;
+
 typedef struct coordination {
     bool active;
 
+    pthread_mutex_t lock;
+    
+    mc_neighbour_s neighbours[MC_MAX_NEIGHBOURS];
+    uint8_t neighbours_len;
+
 } coordination_s;
 
 int dcm_check(void* fc, DCM_t* dcm);
 void* dc_service(void* fc);
+void coordination_init(coordination_s* coordination);
diff --git a/src/facilities.c b/src/facilities.c
index 58516ca..950ad8f 100644
--- a/src/facilities.c
+++ b/src/facilities.c
@@ -678,6 +678,7 @@ int main() {
     facilities.infrastructure = calloc(1, sizeof(infrastructure_t));
     facilities.dissemination = dissemination_init();
     bulletin_init(&facilities.bulletin);
+    coordination_init(&facilities.coordination);
 
     time_t t;
     srand((unsigned) time(&t));