#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>


SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem, uint8_t* neighbour) {

    facilities_t* facilities = (facilities_t*) fc;

    int rv = 0;

    if (saem->header.messageID != messageID_saem) {
        return SAEM_INVALID_HEADER_MESSAGE_ID;
    }

    if (saem->header.protocolVersion != 1) {
        return SAEM_INVALID_HEADER_VERSION;
    }

    pthread_mutex_lock(&bulletin->lock);

    if (saem->sam.body.serviceInfos) {
        for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
            ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i];

            uint16_t its_aid = si->serviceID;

            int index = -1;

            for (int a = 0; a < bulletin->to_consume_len; ++a) {
                // TODO allow different services for same itsAid and stationID

                /* Check existence through stationID and itsAid */
                if (saem->header.stationID == bulletin->to_consume[a]->station_id && 
                    its_aid == bulletin->to_consume[a]->its_aid) {
                    index = a;
                    break;
                }
            }

            if (index == -1) {
                rv = SAEM_NEW;
                if (bulletin->to_consume_len + 1 < MAX_ANNOUNCEMENTS_LEN) {
                    bulletin->to_consume[bulletin->to_consume_len]->its_aid = its_aid;
                    bulletin->to_consume[bulletin->to_consume_len]->station_id = saem->header.stationID;
                    bulletin->to_consume[bulletin->to_consume_len]->timestamp = it2s_tender_get_clock(&facilities->epv);

                    if (si->chOptions.extensions) {
                        for (int e = 0; e < si->chOptions.extensions->list.count; ++e) {
                            switch (si->chOptions.extensions->list.array[e]->present) {
                                case ServiceInfoExt_PR_providerServiceContext:
                                    bulletin->to_consume[bulletin->to_provide_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size;
                                    bulletin->to_consume[bulletin->to_provide_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size);
                                    memcpy(bulletin->to_consume[bulletin->to_provide_len]->info.context, 
                                           si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf, 
                                           si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size
                                           );
                                    break;

                                case ServiceInfoExt_PR_applicationDataSAM:
                                    bulletin->to_consume[bulletin->to_provide_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size;
                                    bulletin->to_consume[bulletin->to_provide_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size);
                                    memcpy(bulletin->to_consume[bulletin->to_provide_len]->info.data, 
                                           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) {
                                        TollingPaymentInfo_t* tpi = NULL;
                                        asn_dec_rval_t dec = uper_decode_complete(
                                                NULL, 
                                                &asn_DEF_TollingPaymentInfo, 
                                                (void**) &tpi,
                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, 
                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
                                                );
                                        if (!dec.code) {
                                            facilities->tolling.infos.z[facilities->tolling.infos.length] = tolling_info_new(&facilities->epv, tpi);
                                            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, tpi);
                                        }
                                    }
                                    break;

                                case ServiceInfoExt_PR_addressIPv6:
                                    memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16);
                                    break;

                                case ServiceInfoExt_PR_servicePort:
                                    bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort;
                                    break;

                                default:
                                    break;
                            }
                        }
                    }

                    if (neighbour) {
                        bulletin->to_consume[bulletin->to_consume_len]->certificate_id = malloc(8);
                        memcpy(bulletin->to_consume[bulletin->to_consume_len]->certificate_id, neighbour, 8);
                    }

                    index = bulletin->to_consume_len;
                    ++bulletin->to_consume_len;
                }
            }

            uint16_t ci_index = si->channelIndex;

            // TODO channelInfos
            if (saem->sam.body.channelInfos) {
                if (saem->sam.body.channelInfos->list.count >= ci_index + 1) {
                }
            }

        }
    }

    pthread_mutex_unlock(&bulletin->lock);

    return rv == SAEM_NEW ? SAEM_NEW : SAEM_OK;
}

void bulletin_init(bulletin_t* bulletin) {

    pthread_mutex_init(&bulletin->lock, NULL);

    bulletin->to_consume_len = 0;
    for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) {
        bulletin->to_consume[i] = calloc(1, sizeof(announcement_t));
    }

}

int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
    int rv = 0;

    // Check tolling advertisements
    if (!facilities->tolling.infos.length) {
        return 1;
    }

    asn_enc_rval_t enc;

    SAEM_t* saem = calloc(1, sizeof(SAEM_t));

    /* header */
    saem->header.protocolVersion = 1;
    saem->header.messageID = messageID_saem;

    pthread_mutex_lock(&facilities->id.lock);
    saem->header.stationID = facilities->id.station_id;

    /* sam */
    saem->sam.version = 0;

    saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t));
    saem->sam.body.serviceInfos->list.count = facilities->bulletin.to_provide_len;
    saem->sam.body.serviceInfos->list.size = facilities->bulletin.to_provide_len * sizeof(void*);
    saem->sam.body.serviceInfos->list.array = malloc(facilities->bulletin.to_provide_len * sizeof(void*));

    uint8_t buf[1024];
    for (int i = 0; i < facilities->bulletin.to_provide_len; ++i) {
        saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t));
        saem->sam.body.serviceInfos->list.array[i]->serviceID = facilities->bulletin.to_provide[i]->its_aid;

        saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t));

        ServiceInfoExts_t* exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions;

        switch (facilities->tolling.protocol) {
            case TOLLING_PROTOCOL_SIMPLE:
                exts->list.count = 3;
                exts->list.size = 3 * sizeof(void*);
                exts->list.array = malloc(3 * sizeof(void*));

                exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext;
                
                char ctx_s[] = "tolling:simple";
                exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s);
                exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s));
                memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s));

                exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[1]->present = ServiceInfoExt_PR_servicePort;
                exts->list.array[1]->choice.servicePort = 7011;

                exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM;
                exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024);

                enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024);
                if (enc.encoded == -1) {
                    syslog_err("[facilities] [sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name);
                    rv = 1;
                    goto cleanup;
                }

                exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8;
                break;

            case TOLLING_PROTOCOL_TLS:
                exts->list.count = 4;
                exts->list.size = 4 * sizeof(void*);
                exts->list.array = malloc(4 * sizeof(void*));

                exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext;
                char ctx_t[] = "tolling:tls";
                exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t);
                exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t));
                memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t));

                exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6;
                exts->list.array[1]->choice.addressIPv6.size = 16;
                exts->list.array[1]->choice.addressIPv6.buf = malloc(16);
                memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities->id.ipv6_addr, 16);

                exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[2]->present = ServiceInfoExt_PR_servicePort;
                exts->list.array[2]->choice.servicePort = 7011;

                exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t));
                exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM;
                exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024);

                enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0], exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
                if (enc.encoded == -1) {
                    syslog_err("[facilities] [sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name);
                    rv = 1;
                    goto cleanup;
                }

                exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8;
                break;
        }
    }

    pthread_mutex_unlock(&facilities->id.lock);

    enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [sa] failure to encode SAEM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    *b_saem_len = enc.encoded;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_SAEM, saem);
    
    return rv;
}

void *sa_service(void *fc) {
    facilities_t *facilities = (facilities_t*) fc;

    pthread_mutex_init(&facilities->bulletin.lock, NULL);

    TransportRequest_t *tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    TransportPacketRequest_t* tpr = &tr->choice.packet;

    tpr->present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t *bpr = &tpr->choice.btp;

    bulletin_t* bulletin = &facilities->bulletin;

    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.packetTransportType = PacketTransportType_shb;

    bpr->destinationPort = Port_saem;

    bpr->gn.trafficClass = 2;

    bpr->data.buf = malloc(512);
    bpr->data.size = 512;

    bpr->gn.securityProfile.sign = true;

    void* security_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ);
    zmq_connect(security_socket, facilities->zmq.security_address);

    uint8_t tr_oer[1024];
    tr_oer[0] = 4; // Facilities

    int rv = 0;

    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) {
            rv = mk_saem(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size);
            if (!rv) {
                asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
                if (enc.encoded == -1) {
                    syslog_err("[facilities] encoding TR for SAEM failed");
                    continue;
                } else {
                    queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
                    pthread_cond_signal(&facilities->tx_queue->trigger);
                }
            }
            mk_saem_n_sleep = 0;
        }
        ++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);

        uint64_t now = it2s_tender_get_clock(&facilities->epv);

        pthread_mutex_lock(&bulletin->lock);
        for (int a = 0; a < bulletin->to_consume_len; ++a) {
            /* do some checks, e.g.
             * is advertised service close by?
             * do we want to enjoy the advertised service? */

            // Tolling
            if (facilities->tolling.enabled && 
                bulletin->to_consume[a]->its_aid == 0 && 
                now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS &&
                facilities->station_type != 15) {

                if (!tpm_is_inside_zone(facilities, (tolling_info_s*) bulletin->to_consume[a]->info.internal_p)) {
                    continue;
                }

                switch (facilities->tolling.protocol) {
                    case TOLLING_PROTOCOL_SIMPLE:
                        tpm_pay(facilities, bulletin->to_consume[a]->certificate_id);
                        ++bulletin->to_consume[a]->n_trigger;
                        bulletin->to_consume[a]->t_trigger = now;
                        break;

                    case TOLLING_PROTOCOL_TLS:;
                        SecurityRequest_t* sreq = calloc(1, sizeof(SecurityRequest_t));
                        sreq->present = SecurityRequest_PR_tlsSend;
                        sreq->choice.tlsSend.data.buf = malloc(7);
                        sreq->choice.tlsSend.data.size = 7;
                        char hello[] = "Hello!";
                        memcpy(sreq->choice.tlsSend.data.buf, hello, 7);

                        uint8_t buffer[1024];
                        buffer[0] = 4;
                        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buffer+1, 1023);
                        ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
                        
                        syslog_debug("[facilities] [sa]-> SecurityRequest.tlsSend ->[security]");
                        zmq_send(security_socket, buffer, enc.encoded, 0);
                        zmq_recv(security_socket, buffer, 1024, 0);
                        syslog_debug("[facilities] [sa]<- SecurityReply <-[security]");

                        SecurityReply_t* srep = NULL;
                        asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buffer, 1024);
                        
                        if (dec.code || srep->returnCode != SecurityReplyReturnCode_accepted) {
                            ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
                            continue; 
                        }

                        TransportRequest_t* tr_etc = calloc(1, sizeof(TransportRequest_t));
                        tr_etc->present = TransportRequest_PR_packet;
                        TransportPacketRequest_t* tpr_etc = &tr_etc->choice.packet;
                        tpr_etc->present = TransportPacketRequest_PR_tcp;

                        tpr_etc->choice.tcp.destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
                        tpr_etc->choice.tcp.destinationAddress->buf = malloc(16);
                        tpr_etc->choice.tcp.destinationAddress->size = 16;
                        memcpy(tpr_etc->choice.tcp.destinationAddress->buf, bulletin->to_consume[a]->endpoint.ipv6_addr, 16);


                        tpr_etc->choice.tcp.destinationPort = bulletin->to_consume[a]->endpoint.port;
                        tpr_etc->choice.tcp.sourcePort = 7011;

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

                        ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);

                        tpr_etc->choice.tcp.gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
                        tpr_etc->choice.tcp.gn->packetTransportType = PacketTransportType_shb;
                        tpr_etc->choice.tcp.gn->destinationAddress.buf = calloc(1, 6);
                        tpr_etc->choice.tcp.gn->destinationAddress.size = 6;

                        ++bulletin->to_consume[a]->n_trigger;

                        enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr_etc, tr_oer+1, 1023);
                        if (enc.encoded == -1) {
                            syslog_err("[facilities] encoding TR for ETC-Req failed");
                            continue;
                        } else {
                            queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
                            pthread_cond_signal(&facilities->tx_queue->trigger);
                        }

                        ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr_etc);
                        break;
                }
            }
        }
        pthread_mutex_unlock(&bulletin->lock);

        usleep(sleep_ms*1000);
    }

    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);


    return NULL;
}