#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/space.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
#include <it2s-asn/etsi-its-v1/tpm/EI1_TollingPaymentInfo.h>
#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
#include <zmq.h>


SAEM_CODE_R saem_check(EI1_SAEM_t* saem, uint8_t* neighbour) {

    int rv = 0;

    bulletin_t* bulletin = &facilities.bulletin;

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

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

    if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) {
        return SAEM_MAX_ANNOUNCEMENTS_REACHED;
    }

    pthread_mutex_lock(&bulletin->lock);

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

            uint16_t its_aid = si->serviceID;

            bool new_announcement = false;

            if (si->chOptions.extensions) {

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

                        case EI1_ServiceInfoExt_PR_applicationDataSAM:
                            bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size;
                            bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size);
                            memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data, 
                                    si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, 
                                    si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
                                  );

                            switch (its_aid) {
                                case SAID_ETC:
                                    if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) {
                                        EI1_TollingPaymentInfo_t* tpi = NULL;
                                        asn_dec_rval_t dec = uper_decode_complete(
                                                NULL, 
                                                &asn_DEF_EI1_TollingPaymentInfo, 
                                                (void**) &tpi,
                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, 
                                                si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
                                                );
                                        if (!dec.code) {

                                            bool found = false;
                                            for (int t = 0; t < bulletin->to_consume_len; ++t) {
                                                if (((tolling_info_t*)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) {
                                                    found = true;
                                                    break;
                                                }
                                            }

                                            if (!found) {
                                                facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi);
                                                bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length];
                                                ++facilities.tolling.infos.length;
                                                new_announcement = true;
                                            }
                                        } else {
                                            ASN_STRUCT_FREE(asn_DEF_EI1_TollingPaymentInfo, tpi);
                                        }
                                    }
                                    break;

                                case SAID_EVCSN:
                                    // TODO
                                    break;
                            }
                            break;

                        case EI1_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 EI1_ServiceInfoExt_PR_servicePort:
                            bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort;
                            break;

                        default:
                            break;
                    }
                }
            }

            uint16_t ci_index = si->channelIndex;

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

            if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) {
                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 = itss_time_get();
                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);
                }
                ++bulletin->to_consume_len;
                rv = SAEM_NEW;
            }

        }
    }

    pthread_mutex_unlock(&bulletin->lock);

    return rv == SAEM_NEW ? SAEM_NEW : SAEM_OK;
}

void bulletin_init() {

    bulletin_t* bulletin = &facilities.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));
    }

}

/**
 * @brief Make a SAEM.
 * @param b_saem A large enough buffer.
 * @param b_saem_len The encoded SAEM length.
 * @return 0 on success, 1 otherwise.
 */
static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) {
    int rv = 0;

    // Check tolling advertisements
    if (!facilities.tolling.infos.length /* || facilites.evcsn.infos.length */) {
        return 1;
    }

    asn_enc_rval_t enc;

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

    /* header */
    saem->header.protocolVersion = 1;
    saem->header.messageID = EI1_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(EI1_ServiceInfos_t));
    saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length; // + facilities.evcsn.infos.length;
    saem->sam.body.serviceInfos->list.size = facilities.tolling.infos.length * sizeof(void*); // + facilities.evcsn.infos.length * sizeof(void*);
    saem->sam.body.serviceInfos->list.array = malloc(facilities.tolling.infos.length * sizeof(void*)); // + facilities.evcsn.infos.length * sizeof(void*);

    uint8_t buf[1024];
    int i;
    for (i = 0; i < facilities.tolling.infos.length; ++i) {
        saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(EI1_ServiceInfo_t));
        saem->sam.body.serviceInfos->list.array[i]->serviceID = SAID_ETC;

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

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

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

                exts->list.array[0] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
                exts->list.array[0]->present = EI1_ServiceInfoExt_PR_providerServiceContext;
                
                char ctx_s[] = "tolling:gn";
                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(EI1_ServiceInfoExt_t));
                exts->list.array[1]->present = EI1_ServiceInfoExt_PR_servicePort;
                exts->list.array[1]->choice.servicePort = 7011;

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

                enc = uper_encode_to_buffer(&asn_DEF_EI1_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024);
                if (enc.encoded == -1) {
                    log_error("[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:
            case TOLLING_PROTOCOL_TLS_GN:
            case TOLLING_PROTOCOL_TLS_SHS:
                exts->list.count = 4;
                exts->list.size = 4 * sizeof(void*);
                exts->list.array = malloc(4 * sizeof(void*));

                exts->list.array[0] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
                exts->list.array[0]->present = EI1_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(EI1_ServiceInfoExt_t));
                exts->list.array[1]->present = EI1_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(EI1_ServiceInfoExt_t));
                exts->list.array[2]->present = EI1_ServiceInfoExt_PR_servicePort;
                exts->list.array[2]->choice.servicePort = 7011;

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

                enc = uper_encode_to_buffer(&asn_DEF_EI1_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
                if (enc.encoded == -1) {
                    log_error("[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;
        }
    }

    for (int j = 0; j < 0/* + facilities.evcsn.infos.length */; ++j) {
        saem->sam.body.serviceInfos->list.array[i+j] = calloc(1, sizeof(EI1_ServiceInfo_t));
        saem->sam.body.serviceInfos->list.array[i+j]->serviceID = SAID_EVCSN;
        // TODO
    }

    pthread_mutex_unlock(&facilities.id.lock);

    enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_EI1_SAEM, saem, b_saem, *b_saem_len);
    if (enc.encoded == -1) {
        log_error("[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_EI1_SAEM, saem);
    
    return rv;
}

void *sa_service() {

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

    EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
    nr->present = EIS_NetworkingRequest_PR_packet;
    EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet;

    npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;
    npr->network.choice.gn.trafficClass = 2;
    npr->network.choice.gn.destinationAddress.buf = malloc(6);
    for (int i = 0; i < 6; ++i) {
        npr->network.choice.gn.destinationAddress.buf[i] = 0xff;
    }
    npr->network.choice.gn.destinationAddress.size = 6;
    npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
    npr->network.choice.gn.securityProfile.sign = true;

    npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;
    npr->transport.choice.btp.btpType = EIS_BTPType_btpB;
    npr->transport.choice.btp.destinationPort = EIS_Port_saem;

    bulletin_t* bulletin = &facilities.bulletin;

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

    int linger_ms = 0;
    void* security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);

    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(npr->data.buf, (uint32_t *) &npr->data.size);
            if (!rv) {

                npr->id = itss_id(npr->data.buf, npr->data.size);

                asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer+1, 1023);
                if (enc.encoded == -1) {
                    log_error("encoding TR for SAEM failed");
                    continue;
                } else {
                    itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded+1, ITSS_NETWORKING, npr->id, "NR.packet.btp");
                    // Logging 
                    if (facilities.logging.dbms) {
                        pthread_mutex_lock(&facilities.id.lock);
                        uint32_t station_id = facilities.id.station_id;
                        pthread_mutex_unlock(&facilities.id.lock);
                        itss_db_add(facilities.logging.dbms, station_id, npr->id, true, EI1_messageID_saem, NULL, npr->data.buf, npr->data.size);
                    }
                    if (facilities.logging.recorder) {
                        uint16_t buffer_len = 2048;
                        uint8_t buffer[buffer_len];
                        int e = itss_management_record_packet_sdu(
                                buffer,
                                buffer_len,
                                npr->data.buf,
                                npr->data.size,
                                npr->id,
                                itss_time_get(),
                                ITSS_FACILITIES,
                                true);
                        if (e != -1) {
                            itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, npr->id, "MReq.packet.set");  
                        }
                    }
                }
            }
            mk_saem_n_sleep = 0;
        }
        ++mk_saem_n_sleep;

        int32_t lat, lon;
        itss_space_lock();
        itss_space_get();
        lat = epv.space.latitude;
        lon = epv.space.longitude;
        itss_space_unlock();

        uint64_t now = itss_time_get();

        pthread_mutex_lock(&bulletin->lock);
        for (int a = 0; a < bulletin->to_consume_len; ++a) {

            // Tolling
            if (facilities.tolling.enabled && 
                bulletin->to_consume[a]->its_aid == 1 && 
                (
                    now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS || 
                    ((facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_SPKI || facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_DPKI) && tpm_should_retransmit())
                ) &&
                facilities.station_type != 15) {

                tolling_info_t* info = (tolling_info_t*) bulletin->to_consume[a]->info.internal_p;

                if (!tpm_is_inside_zone(info)) {
                    continue;
                }

                switch (facilities.tolling.protocol.p) {
                    case TOLLING_PROTOCOL_GN_SPKI:
                    case TOLLING_PROTOCOL_GN_DPKI:
                        tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL);
                        ++bulletin->to_consume[a]->n_trigger;
                        bulletin->to_consume[a]->t_trigger = now;
                        break;

                    case TOLLING_PROTOCOL_TLS:
                    case TOLLING_PROTOCOL_TLS_GN:
                    case TOLLING_PROTOCOL_TLS_SHS:
                        tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr);
                        ++bulletin->to_consume[a]->n_trigger;
                        bulletin->to_consume[a]->t_trigger = now;
                        break;

                    default:
                        break;
                }
            }
        }
        pthread_mutex_unlock(&bulletin->lock);

        // Tolling management
        if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS ||
            facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN ||
            facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) {
            tolling_tlsc_mgmt(facilities.tx_queue, &security_socket);
        }

        usleep(sleep_ms*1000);
    }

    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);

    itss_0close(security_socket);


    return NULL;
}