#include "tpm.h"
#include "facilities.h"

#include <it2s-tender/constants.h>
#include <itss-security/SecurityRequest.h>
#include <itss-security/SecurityReply.h>
#include <itss-facilities/FacilitiesIndication.h>
#include <it2s-tender/time.h>
#include <itss-transport/TransportRequest.h>
#include <it2s-tender/space.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/recorder.h>
#include <stdint.h>
#include <string.h>

int tpm_is_inside_zone(void* fc, tolling_info_t* ti) {
    int rv = 0;

    facilities_t* facilities = (facilities_t*) fc;
    double point[2];
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    point[0] = facilities->epv.space.latitude/1.0e7;
    point[1] = facilities->epv.space.longitude/1.0e7;
    it2s_tender_unlock_space(&facilities->epv);

    if (it2s_tender_is_inside_polygon(point, ti->zone.polygon, ti->zone.polygon_len)) {
        return 1;
    }

    return 0;
}

int tpm_pay(void* fc, tolling_info_t* info, void* security_socket, uint8_t* neighbour, uint8_t* dst_addr) {
    int rv = 0;

    facilities_t* facilities = (facilities_t*) fc;
    tolling_t* tolling = (tolling_t*) &facilities->tolling;

    pthread_mutex_lock(&facilities->epv.time.lock);
    tolling->tz = it2s_tender_get_now(TIME_MICROSECONDS) ;
    pthread_mutex_unlock(&facilities->epv.time.lock);

    TPM_t* tpm = NULL;
    TransportRequest_t* tr = NULL;
    SecurityRequest_t* sreq = NULL;
    SecurityReply_t* srep = NULL;
    FacilitiesIndication_t* fi = NULL;

    const size_t buf_len = 2048;
    uint8_t tpm_uper[buf_len];
    uint8_t buf[buf_len];

    if (!tolling->enabled) {
        syslog_debug("[facilities] [tolling] tolling is disabled");
        goto cleanup;
    }

    tolling->station.obu.active = true;
    tolling->station.obu.nonce = rand() + 1;

    syslog_info("[facilities] [tolling] issuing toll payment > client: %ld | nonce: %ld", tolling->station.obu.client_id, tolling->station.obu.nonce);

    // 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);

    tpm->tpm = calloc(1, sizeof(TollingPaymentMessage_t));

    // timestamp
    asn_ulong2INTEGER(&tpm->tpm->timestamp, it2s_tender_get_clock(&facilities->epv));

    // stationType
    tpm->tpm->stationType = facilities->station_type;

    // referencePosition
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    tpm->tpm->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
    tpm->tpm->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
    tpm->tpm->referencePosition.latitude = facilities->epv.space.latitude;
    tpm->tpm->referencePosition.longitude = facilities->epv.space.longitude;
    uint16_t lat_conf = facilities->epv.space.latitude_conf;
    uint16_t lon_conf = facilities->epv.space.longitude_conf;
    it2s_tender_unlock_space(&facilities->epv);
    if (lat_conf > lon_conf) {
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
    } else {
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
    }

    tpm->tpm->tollingType = calloc(1, sizeof(TollingType_t));
    TollingType_t* type = tpm->tpm->tollingType;

    tolling->station.obu.toll_type = info->asn->tollType;
    switch (info->asn->tollType) {
        case TollType_entry:
            type->present = TollingType_PR_entry;
            type->choice.entry.present = TollingEntry_PR_request;
            type->choice.entry.choice.request.clientId = tolling->station.obu.client_id;
            type->choice.entry.choice.request.infoId = info->asn->id;
            type->choice.entry.choice.request.transactionNonce = tolling->station.obu.nonce;
            break;
        case TollType_exit:
            type->present = TollingType_PR_exit;
            type->choice.exit = calloc(1, sizeof(TollingExit_t));
            type->choice.exit->present = TollingExit_PR_request;
            type->choice.exit->choice.request = calloc(1, sizeof(TollingExitRequest_t));
            type->choice.exit->choice.request->clientId = tolling->station.obu.client_id;
            type->choice.exit->choice.request->infoId = info->asn->id;
            type->choice.exit->choice.request->transactionNonce = tolling->station.obu.nonce;
            uint8_t b_tep[1024];
            asn_enc_rval_t e_tep = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tolling->station.obu.entry_proof, b_tep, 1024);
            if (e_tep.encoded == -1) {
                syslog_err("[facilities] [tolling] error encoding TPM entry proof");
                return 1;
            }
            uper_decode_complete(NULL, &asn_DEF_TPM, (void**) &type->choice.exit->choice.request->entryProof, b_tep, (e_tep.encoded+7)/8);
            break;
        case TollType_single:
            type->present = TollingType_PR_single;
            type->choice.single.present = TollingSingle_PR_request;
            type->choice.single.choice.request.clientId = tolling->station.obu.client_id;
            type->choice.single.choice.request.infoId = info->asn->id;
            type->choice.single.choice.request.transactionNonce = tolling->station.obu.nonce;
            break;
    }

    // Encode TollingPaymentMessage 
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm->tpm, buf, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    if (tolling->protocol == TOLLING_PROTOCOL_SIMPLE) {
        // Sign
        sreq = calloc(1, sizeof(SecurityRequest_t));
        sreq->present = SecurityRequest_PR_sign;
        sreq->choice.sign.data.size = enc.encoded;
        sreq->choice.sign.data.buf = malloc(enc.encoded);
        memcpy(sreq->choice.sign.data.buf, buf, enc.encoded);

        buf[0] = 4;
        enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, 2047);
        syslog_debug("[facilities]->[security] SecurityRequest.sign (%ldB)", enc.encoded+1);
        zmq_send(security_socket, buf, enc.encoded+1, 0);
        int32_t rl = zmq_recv(security_socket, buf, buf_len, 0);
        syslog_debug("[facilities]<-[security] SecurityReply.sign (%dB)", rl);

        if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, rl).code) {
            syslog_err("[facilities] SecurityReply.sign decode failure");
            rv = 1;
            goto cleanup;
        }

        if (srep->returnCode == SecurityReplyReturnCode_rejected) {
            syslog_err("[facilities] SecurityReply.sign rejected");
            rv = 1;
            goto cleanup;
        }

        tpm->tpmSignature = calloc(1, sizeof(TpmSignature_t));
        tpm->tpmSignature->r.size = srep->data->choice.sign.r.size;
        tpm->tpmSignature->r.buf = malloc(srep->data->choice.sign.r.size);
        memcpy(tpm->tpmSignature->r.buf, srep->data->choice.sign.r.buf,  srep->data->choice.sign.r.size);
        tpm->tpmSignature->s.size = srep->data->choice.sign.s.size;
        tpm->tpmSignature->s.buf = malloc(srep->data->choice.sign.s.size);
        memcpy(tpm->tpmSignature->s.buf, srep->data->choice.sign.s.buf,  srep->data->choice.sign.s.size);
        tpm->tpmSignature->signer.size = srep->data->choice.sign.signer.size;
        tpm->tpmSignature->signer.buf = malloc(srep->data->choice.sign.signer.size);
        memcpy(tpm->tpmSignature->signer.buf, srep->data->choice.sign.signer.buf,  srep->data->choice.sign.signer.size);
        tpm->tpmSignature->type = srep->data->choice.sign.type;
    }

    // Encode TPM 
    enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, tpm_uper, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding TPM.request (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    size_t tpm_uper_len = (enc.encoded + 7) / 8;

    // [transport] request (TR)
    tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;

    uint64_t id = 0;

    switch (facilities->tolling.protocol) {
        case TOLLING_PROTOCOL_SIMPLE:
            tr->choice.packet.present = TransportPacketRequest_PR_btp;
            BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

            bpr->id = rand() + 1;
            id = bpr->id;

            bpr->gn.securityProfile.encrypt = true;
            bpr->gn.securityProfile.sign = true;

            bpr->gn.securityNeighbour = calloc(1, sizeof(OCTET_STRING_t));
            bpr->gn.securityNeighbour->size = 8;
            bpr->gn.securityNeighbour->buf = malloc(8);
            memcpy(bpr->gn.securityNeighbour->buf, neighbour, 8);

            bpr->data.size = tpm_uper_len;
            bpr->data.buf = malloc(tpm_uper_len);
            memcpy(bpr->data.buf, tpm_uper, tpm_uper_len);

            bpr->destinationPort = 7011;
            bpr->btpType = BTPType_btpB;
            bpr->gn.destinationAddress.buf = malloc(6);
            for (int i = 0; i < 6; ++i) {
                bpr->gn.destinationAddress.buf[i] = 0xff;
            }
            bpr->gn.destinationAddress.size = 6;
            bpr->gn.trafficClass = 2;
            bpr->gn.packetTransportType = PacketTransportType_shb;
            break;
        case TOLLING_PROTOCOL_TLS:
            sreq = calloc(1, sizeof(SecurityRequest_t));
            sreq->present = SecurityRequest_PR_tlsSend;
            sreq->choice.tlsSend.data.buf = malloc(tpm_uper_len);
            sreq->choice.tlsSend.data.size = tpm_uper_len;
            memcpy(sreq->choice.tlsSend.data.buf, tpm_uper, tpm_uper_len);
            id = rand() + 1;
            if (!tolling->station.obu.tls_conn_id) {
                tolling->station.obu.tls_conn_id = id;
            }
            sreq->choice.tlsSend.connId = tolling->station.obu.tls_conn_id;

            buf[0] = 4;
            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, buf_len-1);
            
            syslog_debug("[facilities] [tolling]-> SecurityRequest.tlsSend ->[security] | size:%ldB", enc.encoded);
            zmq_send(security_socket, buf, enc.encoded+1, 0);
            int rc = zmq_recv(security_socket, buf, buf_len, 0);
            if (rc == -1) {
                syslog_err("[facilities] [tolling]-> SecurityRequest.tlsSend ->[security] <TIMEOUT>");
                rv = 1;
                goto cleanup;
            }
            syslog_debug("[facilities] [tolling]<- SecurityReply.tlsSend <-[security] | size:%dB", rc);

            SecurityReply_t* srep = NULL;
            asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, buf_len);
            
            if (dec.code || 
                srep->returnCode != SecurityReplyReturnCode_accepted ||
                !srep->data ||
                srep->data->present != SecurityReplyData_PR_tlsSend) {
                syslog_err("[facilities] [tolling]<- SecurityReply.tlsSend rejected");
                rv = 1;
                goto cleanup;
            }

            // TR TCP
            tr->choice.packet.present = TransportPacketRequest_PR_tcp;
            TCPPacketRequest_t* tcp = &tr->choice.packet.choice.tcp;

            tcp->id = id;

            tcp->destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
            tcp->destinationAddress->buf = malloc(16);
            tcp->destinationAddress->size = 16;
            memcpy(tcp->destinationAddress->buf, dst_addr, 16);
            tcp->destinationPort = 7011;
            tcp->sourcePort = 7011;

            tcp->data.buf = malloc(srep->data->choice.tlsSend.data.size);
            tcp->data.size = srep->data->choice.tlsSend.data.size;
            memcpy(tcp->data.buf, srep->data->choice.tlsSend.data.buf, srep->data->choice.tlsSend.data.size);

            tcp->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
            tcp->gn->packetTransportType = PacketTransportType_shb;
            tcp->gn->destinationAddress.buf = calloc(1, 6);
            tcp->gn->destinationAddress.size = 6;
            tcp->gn->securityProfile.encrypt = false;
            tcp->gn->securityProfile.sign = true;
            break;
    }

    // encode TR
    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);
        rv = 1;
        goto cleanup;
    }

    it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, id, 
            tolling->protocol == TOLLING_PROTOCOL_SIMPLE ? "TR.packet.btp" : "TR.packet.tcp");

    // Logging 
    if (facilities->logging.dbms) {
        pthread_mutex_lock(&facilities->id.lock);
        uint64_t station_id = facilities->id.station_id;
        pthread_mutex_unlock(&facilities->id.lock);
        it2s_tender_db_add(facilities->logging.dbms, station_id, id, &facilities->epv, true, 117, NULL, tpm_uper, tpm_uper_len);
    }

    // send to [applications]
    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.id = id;
    fi->choice.message.itsMessageType = 7011;
    fi->choice.message.data.size = tpm_uper_len;
    fi->choice.message.data.buf = malloc(tpm_uper_len);
    memcpy(fi->choice.message.data.buf, tpm_uper, tpm_uper_len);
    enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding FI TPM.request (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, id, "FI.message (TPM.request)");

    if (facilities->logging.recorder) {
        uint16_t buffer_len = 2048;
        uint8_t buffer[buffer_len];
        int e = it2s_tender_management_record_packet_sdu(
                buffer,
                buffer_len,
                tpm_uper,
                tpm_uper_len,
                id,
                it2s_tender_get_clock(&facilities->epv),
                ITSS_FACILITIES,
                true);
        if (e != -1) {
            it2s_tender_queue_send(facilities->tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set");  
        }
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_TPM, tpm);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
    ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);

    return rv;
}

static void rsu_handle_recv(facilities_t* facilities, TPM_t* tpm_rx, void* security_socket, uint8_t* neighbour, uint8_t* src_addr) {

    if (!tpm_rx->tpm->tollingType) {
        syslog_err("[facilities] [tolling] received TPM does not have a type");
        return;
    }

    TollingType_t* type_rx = tpm_rx->tpm->tollingType;

    uint64_t client_id, nonce, info_id;

    switch (type_rx->present) {
        case TollingType_PR_entry:
            if (type_rx->choice.entry.present != TollingEntry_PR_request) {
                syslog_err("[facilities] [tolling] received TPM.entry is not request");
                return;
            }
            client_id = type_rx->choice.entry.choice.request.clientId;
            nonce = type_rx->choice.entry.choice.request.transactionNonce;
            info_id = type_rx->choice.entry.choice.request.infoId;
            break;
        case TollingType_PR_exit:
            if (!type_rx->choice.exit ||
                type_rx->choice.exit->present != TollingExit_PR_request ||
                !type_rx->choice.exit->choice.request
                ) {
                syslog_err("[facilities] [tolling] received TPM.exit is not request");
                return;
            }
            client_id = type_rx->choice.exit->choice.request->clientId;
            nonce = type_rx->choice.exit->choice.request->transactionNonce;
            info_id = type_rx->choice.exit->choice.request->infoId;

            if (!type_rx->choice.exit->choice.request->entryProof) {
                syslog_err("[facilities] [tolling] received TPM.exit.request does not contain entry proof");
                return;
            }

            break;
        case TollingType_PR_single:
            if (type_rx->choice.single.present != TollingSingle_PR_request) {
                syslog_err("[facilities] [tolling] received TPM.single is not request");
                return;
            }
            client_id = type_rx->choice.single.choice.request.clientId;
            nonce = type_rx->choice.single.choice.request.transactionNonce;
            info_id = type_rx->choice.single.choice.request.infoId;
            break;
        default:
            syslog_err("[facilities] [tolling] received TPM has unrecognized type");
            return;
    }

    tolling_t* tolling = &facilities->tolling;

    switch (tolling->protocol) {
        case TOLLING_PROTOCOL_SIMPLE:
            syslog_info("[facilities] [tolling] received toll payment > client: %lld (certificate id: %02x%02x%02x) | nonce: %lld", 
            (long long) client_id, 
            neighbour ? neighbour[5] : 0,
            neighbour ? neighbour[6] : 0,
            neighbour ? neighbour[7] : 0,
            (long long) nonce
            );
            break;
        case TOLLING_PROTOCOL_TLS:
            syslog_info("[facilities] [tolling] received toll payment > client: %lld | nonce: %lld", 
            (long long) client_id, 
            (long long) nonce
            );
            break;
    }

    const size_t buf_len = 2048;
    uint8_t buf[buf_len];
    uint8_t tpm_uper[buf_len];

    asn_enc_rval_t enc;
    TransportRequest_t* tr = NULL;
    SecurityRequest_t* sreq = NULL;
    SecurityReply_t* srep = NULL;
    FacilitiesIndication_t* fi = NULL;
    TPM_t* tpm = NULL;

    if (tolling->protocol == TOLLING_PROTOCOL_SIMPLE) {

        if (!tpm_rx->tpmSignature) {
            syslog_err("[facilities] [tolling] in simple mode but TPM without signature received");
            goto cleanup;
        }

        // Encode TollingPaymentMessage 
        enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm_rx->tpm, buf, buf_len);
        if (enc.encoded == -1) {
            syslog_err("[facilities] [tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name);
            goto cleanup;
        }

        // Verify
        sreq = calloc(1, sizeof(SecurityRequest_t));
        sreq->present = SecurityRequest_PR_verify;
        // message
        sreq->choice.verify.message.size = enc.encoded;
        sreq->choice.verify.message.buf = malloc(enc.encoded);
        memcpy(sreq->choice.verify.message.buf, buf, enc.encoded);

        // r
        sreq->choice.verify.r.size = tpm_rx->tpmSignature->r.size;
        sreq->choice.verify.r.buf = malloc(tpm_rx->tpmSignature->r.size);
        memcpy(sreq->choice.verify.r.buf, tpm_rx->tpmSignature->r.buf, tpm_rx->tpmSignature->r.size);
        // s
        sreq->choice.verify.s.size = tpm_rx->tpmSignature->s.size;
        sreq->choice.verify.s.buf = malloc(tpm_rx->tpmSignature->s.size);
        memcpy(sreq->choice.verify.s.buf, tpm_rx->tpmSignature->s.buf, tpm_rx->tpmSignature->s.size);
        // signer
        sreq->choice.verify.signer.size = tpm_rx->tpmSignature->signer.size;
        sreq->choice.verify.signer.buf = malloc(tpm_rx->tpmSignature->signer.size);
        memcpy(sreq->choice.verify.signer.buf, tpm_rx->tpmSignature->signer.buf, tpm_rx->tpmSignature->signer.size);
        // signature type
        sreq->choice.verify.type = tpm_rx->tpmSignature->type;

        buf[0] = 4;
        enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, 2047);
        syslog_debug("[facilities]->[security] SecurityRequest.verify (%ldB)", enc.encoded+1);
        zmq_send(security_socket, buf, enc.encoded+1, 0);
        int32_t rl = zmq_recv(security_socket, buf, buf_len, 0);
        syslog_debug("[facilities]<-[security] SecurityReply.verify (%dB)", rl);

        if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, rl).code) {
            syslog_err("[facilities] SecurityReply.verify decode failure");
            goto cleanup;
        }

        if (srep->returnCode == SecurityReplyReturnCode_rejected) {
            syslog_err("[facilities] SecurityReply.verify rejected");
            goto cleanup;
        }

        if (srep->data->choice.verify.report != SecurityVerifyConfirmCode_success) {
            syslog_debug("[facilities] signature verify failed");
            goto cleanup;
        }

        ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
        ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
        sreq = NULL;
        srep = NULL;
    }

    // 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);

    tpm->tpm = calloc(1, sizeof(TollingPaymentMessage_t));

    // timestamp
    asn_ulong2INTEGER(&tpm->tpm->timestamp, it2s_tender_get_clock(&facilities->epv));

    // stationType
    tpm->tpm->stationType = facilities->station_type;

    // referencePosition
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    tpm->tpm->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
    tpm->tpm->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
    tpm->tpm->referencePosition.latitude = facilities->epv.space.latitude;
    tpm->tpm->referencePosition.longitude = facilities->epv.space.longitude;
    uint16_t lat_conf = facilities->epv.space.latitude_conf;
    uint16_t lon_conf = facilities->epv.space.longitude_conf;
    it2s_tender_unlock_space(&facilities->epv);
    if (lat_conf > lon_conf) {
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
    } else {
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
        tpm->tpm->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
    }

    tpm->tpm->tollingType = calloc(1, sizeof(TollingType_t));
    switch (tpm_rx->tpm->tollingType->present) {
        case TollingType_PR_entry:
            tpm->tpm->tollingType->present = TollingType_PR_entry;
            tpm->tpm->tollingType->choice.entry.present = TollingEntry_PR_reply;
            tpm->tpm->tollingType->choice.entry.choice.reply.clientId = client_id;
            tpm->tpm->tollingType->choice.entry.choice.reply.infoId = info_id;
            tpm->tpm->tollingType->choice.entry.choice.reply.transactionNonce = nonce;
            tpm->tpm->tollingType->choice.entry.choice.reply.confirmationCode = TollingConfirmationCode_accepted;
            break;
        case TollingType_PR_exit:
            tpm->tpm->tollingType->present = TollingType_PR_exit;
            tpm->tpm->tollingType->choice.exit = calloc(1, sizeof(TollingExit_t));
            tpm->tpm->tollingType->choice.exit->present = TollingExit_PR_reply;
            tpm->tpm->tollingType->choice.exit->choice.reply.clientId = client_id;
            tpm->tpm->tollingType->choice.exit->choice.reply.infoId = info_id;
            tpm->tpm->tollingType->choice.exit->choice.reply.transactionNonce = nonce;
            tpm->tpm->tollingType->choice.exit->choice.reply.confirmationCode = TollingConfirmationCode_accepted;
            break;
        case TollingType_PR_single:
            tpm->tpm->tollingType->present = TollingType_PR_single;
            tpm->tpm->tollingType->choice.single.present = TollingSingle_PR_reply;
            tpm->tpm->tollingType->choice.single.choice.reply.clientId = client_id;
            tpm->tpm->tollingType->choice.single.choice.reply.infoId = info_id;
            tpm->tpm->tollingType->choice.single.choice.reply.transactionNonce = nonce;
            tpm->tpm->tollingType->choice.single.choice.reply.confirmationCode = TollingConfirmationCode_accepted;
            break;
        default:
            break;
    }

    // TODO check clientId
    
    // Encode TollingPaymentMessage 
    enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm->tpm, buf, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name);
        goto cleanup;
    }

    if (tolling->protocol == TOLLING_PROTOCOL_SIMPLE) {
        // Sign
        sreq = calloc(1, sizeof(SecurityRequest_t));
        sreq->present = SecurityRequest_PR_sign;
        sreq->choice.sign.data.size = enc.encoded;
        sreq->choice.sign.data.buf = malloc(enc.encoded);
        memcpy(sreq->choice.sign.data.buf, buf, enc.encoded);

        buf[0] = 4;
        enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, 2047);
        syslog_debug("[facilities]->[security] SecurityRequest.sign (%ldB)", enc.encoded+1);
        zmq_send(security_socket, buf, enc.encoded+1, 0);
        int rc = zmq_recv(security_socket, buf, buf_len, 0);
        syslog_debug("[facilities]<-[security] SecurityReply.sign (%dB)", rc);

        if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, rc).code) {
            syslog_err("[facilities] SecurityReply.sign decode failure");
            goto cleanup;
        }

        if (srep->returnCode == SecurityReplyReturnCode_rejected) {
            syslog_err("[facilities] SecurityReply.sign rejected");
            goto cleanup;
        }

        tpm->tpmSignature->r.size = srep->data->choice.sign.r.size;
        tpm->tpmSignature->r.buf = malloc(srep->data->choice.sign.r.size);
        memcpy(tpm->tpmSignature->r.buf, srep->data->choice.sign.r.buf,  srep->data->choice.sign.r.size);
        tpm->tpmSignature->s.size = srep->data->choice.sign.s.size;
        tpm->tpmSignature->s.buf = malloc(srep->data->choice.sign.s.size);
        memcpy(tpm->tpmSignature->s.buf, srep->data->choice.sign.s.buf,  srep->data->choice.sign.s.size);
        tpm->tpmSignature->signer.size = srep->data->choice.sign.signer.size;
        tpm->tpmSignature->signer.buf = malloc(srep->data->choice.sign.signer.size);
        memcpy(tpm->tpmSignature->signer.buf, srep->data->choice.sign.signer.buf,  srep->data->choice.sign.signer.size);
        tpm->tpmSignature->type = srep->data->choice.sign.type;
    }

    // encode TPM 
    enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, tpm_uper, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding TPM.reply (%s)", enc.failed_type->name);
        goto cleanup;
    }
    size_t tpm_uper_len = (enc.encoded + 7) / 8;

    uint64_t id = 0;

    tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;

    // [transport] request (TR)
    switch (tolling->protocol) {
        case TOLLING_PROTOCOL_SIMPLE:
            tr->choice.packet.present = TransportPacketRequest_PR_btp;
            BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

            bpr->id = rand() + 1;
            id = bpr->id;

            bpr->gn.securityProfile.encrypt = true;
            bpr->gn.securityProfile.sign = true;

            bpr->data.size = tpm_uper_len;
            bpr->data.buf = malloc(tpm_uper_len);
            memcpy(bpr->data.buf, tpm_uper, tpm_uper_len);

            bpr->destinationPort = 7011;
            bpr->btpType = BTPType_btpB;
            bpr->gn.destinationAddress.buf = malloc(6);
            for (int i = 0; i < 6; ++i) {
                bpr->gn.destinationAddress.buf[i] = 0xff;
            }
            bpr->gn.destinationAddress.size = 6;
            bpr->gn.trafficClass = 2;
            bpr->gn.packetTransportType = PacketTransportType_shb;

            if (neighbour) { /* neighbour ID for [security] encryption */
                bpr->gn.securityNeighbour = calloc(1, sizeof(OCTET_STRING_t));
                bpr->gn.securityNeighbour->size = 8;
                bpr->gn.securityNeighbour->buf = malloc(8);
                memcpy(bpr->gn.securityNeighbour->buf, neighbour, 8);
            }
            break;

        case TOLLING_PROTOCOL_TLS:
            sreq = calloc(1, sizeof(SecurityRequest_t));
            sreq->present = SecurityRequest_PR_tlsSend;
            sreq->choice.tlsSend.data.buf = malloc(tpm_uper_len);
            sreq->choice.tlsSend.data.size = tpm_uper_len;
            memcpy(sreq->choice.tlsSend.data.buf, tpm_uper, tpm_uper_len);
            id = rand() + 1;
            // TODO handle various vehicles
            if (!tolling->station.obu.tls_conn_id) {
                tolling->station.obu.tls_conn_id = id;
            }
            sreq->choice.tlsSend.connId = tolling->station.obu.tls_conn_id;

            buf[0] = 4;
            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, buf_len-1);
            
            syslog_debug("[facilities] [tolling]-> SecurityRequest.tlsSend ->[security] | size:%ldB", enc.encoded);
            zmq_send(security_socket, buf, enc.encoded+1, 0);
            int rc = zmq_recv(security_socket, buf, buf_len, 0);
            if (rc == -1) {
                syslog_err("[facilities] [tolling]-> SecurityRequest.tlsSend ->[security] <TIMEOUT>");
                goto cleanup;
            }
            syslog_debug("[facilities] [tolling]<- SecurityReply.tlsSend <-[security] | size:%dB", rc);

            asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, buf_len);
            
            if (dec.code || 
                srep->returnCode != SecurityReplyReturnCode_accepted ||
                !srep->data ||
                srep->data->present != SecurityReplyData_PR_tlsSend) {
                syslog_err("[facilities] [tolling]<- SecurityReply.tlsSend rejected");
                goto cleanup;
            }

            // TR TCP
            tr->choice.packet.present = TransportPacketRequest_PR_tcp;
            TCPPacketRequest_t* tcp = &tr->choice.packet.choice.tcp;

            tcp->id = id;

            tcp->destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
            tcp->destinationAddress->buf = malloc(16);
            tcp->destinationAddress->size = 16;
            memcpy(tcp->destinationAddress->buf, src_addr, 16);
            tcp->destinationPort = 7011;
            tcp->sourcePort = 7011;

            tcp->data.buf = malloc(srep->data->choice.tlsSend.data.size);
            tcp->data.size = srep->data->choice.tlsSend.data.size;
            memcpy(tcp->data.buf, srep->data->choice.tlsSend.data.buf, srep->data->choice.tlsSend.data.size);

            tcp->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
            tcp->gn->packetTransportType = PacketTransportType_shb;
            tcp->gn->destinationAddress.buf = calloc(1, 6);
            tcp->gn->destinationAddress.size = 6;
            tcp->gn->securityProfile.encrypt = false;
            tcp->gn->securityProfile.sign = true;
            break;
    }

    // encode TR
    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.reply (%s)", enc.failed_type->name);
        goto cleanup;
    }

    it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, id, 
            tolling->protocol == TOLLING_PROTOCOL_SIMPLE ? "TR.packet.btp" : "TR.packet.tcp");

    // Logging 
    if (facilities->logging.dbms) {
        pthread_mutex_lock(&facilities->id.lock);
        uint64_t station_id = facilities->id.station_id;
        pthread_mutex_unlock(&facilities->id.lock);
        it2s_tender_db_add(facilities->logging.dbms, station_id, id, &facilities->epv, true, 117, NULL, tpm_uper, tpm_uper_len);
    }

    // send to [applications]
    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.id = id;
    fi->choice.message.itsMessageType = 7011;
    fi->choice.message.data.size = tpm_uper_len;
    fi->choice.message.data.buf = malloc(tpm_uper_len);
    memcpy(fi->choice.message.data.buf, tpm_uper, tpm_uper_len);
    enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding FI TPM.reply (%s)", enc.failed_type->name);
        goto cleanup;
    }

    it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, id, "FI.message (TPM.reply)");
    if (facilities->logging.recorder) {
        uint16_t buffer_len = 2048;
        uint8_t buffer[buffer_len];
        int e = it2s_tender_management_record_packet_sdu(
                buffer,
                buffer_len,
                tpm_uper,
                tpm_uper_len,
                id,
                it2s_tender_get_clock(&facilities->epv),
                ITSS_FACILITIES,
                true);
        if (e != -1) {
            it2s_tender_queue_send(facilities->tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set");  
        }
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_TPM, tpm);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
    ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
}

static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void* security_socket, uint8_t* neighbour) {

    if (!tpm_rx->tpm->tollingType) {
        syslog_err("[facilities] [tolling] received TPM does not have a type");
        return;
    }

    TollingType_t* type_rx = tpm_rx->tpm->tollingType;

    uint64_t client_id, nonce, info_id;
    int confirmation_code;

    switch (type_rx->present) {
        case TollingType_PR_entry:
            if (type_rx->choice.entry.present != TollingEntry_PR_reply) {
                syslog_err("[facilities] [tolling] received TPM.entry is not reply");
                return;
            }
            if (tolling->station.obu.toll_type != TollingType_PR_entry) {
                syslog_err("[facilities] [tolling] received TPM (%d) is not expected toll type (%d)", 
                        TollingType_PR_entry, tolling->station.obu.toll_type); 
            }
            client_id = type_rx->choice.entry.choice.reply.clientId;
            nonce = type_rx->choice.entry.choice.reply.transactionNonce;
            info_id = type_rx->choice.entry.choice.reply.infoId;
            confirmation_code = type_rx->choice.entry.choice.reply.confirmationCode;

            uint8_t b_tep[1024];
            asn_enc_rval_t e_tep = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm_rx, b_tep, 1024);
            if (e_tep.encoded == -1) {
                syslog_err("[facilities] [tolling] error encoding received TPM as entry proof");
                return;
            }
            uper_decode_complete(NULL, &asn_DEF_TPM, (void**) &tolling->station.obu.entry_proof, b_tep, (e_tep.encoded+7)/8);
            break;

        case TollingType_PR_exit:
            if (!type_rx->choice.exit ||
                type_rx->choice.exit->present != TollingExit_PR_reply
                ) {
                syslog_err("[facilities] [tolling] received TPM.exit is not reply");
                return;
            }
            if (tolling->station.obu.toll_type != TollingType_PR_exit) {
                syslog_err("[facilities] [tolling] received TPM (%d) is not expected toll type (%d)", 
                        TollingType_PR_exit, tolling->station.obu.toll_type); 
                return;
            }
            client_id = type_rx->choice.exit->choice.reply.clientId;
            nonce = type_rx->choice.exit->choice.reply.transactionNonce;
            info_id = type_rx->choice.exit->choice.reply.infoId;
            confirmation_code = type_rx->choice.exit->choice.reply.confirmationCode;
            break;

        case TollingType_PR_single:
            if (type_rx->choice.single.present != TollingSingle_PR_reply) {
                syslog_err("[facilities] [tolling] received TPM.single is not reply");
                return;
            }
            if (tolling->station.obu.toll_type != TollingType_PR_single) {
                syslog_err("[facilities] [tolling] received TPM (%d) is not expected toll type (%d)", 
                        TollingType_PR_single, tolling->station.obu.toll_type); 
            }
            client_id = type_rx->choice.single.choice.reply.clientId;
            nonce = type_rx->choice.single.choice.reply.transactionNonce;
            info_id = type_rx->choice.single.choice.reply.infoId;
            confirmation_code = type_rx->choice.single.choice.reply.confirmationCode;
            break;

        default:
            syslog_err("[facilities] [tolling] received TPM has unrecognized type");
            return;
    }

    if (client_id != tolling->station.obu.client_id) {
        syslog_debug("[facilities] [tolling]<- received TPM.reply clientId different from ego");
        return;
    }

    if (nonce != tolling->station.obu.nonce) {
        syslog_err("[facilities] [tolling]<- received TPM.reply nonce different from sent request");
        return;
    }

    SecurityRequest_t* sreq = NULL;
    SecurityReply_t* srep = NULL;

    uint16_t buf_len = 1024;
    uint8_t buf[buf_len];

    // Encode TollingPaymentMessage 
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm_rx->tpm, buf, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name);
        goto cleanup;
    }

    bool accepted = true;
    if (tolling->protocol == TOLLING_PROTOCOL_SIMPLE) {
        // Verify
        sreq = calloc(1, sizeof(SecurityRequest_t));
        sreq->present = SecurityRequest_PR_verify;
        // message
        sreq->choice.verify.message.size = enc.encoded;
        sreq->choice.verify.message.buf = malloc(enc.encoded);
        memcpy(sreq->choice.verify.message.buf, buf, enc.encoded);
        // r
        sreq->choice.verify.r.size = tpm_rx->tpmSignature->r.size;
        sreq->choice.verify.r.buf = malloc(tpm_rx->tpmSignature->r.size);
        memcpy(sreq->choice.verify.r.buf, tpm_rx->tpmSignature->r.buf, tpm_rx->tpmSignature->r.size);
        // s
        sreq->choice.verify.s.size = tpm_rx->tpmSignature->s.size;
        sreq->choice.verify.s.buf = malloc(tpm_rx->tpmSignature->s.size);
        memcpy(sreq->choice.verify.s.buf, tpm_rx->tpmSignature->s.buf, tpm_rx->tpmSignature->s.size);
        // signer
        sreq->choice.verify.signer.size = tpm_rx->tpmSignature->signer.size;
        sreq->choice.verify.signer.buf = malloc(tpm_rx->tpmSignature->signer.size);
        memcpy(sreq->choice.verify.signer.buf, tpm_rx->tpmSignature->signer.buf, tpm_rx->tpmSignature->signer.size);
        // signature type
        sreq->choice.verify.type = tpm_rx->tpmSignature->type;

        buf[0] = 4;
        enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf+1, 2047);
        syslog_debug("[facilities]->[security] SecurityRequest.verify (%ldB)", enc.encoded+1);
        zmq_send(security_socket, buf, enc.encoded+1, 0);
        int32_t rl = zmq_recv(security_socket, buf, buf_len, 0);
        syslog_debug("[facilities]<-[security] SecurityReply.verify (%dB)", rl);

        if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf, rl).code) {
            syslog_err("[facilities] SecurityReply.verify decode failure");
            goto cleanup;
        }

        if (srep->returnCode == SecurityReplyReturnCode_rejected) {
            syslog_err("[facilities] SecurityReply.verify rejected");
            goto cleanup;
        }

        if (srep->data->choice.verify.report != SecurityVerifyConfirmCode_success) {
            syslog_debug("[facilities] signature verify failed");
            goto cleanup;
        }

        accepted = confirmation_code == TollingConfirmationCode_accepted;
    }

    switch (type_rx->present) {
        case TollingType_PR_entry:
            syslog_info("[facilities] [tolling] received entry tolling reply | client:%lld nonce:%ld accepted:%s", (long long) tolling->station.obu.client_id, tolling->station.obu.nonce, accepted ? "yes" : "no");
            break;

        case TollingType_PR_exit:
            syslog_info("[facilities] [tolling] received exit tolling reply | client:%lld nonce:%ld accepted:%s", (long long) tolling->station.obu.client_id, tolling->station.obu.nonce, accepted ? "yes" : "no");
            tolling->station.obu.toll_type = -1;
            tolling->station.obu.active = false;
            ASN_STRUCT_FREE(asn_DEF_TPM, tolling->station.obu.entry_proof);
            tolling->station.obu.entry_proof = NULL;
            break;

        case TollingType_PR_single:
            syslog_info("[facilities] [tolling] received single tolling reply | client:%lld nonce:%ld accepted:%s", (long long) tolling->station.obu.client_id, tolling->station.obu.nonce, accepted ? "yes" : "no");
            tolling->station.obu.toll_type = -1;
            tolling->station.obu.active = false;
            break;

        default:
            break;
    }


cleanup:
    ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
    ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
}

int tpm_recv(void* fc, TPM_t* tpm_rx, void* security_socket, uint8_t* neighbour, uint8_t* src_addr) {

    facilities_t* facilities = (facilities_t*) fc;
    tolling_t* tolling = (tolling_t*) &facilities->tolling;

    if (!tolling->enabled) {
        syslog_debug("[facilities] [tolling] tolling is disabled");
        return 0;
    }

    if (!tpm_rx->tpm) {
        syslog_err("[facilities] [tolling] received TPM does not have substructure TollingPaymentMessage");
        return 1;
    }

    if (!tpm_rx->tpm->tollingType) {
        syslog_err("[facilities] [tolling] received TPM does not have a type");
        return 1;
    }

    switch (tpm_rx->tpm->tollingType->present) {
        // Entry
        case TollingType_PR_entry:
            switch (tpm_rx->tpm->tollingType->choice.entry.present) {
                case TollingSingle_PR_request:
                    if (facilities->station_type != 15) {
                        syslog_debug("[facilities] [tolling] received TPM.entry.request, ignoring");
                        return 0;
                    }
                    rsu_handle_recv(facilities, tpm_rx, security_socket, neighbour, src_addr);
                    break;
                case TollingSingle_PR_reply:
                    if (facilities->station_type == 15) {
                        syslog_debug("[facilities] [tolling] received TPM.entry.reply, ignoring");
                        return 0;
                    }
                    pthread_mutex_lock(&facilities->epv.time.lock);
                    syslog_info("[facilities] [tolling] entry.reply took %ld us", it2s_tender_get_now(TIME_MICROSECONDS) - tolling->tz);
                    pthread_mutex_unlock(&facilities->epv.time.lock);
                    veh_handle_recv(tolling, tpm_rx, security_socket, neighbour);
                    break;
                default:
                    break;
            }
            break;
        // Exit
        case TollingType_PR_exit:
            if (!tpm_rx->tpm->tollingType->choice.exit) {
                return 1;
            }
            switch (tpm_rx->tpm->tollingType->choice.exit->present) {
                case TollingSingle_PR_request:
                    if (facilities->station_type != 15) {
                        syslog_debug("[facilities] [tolling] received TPM.exit.request, ignoring");
                        return 0;
                    }
                    rsu_handle_recv(facilities, tpm_rx, security_socket, neighbour, src_addr);
                    break;
                case TollingSingle_PR_reply:
                    if (facilities->station_type == 15) {
                        syslog_debug("[facilities] [tolling] received TPM.exit.reply, ignoring");
                        return 0;
                    }
                    pthread_mutex_lock(&facilities->epv.time.lock);
                    syslog_info("[facilities] [tolling] exit.reply took %ld us", it2s_tender_get_now(TIME_MICROSECONDS) - tolling->tz);
                    pthread_mutex_unlock(&facilities->epv.time.lock);
                    veh_handle_recv(tolling, tpm_rx, security_socket, neighbour);
                    break;
                default:
                    break;
            }
            break;
        // Single
        case TollingType_PR_single:
            switch (tpm_rx->tpm->tollingType->choice.single.present) {
                case TollingSingle_PR_request:
                    if (facilities->station_type != 15) {
                        syslog_debug("[facilities] [tolling] received TPM.single.request, ignoring");
                        return 0;
                    }
                    rsu_handle_recv(facilities, tpm_rx, security_socket, neighbour, src_addr);
                    break;
                case TollingSingle_PR_reply:
                    if (facilities->station_type == 15) {
                        syslog_debug("[facilities] [tolling] received TPM.single.reply, ignoring");
                        return 0;
                    }
                    pthread_mutex_lock(&facilities->epv.time.lock);
                    syslog_info("[facilities] [tolling] single.reply took %ld us", it2s_tender_get_now(TIME_MICROSECONDS) - tolling->tz);
                    pthread_mutex_unlock(&facilities->epv.time.lock);
                    veh_handle_recv(tolling, tpm_rx, security_socket, neighbour);
                    break;
                default:
                    break;
            }
            break;

        default:
            break;
    }

    return 0;
}

int tolling_init(tolling_t* tolling, void* zmq_ctx, char* security_address) {
    return 0;
}

tolling_info_t* tolling_info_new(it2s_tender_epv_t* epv, TollingPaymentInfo_t* tpi) {
    tolling_info_t* ti = calloc(1, sizeof(tolling_info_t));
    
    ti->timestamp = it2s_tender_get_clock(epv);
    ti->asn = tpi;

    ti->zone.polygon_len = tpi->zone.list.count;
    ti->zone.polygon = malloc(tpi->zone.list.count * sizeof(double[2]));

    for (int i = 0; i < tpi->zone.list.count; ++i) {
        ti->zone.polygon[i][0] = tpi->zone.list.array[i]->latitude / 1.0e7;
        ti->zone.polygon[i][1] = tpi->zone.list.array[i]->longitude / 1.0e7;
    }

    return ti;
}

void tolling_info_free(tolling_info_t* ti) {
    ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, ti->asn);
    free(ti->zone.polygon);
    free(ti);
}