From 5441c102cb279242c82aab09acc1ffb0a7c2b805 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Mon, 8 May 2023 10:21:24 +0100
Subject: [PATCH] Variable VCM frequency based on curvature change

---
 src/edm.c |  5 -----
 src/edm.h |  1 -
 src/vcm.c | 43 ++++++++++++++++++++++++++++++++++++++++++-
 src/vcm.h |  1 +
 4 files changed, 43 insertions(+), 7 deletions(-)

diff --git a/src/edm.c b/src/edm.c
index a61fbdf..edcc6fd 100644
--- a/src/edm.c
+++ b/src/edm.c
@@ -63,11 +63,6 @@ cleanup:
     return rv;
 }
 
-int edm_decap(uint8_t* data, uint16_t data_len, int its_msg_type) {
-
-    return 0;
-}
-
 void edm_init() {
     facilities.edm.app_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ);
     pthread_mutex_init(&facilities.edm.lock, NULL);
diff --git a/src/edm.h b/src/edm.h
index d0be32f..ec9a56c 100644
--- a/src/edm.h
+++ b/src/edm.h
@@ -11,6 +11,5 @@ typedef struct edm {
 } edm_t;
 
 int edm_encap(uint8_t* msg, uint16_t* msg_len, uint16_t msg_buf_len, int its_msg_type);
-int edm_decap(uint8_t* data, uint16_t data_len, int its_msg_type);
 
 void edm_init();
diff --git a/src/vcm.c b/src/vcm.c
index f34af61..2c5aae0 100644
--- a/src/vcm.c
+++ b/src/vcm.c
@@ -12,6 +12,10 @@
 #include <it2s-asn/itss-management/ManagementRequest.h>
 #include <it2s-asn/vcm/VCM.h>
 
+#include <tgmath.h>
+
+#define TRAJECTORY_C_TH 1.0
+
 static int do_paths_intersect(
         itss_st_t* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, 
         itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
@@ -135,6 +139,7 @@ static void tx_vcm(VCM_t* vcm) {
 
     itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
 
+    log_warn("Last VCM sent %d ms ago", itss_time_get() - facilities.coordination.t_last_send_vcm);
     facilities.coordination.t_last_send_vcm = itss_time_get();
 
     if (facilities.logging.recorder) {
@@ -1131,7 +1136,43 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
     bool send = false;
 
     if (now > coordination->t_last_send_vcm + coordination->vcm_period_max) {
-        send = true;
+        return true;
+    }
+
+    if (now < coordination->t_last_send_vcm + coordination->vcm_period_min) {
+        return false;
+    }
+
+    if (facilities.station_type != StationType_roadSideUnit) { // Vehicle
+
+        itss_trajectory_lock();
+
+        if (epv.trajectory.len > 2) {
+
+            double c_sum = 0;
+            for (int i = 0; i < epv.trajectory.len - 2; ++i) {
+                double d1 = itss_haversine(
+                        epv.trajectory.path[i+1].latitude / 1.0e7,
+                        epv.trajectory.path[i+1].longitude / 1.0e7,
+                        epv.trajectory.path[i].latitude / 1.0e7,
+                        epv.trajectory.path[i].longitude / 1.0e7);
+                double t1 = atan2(
+                        epv.trajectory.path[i+1].latitude - epv.trajectory.path[i].latitude ,
+                        epv.trajectory.path[i+1].longitude - epv.trajectory.path[i].longitude);
+                double t2 = atan2(
+                        epv.trajectory.path[i+2].latitude - epv.trajectory.path[i].latitude ,
+                        epv.trajectory.path[i+2].longitude - epv.trajectory.path[i].longitude);
+                c_sum += fabs(sin(t2-t1)*d1);
+
+            }
+
+            if (fabs(c_sum - coordination->previous_c) > TRAJECTORY_C_TH) {
+                send = true;
+                coordination->previous_c = c_sum;
+            } 
+        }
+
+        itss_trajectory_unlock(epv);
     }
 
     return send;
diff --git a/src/vcm.h b/src/vcm.h
index 2f641fb..231f17a 100644
--- a/src/vcm.h
+++ b/src/vcm.h
@@ -50,6 +50,7 @@ typedef struct coordination {
     uint64_t t_last_send_vcm; /* timestamp of last sent (basic) VCM */
     uint64_t vcm_period_min;
     uint64_t vcm_period_max;
+    double previous_c;
 
     struct {
         VCM_t* own_req; /* last VCM.request sent */