DCM intersection check

src/dcm.c

@@ -4,17 +4,91 @@
 #include <it2s-tender/epv.h>
 #include <it2s-tender/space.h>
 #include <it2s-tender/trajectory.h>
+#include <it2s-tender/geodesy.h>
 #include <itss-transport/TransportRequest.h>
 #include <dcm/DCM.h>
 
 #define SLEEP_TIME_MS 100
 
-int dcm_check() {
+static int are_paths_intersecting(it2s_tender_st_s* tA, int tA_len, it2s_tender_st_s* tB, int tB_len) {
+
+    // TODO check first time intersection, then the spacial trajectory
+
+    double A1[2], A2[2], B1[2], B2[2];
+    uint64_t tsA, tsB;
+    for (int a = 0; a < tA_len-1; ++a) {
+        A1[0] = tA[a].latitude;
+        A1[1] = tA[a].longitude;
+        A2[0] = tA[a+1].latitude;
+        A2[1] = tA[a+1].longitude;
+
+
+        for (int b = 0; b < tB_len-1; ++b) {
+            B1[0] = tB[b].latitude;
+            B1[1] = tB[b].longitude;
+            B2[0] = tB[b+1].latitude;
+            B2[1] = tB[b+1].longitude;
+
+            if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
+                if (tA[a].timestamp < tB[b].timestamp + 2000 &&
+                    tA[a].timestamp > tB[b].timestamp - 2000) {
+                    syslog_info("[facilities] [dc] intersecting @ (%d, %d) in %ld ms", tA[a].latitude, tA[a].longitude, tA[a].timestamp-tA[0].timestamp);
+                    return 1;
+                }
+            }
+        }
+    }
 
     return 0;
 }
 
-int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
+int dcm_check(void* fc, DCM_t* dcm) {
+    facilities_t* facilities = (facilities_t*) fc;
+
+    it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
+    it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
+    uint16_t trajectoryA_len = 0;
+    uint16_t trajectoryB_len = 0;
+
+    uint64_t now = it2s_tender_get_clock(&facilities->epv);
+
+    int32_t lat, lon;
+    it2s_tender_lock_space(&facilities->epv);
+    it2s_tender_get_space(&facilities->epv);
+    lat = facilities->epv.space.latitude;
+    lon = facilities->epv.space.longitude;
+    it2s_tender_unlock_space(&facilities->epv);
+
+    it2s_tender_lock_trajectory(&facilities->epv);
+    trajectoryA_len = facilities->epv.trajectory.len;
+    memcpy(trajectoryA + sizeof(it2s_tender_st_s), facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
+    it2s_tender_unlock_trajectory(&facilities->epv);
+
+    trajectoryA[0].latitude = lat;
+    trajectoryA[0].longitude = lon;
+    trajectoryA[0].timestamp = now;
+    ++trajectoryA_len;
+
+    trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude;
+    trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude;
+    asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp);
+    ++trajectoryB_len;
+    for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
+        trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude;
+        trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude;
+        asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp);
+        ++trajectoryB_len;
+    }
+
+    if (trajectoryA_len > 1 && trajectoryB_len > 1) {
+        are_paths_intersecting(trajectoryA, trajectoryA_len, trajectoryB, trajectoryB_len);
+    }
+
+cleanup:
+    return 0;
+}
+
+static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
     int rv = 0;
 
     it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];

src/dcm.h

@@ -1,12 +1,12 @@
 #pragma once
 
 #include <stdbool.h>
+#include <dcm/DCM.h>
 
 typedef struct coordination {
     bool active;
 
 } coordination_s;
 
-int dcm_check();
+int dcm_check(void* fc, DCM_t* dcm);
-
 void* dc_service(void* fc);

src/facilities.c

@@ -122,6 +122,12 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
                     handled_msg = true;
                     break;
 
+                case 2043: /* maneuvers */
+                    its_msg_descriptor = &asn_DEF_DCM;
+                    its_msg = calloc(1, sizeof(DCM_t));
+                    handled_msg = true;
+                    break;
+
                 default:
                     syslog_debug("[facilities] messsage with unhandled BTP port received, ignoring");
                     goto cleanup;
@@ -216,6 +222,10 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
                     tpm_recv(facilities, its_msg, neighbour_cert);
                     break;
 
+                case 2043:
+                    dcm_check(facilities, its_msg);
+                    break;
+
                 default:
                     break;
             }