From d8489df7d8927b5e28219ad1bc6b1c86fb28895b Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Mon, 17 Jan 2022 12:59:36 +0000
Subject: [PATCH] Check tolling zone

---
 src/saem.c | 56 ++++++++++++++++++++++++++++++++++++++++++------------
 src/saem.h |  1 +
 2 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/src/saem.c b/src/saem.c
index b6fe41b..188fda9 100644
--- a/src/saem.c
+++ b/src/saem.c
@@ -1,26 +1,21 @@
 #include "saem.h"
 #include "facilities.h"
 #include "infrastructure.h"
+#include "tpm.h"
 
 #include <it2s-tender/time.h>
+#include <it2s-tender/geodesy.h>
+#include <it2s-tender/syslog.h>
+#include <it2s-tender/space.h>
 #include <itss-transport/TransportRequest.h>
 #include <itss-security/SecurityRequest.h>
 #include <itss-security/SecurityReply.h>
 #include <saem/asn_application.h>
+#include <saem/per_decoder.h>
 #include <saem/per_encoder.h>
 #include <tpm/TollingPaymentInfo.h>
 #include <saem/SAEM.h>
-#include <syslog.h>
 
-#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__)
-#define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]",  __func__, __LINE__, ##__VA_ARGS__)
-#define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]",  __func__, __LINE__, ##__VA_ARGS__)
-
-#ifndef NDEBUG
-#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__)
-#else
-#define syslog_debug(msg, ...)
-#endif
 
 SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem, uint8_t* neighbour) {
 
@@ -82,6 +77,23 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem, uint8_t* ne
                                            si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, 
                                            si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
                                            );
+
+                                    if (facilities->station_type != 15 && facilities->tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) {
+                                        asn_dec_rval_t dec = uper_decode_complete(
+                                                NULL, 
+                                                &asn_DEF_TollingPaymentInfo, 
+                                                (void**) &facilities->tolling.infos.z[facilities->tolling.infos.length],
+                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, 
+                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
+                                                );
+                                        syslog_err("dec.code:: %d", dec.code);
+                                        if (!dec.code) {
+                                            bulletin->to_consume[bulletin->to_provide_len]->info.internal_p = facilities->tolling.infos.z[facilities->tolling.infos.length];
+                                            ++facilities->tolling.infos.length;
+                                        } else {
+                                            ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, facilities->tolling.infos.z[facilities->tolling.infos.length]);
+                                        }
+                                    }
                                     break;
 
                                 case ServiceInfoExt_PR_addressIPv6:
@@ -301,7 +313,6 @@ void *sa_service(void *fc) {
     int sleep_ms = 100;
     int mk_saem_n_sleep = 0;
 
-
     while (!facilities->exit) {
 
         if (bulletin->to_provide_len && sleep_ms*mk_saem_n_sleep >= 1000) {
@@ -320,6 +331,13 @@ void *sa_service(void *fc) {
         }
         ++mk_saem_n_sleep;
 
+        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);
+
         pthread_mutex_lock(&bulletin->lock);
         for (int a = 0; a < bulletin->to_consume_len; ++a) {
             /* do some checks, e.g.
@@ -329,7 +347,21 @@ void *sa_service(void *fc) {
             // Tolling
             if (facilities->tolling.enabled && 
                 bulletin->to_consume[a]->its_aid == 0 && 
-                !bulletin->to_consume[a]->n_trigger) {
+                !bulletin->to_consume[a]->n_trigger &&
+                facilities->station_type != 15) {
+
+                if (!it2s_tender_is_inside_rectangle( 
+                            lat/1e07,
+                            lon/1e07,
+                            ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.latitude/1e07,
+                            ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.longitude/1e07,
+                            ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.a/10.0,
+                            ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.b/10.0,
+                            ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.angle/10.0,
+                            DCM_HAVERSINE
+                            )) {
+                    continue;
+                }
 
                 switch (facilities->tolling.protocol) {
                     case TOLLING_PROTOCOL_SIMPLE:
diff --git a/src/saem.h b/src/saem.h
index b251181..36c03b6 100644
--- a/src/saem.h
+++ b/src/saem.h
@@ -22,6 +22,7 @@ typedef struct announcement {
         uint16_t context_len;
         uint8_t* data;
         uint16_t data_len;
+        void* internal_p;
     } info;
 
     uint64_t station_id;