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

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

int tpm_is_inside_zone(void* fc, tolling_info_s* 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, uint8_t* neighbour) {
    int rv = 0;

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

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

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

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

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

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

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

    tpm->tpm.tollingFlow.present = TollingFlow_PR_request;
    tpm->tpm.tollingFlow.choice.request.clientId = tolling->client_id;
    tpm->tpm.tollingFlow.choice.request.paymentMethod.present = TollPaymentMethod_PR_fiat;
    tpm->tpm.tollingFlow.choice.request.paymentMethod.choice.fiat.fiatId = FiatId_eur;
    tpm->tpm.tollingFlow.choice.request.transactionNonce = tolling->nonce;

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

    // 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(tolling->security_socket, buf, enc.encoded+1, 0);
    int32_t rl = zmq_recv(tolling->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->signature.r.size = srep->data->choice.sign.r.size;
    tpm->signature.r.buf = malloc(srep->data->choice.sign.r.size);
    memcpy(tpm->signature.r.buf, srep->data->choice.sign.r.buf,  srep->data->choice.sign.r.size);
    tpm->signature.s.size = srep->data->choice.sign.s.size;
    tpm->signature.s.buf = malloc(srep->data->choice.sign.s.size);
    memcpy(tpm->signature.s.buf, srep->data->choice.sign.s.buf,  srep->data->choice.sign.s.size);
    tpm->signature.signer.size = srep->data->choice.sign.signer.size;
    tpm->signature.signer.buf = malloc(srep->data->choice.sign.signer.size);
    memcpy(tpm->signature.signer.buf, srep->data->choice.sign.signer.buf,  srep->data->choice.sign.signer.size);
    tpm->signature.type = srep->data->choice.sign.type;

    // Encode TPM 
    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);
        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;
    tr->choice.packet.present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

    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, buf, 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;

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

    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);

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

    return rv;
}

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

    TollRequest_t* req = &tpm_rx->tpm.tollingFlow.choice.request;

    syslog_info("[facilities] [tolling] received toll payment > client: %ld (certificate id: %02x%02x%02x) | nonce: %ld", 
            req->clientId, 
            neighbour ? neighbour[5] : 0,
            neighbour ? neighbour[6] : 0,
            neighbour ? neighbour[7] : 0,
            req->transactionNonce
            );

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

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

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

    // 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->signature.r.size;
    sreq->choice.verify.r.buf = malloc(tpm_rx->signature.r.size);
    memcpy(sreq->choice.verify.r.buf, tpm_rx->signature.r.buf, tpm_rx->signature.r.size);
    // s
    sreq->choice.verify.s.size = tpm_rx->signature.s.size;
    sreq->choice.verify.s.buf = malloc(tpm_rx->signature.s.size);
    memcpy(sreq->choice.verify.s.buf, tpm_rx->signature.s.buf, tpm_rx->signature.s.size);
    // signer
    sreq->choice.verify.signer.size = tpm_rx->signature.signer.size;
    sreq->choice.verify.signer.buf = malloc(tpm_rx->signature.signer.size);
    memcpy(sreq->choice.verify.signer.buf, tpm_rx->signature.signer.buf, tpm_rx->signature.signer.size);
    // signature type
    sreq->choice.verify.type = tpm_rx->signature.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(facilities->tolling.security_socket, buf, enc.encoded+1, 0);
    int32_t rl = zmq_recv(facilities->tolling.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);

    asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&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 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;
    }

    // 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(facilities->tolling.security_socket, buf, enc.encoded+1, 0);
    rl = zmq_recv(facilities->tolling.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");
        goto cleanup;
    }

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

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

    // encode TPM 
    enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, buf, 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;

    // [transport] request (TR)
    tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    tr->choice.packet.present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

    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, buf, 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);
    }

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

    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);

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

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

    TollReply_t* rep = &tpm_rx->tpm.tollingFlow.choice.reply;

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

    if (rep->transactionNonce != tolling->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;
    }

    // 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->signature.r.size;
    sreq->choice.verify.r.buf = malloc(tpm_rx->signature.r.size);
    memcpy(sreq->choice.verify.r.buf, tpm_rx->signature.r.buf, tpm_rx->signature.r.size);
    // s
    sreq->choice.verify.s.size = tpm_rx->signature.s.size;
    sreq->choice.verify.s.buf = malloc(tpm_rx->signature.s.size);
    memcpy(sreq->choice.verify.s.buf, tpm_rx->signature.s.buf, tpm_rx->signature.s.size);
    // signer
    sreq->choice.verify.signer.size = tpm_rx->signature.signer.size;
    sreq->choice.verify.signer.buf = malloc(tpm_rx->signature.signer.size);
    memcpy(sreq->choice.verify.signer.buf, tpm_rx->signature.signer.buf, tpm_rx->signature.signer.size);
    // signature type
    sreq->choice.verify.type = tpm_rx->signature.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(tolling->security_socket, buf, enc.encoded+1, 0);
    int32_t rl = zmq_recv(tolling->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;
    }

    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;

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

int tpm_recv(void* fc, TPM_t* tpm_rx, uint8_t* neighbour) {
    int rv = 0;

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

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

    switch (tpm_rx->tpm.tollingFlow.present) {
        case TollingFlow_PR_request:
            if (facilities->station_type != 15) {
                syslog_debug("[facilities] [tolling] received TPM.request, ignoring");
                goto cleanup;
            }
            rsu_handle_recv(facilities, tpm_rx, neighbour);
            break;

        case TollingFlow_PR_reply:
            if (facilities->station_type == 15) {
                syslog_debug("[facilities] [tolling] received TPM.reply, ignoring");
                goto cleanup;
            }
            pthread_mutex_lock(&facilities->epv.time.lock);
            syslog_info("[facilities] [tolling] 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, neighbour);
            break;

        default:
            break;
    }

cleanup:

    return rv;
}

int tolling_init(tolling_s* tolling, void* zmq_ctx, char* security_address) {

    tolling->security_socket = zmq_socket(zmq_ctx, ZMQ_REQ);
    int wait_ms = 1000;
    zmq_setsockopt(tolling->security_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int));
    zmq_connect(tolling->security_socket, security_address);

    return 0;
}

tolling_info_s* tolling_info_new(it2s_tender_epv_t* epv, TollingPaymentInfo_t* tpi) {
    tolling_info_s* ti = calloc(1, sizeof(tolling_info_s));
    
    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_s* ti) {
    ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, ti->asn);
    free(ti->zone.polygon);
    free(ti);
}