From 89943a2789ea74fdbad6ef399a89cfe3a1c7bd53 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Tue, 22 Feb 2022 15:52:34 +0000
Subject: [PATCH] PCM resolution delay

---
 src/pcm.c | 33 ++++++++++++++++++++++++---------
 src/pcm.h |  3 +++
 2 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/src/pcm.c b/src/pcm.c
index 072e84b..8689ea3 100644
--- a/src/pcm.c
+++ b/src/pcm.c
@@ -48,7 +48,7 @@ static int are_vehicles_intersecting(
     return 0;
 }
 
-static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) {
+static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
     int rv = 0;
 
     CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
@@ -64,6 +64,9 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) {
 
     uint64_t now = it2s_tender_get_clock(&facilities->epv);
 
+    neighbour->proposed = true;
+    neighbour->t_proposal = now;
+
     int32_t lat, lon;
     it2s_tender_lock_space(&facilities->epv);
     it2s_tender_get_space(&facilities->epv);
@@ -165,7 +168,7 @@ cleanup:
     return rv;
 }
 
-static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) {
+static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
     int rv = 0;
 
     CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
@@ -175,7 +178,7 @@ static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) {
     return rv;
 }
 
-static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm) {
+static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
 
     int rv = 0;
 
@@ -188,6 +191,21 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm)
     it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
     uint16_t trajectoryA_len = 0;
 
+    uint64_t now = it2s_tender_get_clock(&facilities->epv);
+
+    if (now < neighbour->t_iid + MC_RESOLUTION_TIMEOUT) {
+        rv = 0;
+        goto cleanup;
+    }
+
+    neighbour->intersecting = true;
+    neighbour->t_iid = now;
+
+    if (now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT) {
+        rv = 0;
+        goto cleanup;
+    }
+
     int32_t lat, lon;
     it2s_tender_lock_space(&facilities->epv);
     it2s_tender_get_space(&facilities->epv);
@@ -200,7 +218,6 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm)
     memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
     it2s_tender_unlock_trajectory(&facilities->epv);
 
-    uint64_t now = it2s_tender_get_clock(&facilities->epv);
 
     trajectoryA[0].latitude = lat;
     trajectoryA[0].longitude = lon;
@@ -356,10 +373,10 @@ int pcm_check(void* fc, PCM_t* pcm) {
             if (mvc->negotiation) {
                 switch (mvc->negotiation->present) {
                     case CoordinationNegotiation_PR_request:
-                        pcm_check_handle_request(facilities, pcm);
+                        pcm_check_handle_request(facilities, pcm, &coordination->neighbours[ni]);
                         break; 
                     case CoordinationNegotiation_PR_reply:
-                        pcm_check_handle_reply(facilities, pcm);
+                        pcm_check_handle_reply(facilities, pcm, &coordination->neighbours[ni]);
                         break; 
                     default:
                         break;
@@ -400,9 +417,7 @@ int pcm_check(void* fc, PCM_t* pcm) {
                 if (intersecting) {
                     syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms", 
                             pcm->header.stationID, trajectoryA[index].latitude/1.0e7, trajectoryA[index].longitude/1.0e7, trajectoryA[index].timestamp - now);
-                    coordination->neighbours[ni].intersecting = true;
-                    coordination->neighbours[ni].t_iid = now;
-                    pcm_check_intersection_detected(facilities, pcm);
+                    pcm_check_intersection_detected(facilities, pcm, &coordination->neighbours[ni]);
                 }
             }
             break;
diff --git a/src/pcm.h b/src/pcm.h
index c2c0449..1225dca 100644
--- a/src/pcm.h
+++ b/src/pcm.h
@@ -11,6 +11,9 @@ typedef struct {
 
     bool intersecting;
     uint64_t t_iid; /* initial intersection detection */
+
+    bool proposed;
+    uint64_t t_proposal; 
 } mc_neighbour_s;
 
 typedef struct coordination {