From aec88625a11211faa47c4e4aee1d4115b2a3a60f Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Thu, 16 Dec 2021 16:51:32 +0000
Subject: [PATCH] Misc tolling improvements

---
 src/tpm.c | 41 ++++++++++++++++++++++++++++++++++++-----
 1 file changed, 36 insertions(+), 5 deletions(-)

diff --git a/src/tpm.c b/src/tpm.c
index 6e78b96..6bf1672 100644
--- a/src/tpm.c
+++ b/src/tpm.c
@@ -9,8 +9,6 @@
 int tpm_pay(void* fc) {
     int rv = 0;
 
-    syslog_info("[facilities] [tolling] issuing toll payment");
-
     facilities_t* facilities = (facilities_t*) fc;
     tolling_s* tolling = (tolling_s*) &facilities->tolling;
 
@@ -28,6 +26,8 @@ int tpm_pay(void* fc) {
     tolling->active = true;
     tolling->nonce = rand();
 
+    syslog_info("[facilities] [tolling] issuing toll payment > client: %ld | nonce: %ld", tolling->client_id, tolling->nonce);
+
     // TPM
     tpm = calloc(1, sizeof(TPM_t));
 
@@ -85,7 +85,6 @@ int tpm_pay(void* fc) {
     bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable;
     bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
     bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
-    it2s_tender_unlock_space(&facilities->epv);
 
     asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv));
 
@@ -144,6 +143,8 @@ cleanup:
 
 static void rsu_handle_recv(facilities_t* facilities, TollRequest_t* req) {
 
+    syslog_info("[facilities] [tolling] received toll payment > client: %ld | nonce: %ld", req->clientId, req->transactionNonce);
+
     const size_t buf_len = 2048;
     uint8_t buf[buf_len];
 
@@ -152,15 +153,43 @@ static void rsu_handle_recv(facilities_t* facilities, TollRequest_t* req) {
 
     // TPM
     tpm = calloc(1, sizeof(TPM_t));
+
+    tpm->header.messageID = 117;
+    tpm->header.protocolVersion = 0;
+    pthread_mutex_lock(&facilities->id.lock);
+    tpm->header.stationID = facilities->id.station_id;
+    pthread_mutex_unlock(&facilities->id.lock);
+
     asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv));
 
+    BasicContainer_t* bc = &tpm->tpm.camParameters.basicContainer;
+    tpm->tpm.camParameters.basicContainer.stationType = facilities->station_type;
+
+    it2s_tender_lock_space(&facilities->epv);
+    it2s_tender_get_space(&facilities->epv);
+    bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
+    bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
+
+    // Set GPS coordinates
+    bc->referencePosition.latitude = facilities->epv.space.latitude;
+    bc->referencePosition.longitude = facilities->epv.space.longitude;
+    uint16_t lat_conf = facilities->epv.space.latitude_conf;
+    uint16_t lon_conf = facilities->epv.space.longitude_conf;
+
+    tpm->tpm.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
+    it2s_tender_unlock_space(&facilities->epv);
+
+    tpm->tpm.tollingFlow.present = TollingFlow_PR_reply;
+    tpm->tpm.tollingFlow.choice.reply.clientId = req->clientId;
+    tpm->tpm.tollingFlow.choice.reply.transactionNonce = req->transactionNonce;
+
     // TODO check clientId
     // TODO dlt: check transaction
 
     // encode TPM 
     asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, buf, buf_len);
     if (enc.encoded == -1) {
-        syslog_err("[facilities] [tolling] error encoding TPM.request (%s)", enc.failed_type->name);
+        syslog_err("[facilities] [tolling] error encoding TPM.reply (%s)", enc.failed_type->name);
         goto cleanup;
     }
     size_t tpm_uper_len = (enc.encoded + 7) / 8;
@@ -189,7 +218,7 @@ static void rsu_handle_recv(facilities_t* facilities, TollRequest_t* req) {
     buf[0] = 4;
     enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1);
     if (enc.encoded == -1) {
-        syslog_err("[facilities] [tolling] error encoding TR TPM.request (%s)", enc.failed_type->name);
+        syslog_err("[facilities] [tolling] error encoding TR TPM.reply (%s)", enc.failed_type->name);
         goto cleanup;
     }
 
@@ -212,6 +241,8 @@ static void veh_handle_recv(tolling_s* tolling, TollReply_t* rep) {
         return;
     }
 
+    syslog_info("[facilities] [tolling] received tolling payment reply > client: %ld | nonce: %ld | accepted: %s", tolling->client_id, tolling->nonce, rep->confirmationCode == TollConfirmationCode_accepted ? "yes" : "no");
+
     tolling->active = false;
 }