#include "requests.h"

#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.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-tender/packet.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
#include <zmq.h>

#include "cam.h"
#include "cpm.h"
#include "denm.h"
#include "evm.h"
#include "facilities.h"
#include "infrastructure.h"
#include "tpm.h"

static void fwd_to_apps(uint8_t* msg, uint16_t msg_len, int its_msg_type, uint32_t iid) {
    EIS_FacilitiesIndication_t* fi = NULL;

    fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
    fi->present = EIS_FacilitiesIndication_PR_message;
    EIS_FacilitiesMessageIndication_t *fmi = &fi->choice.message;

    fmi->id = iid;

    fmi->itsMessageType = its_msg_type;

    fmi->data.size = msg_len;
    fmi->data.buf = malloc(msg_len);
    memcpy(fmi->data.buf, msg, msg_len);

    uint8_t buffer[ITSS_SDU_MAX_LEN];
    buffer[0] = 4;  // Facilities
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1);

    log_debug("-> FI.message ->[applications] | id:%08x size:%dB",
            (uint32_t)iid, enc.encoded+1);
    uint8_t code;
    itss_0send(facilities.apps_socket, buffer, enc.encoded+1);
    int rv = itss_0recv_rt(&facilities.apps_socket, &code, 1, buffer, enc.encoded+1, 1000);
    if (rv == -1) {
        log_error("-> FI.message ->[applications] | id:%08x size:%dB <TIMEOUT>",
                (uint32_t)iid, enc.encoded+1);
    }

    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
}

int facilities_request_result_accepted(void *responder) {
    int rv = 0;

    EIS_FacilitiesReply_t *fr = calloc(1, sizeof(EIS_FacilitiesReply_t));
    fr->present = EIS_FacilitiesReply_PR_message;
    fr->choice.message.returnCode = 0;
    uint8_t fr_oer[32];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, fr, fr_oer, 32);
    itss_0send(responder, fr_oer, enc.encoded);

    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, fr);
    return rv;
}

int facilities_request_result_rejected(void *responder) {
    int rv = 0;

    EIS_FacilitiesReply_t *fr = calloc(1, sizeof(EIS_FacilitiesReply_t));
    fr->present = EIS_FacilitiesReply_PR_message;
    fr->choice.message.returnCode = 1;
    uint8_t fr_oer[32];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, fr, fr_oer, 32);
    itss_0send(responder, fr_oer, enc.encoded);

    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, fr);
    return rv;
}

int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequest_t *frm) {
    int rv = 0;

    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->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;

    void *its_msg = NULL;
    asn_TYPE_descriptor_t *its_msg_def = NULL;

    bool fwd = true;
    bool process_msg = true;

    uint64_t transmission_duration = 0;
    uint32_t transmission_interval = 0;
    uint64_t transmission_start = 0;

    switch (frm->itsMessageType) {
        case EIS_ItsMessageType_cam:
            its_msg_def = &asn_DEF_EI1_CAM;
            its_msg = calloc(1, sizeof(EI1_CAM_t));
            npr->transport.choice.btp.destinationPort = EIS_Port_cam;
            npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
            npr->network.choice.gn.trafficClass = 1;

            break;
        case EIS_ItsMessageType_denm:
            its_msg_def = &asn_DEF_EI1_DENM;
            its_msg = calloc(1, sizeof(EI1_DENM_t));
            npr->transport.choice.btp.destinationPort = EIS_Port_denm;
            npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_gbc;
            npr->network.choice.gn.trafficClass = 2;

            break;
        case EIS_ItsMessageType_ivim:
            its_msg_def = &asn_DEF_EI1_IVIM;
            its_msg = calloc(1, sizeof(EI1_IVIM_t));
            npr->transport.choice.btp.destinationPort = EIS_Port_ivim;
            npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
            npr->network.choice.gn.trafficClass = 1;
            break;

        case EIS_ItsMessageType_cpm:
            its_msg_def = &asn_DEF_EI1_CPM;
            its_msg = calloc(1, sizeof(EI1_CPM_t));
            npr->transport.choice.btp.destinationPort = EIS_Port_cpm;
            npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
            npr->network.choice.gn.trafficClass = 2;
            break;

        case EIS_ItsMessageType_evrsr:
            its_msg_def = &asn_DEF_EI1_EV_RSR;
            its_msg = calloc(1, sizeof(EI1_EV_RSR_t));
            npr->transport.choice.btp.destinationPort = EIS_Port_evrsr;
            npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
            npr->network.choice.gn.trafficClass = 1;
            break;

        default:
            if (!facilities.upf) {
                log_error("unrecognized FR message type (%lld)", frm->itsMessageType);
                facilities_request_result_rejected(responder);
                rv = 1;
                goto cleanup;
            } else {
                process_msg = false;
                npr->transport.choice.btp.destinationPort = 7120;
                npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
                npr->network.choice.gn.trafficClass = 1;
            }
    }

    uint64_t id = 0;
    if (!frm->id) {
        id = itss_id(frm->data.buf, frm->data.size);
    } else if (*frm->id == 0) {
        id = itss_id(frm->data.buf, frm->data.size);
    }

    bool is_update = false;
    int managed_msg = false;

    if (process_msg) {
        asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void **)&its_msg, frm->data.buf, frm->data.size);
        if (dec.code) {
            log_warn("invalid FR %s received", its_msg_def->name);
            facilities_request_result_rejected(responder);
            rv = 1;
            goto cleanup;
        }

        switch (frm->itsMessageType) {
        case EIS_ItsMessageType_denm: 
            managed_msg = true;
            uint8_t event_type = event_manage(its_msg, &id, NULL, 0);
            // Do not free its_msg! event_manage takes care of the msg
            // id will get set to another val if EVENT NEW or UPDATE or CANCELLATION or NEGATION
            if (event_type != EVENT_NEW &&
                    event_type != EVENT_UPDATE &&
                    event_type != EVENT_CANCELLATION &&
                    event_type != EVENT_NEGATION) {
                fwd = false;
            }

            if (event_type == EVENT_UPDATE ||
                    event_type == EVENT_CANCELLATION ||
                    event_type == EVENT_NEGATION) {
                is_update = true;
            }

            if (fwd) {
                // set stationID
                pthread_mutex_lock(&facilities.id.lock);
                ((EI1_DENM_t *)its_msg)->header.stationID = facilities.id.station_id;
                pthread_mutex_unlock(&facilities.id.lock);

                // Set only one trace
                if (facilities.station_type != 15) {
                    pthread_mutex_lock(&facilities.lightship.lock);
                    if (facilities.lightship.path_history_len > 0) {
                        if (!((EI1_DENM_t *)its_msg)->denm.location) {
                            ((EI1_DENM_t *)its_msg)->denm.location = calloc(1, sizeof(EI1_LocationContainer_t));
                        }
                        ((EI1_DENM_t *)its_msg)->denm.location->traces.list.count = 1;
                        ((EI1_DENM_t *)its_msg)->denm.location->traces.list.size = 1 * sizeof(void *);
                        ((EI1_DENM_t *)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void *));
                        ((EI1_DENM_t *)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(EI1_PathHistory_t));
                        EI1_PathHistory_t *ph = ((EI1_DENM_t *)its_msg)->denm.location->traces.list.array[0];

                        pos_point_t **path_history = facilities.lightship.path_history;
                        uint16_t path_history_len = facilities.lightship.path_history_len;

                        ph->list.array = malloc((path_history_len) * sizeof(void *));
                        ph->list.count = path_history_len;
                        ph->list.size = (path_history_len) * sizeof(void *);

                        ph->list.array[0] = calloc(1, sizeof(EI1_PathPoint_t));

                        itss_space_lock();
                        if (path_history[0]->alt != EI1_AltitudeValue_unavailable && epv.space.altitude != EI1_AltitudeValue_unavailable) {
                            ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.altitude;
                        } else {
                            ph->list.array[0]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
                        }

                        if (path_history[0]->lat != EI1_Latitude_unavailable && epv.space.latitude != EI1_Latitude_unavailable) {
                            ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.latitude;
                        } else {
                            ph->list.array[0]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
                        }

                        if (path_history[0]->lon != EI1_Longitude_unavailable && epv.space.longitude != EI1_Longitude_unavailable) {
                            ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.longitude;
                        } else {
                            ph->list.array[0]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
                        }
                        itss_space_unlock();

                        ph->list.array[0]->pathDeltaTime = calloc(1, sizeof(EI1_PathDeltaTime_t));
                        *ph->list.array[0]->pathDeltaTime = (itss_time_get() - path_history[0]->ts) / 10;

                        for (int i = 1; i < path_history_len; ++i) {
                            ph->list.array[i] = calloc(1, sizeof(EI1_PathPoint_t));

                            if (path_history[i]->alt != EI1_AltitudeValue_unavailable && path_history[i - 1]->alt != EI1_AltitudeValue_unavailable) {
                                ph->list.array[i]->pathPosition.deltaAltitude = path_history[i]->alt - path_history[i - 1]->alt;
                            } else {
                                ph->list.array[i]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
                            }

                            if (path_history[i]->lat != EI1_Latitude_unavailable && path_history[i - 1]->lat != EI1_Latitude_unavailable) {
                                ph->list.array[i]->pathPosition.deltaLatitude = path_history[i]->lat - path_history[i - 1]->lat;
                            } else {
                                ph->list.array[i]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
                            }

                            if (path_history[i]->lon != EI1_Longitude_unavailable && path_history[i - 1]->lon != EI1_Longitude_unavailable) {
                                ph->list.array[i]->pathPosition.deltaLongitude = path_history[i]->lon - path_history[i - 1]->lon;
                            } else {
                                ph->list.array[i]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
                            }

                            ph->list.array[i]->pathDeltaTime = calloc(1, sizeof(EI1_PathDeltaTime_t));
                            *ph->list.array[i]->pathDeltaTime = (path_history[i - 1]->ts - path_history[i]->ts) / 10;
                        }
                    }
                    pthread_mutex_unlock(&facilities.lightship.lock);
                }

                // get, set retransmission, duration
                if (((EI1_DENM_t *)its_msg)->denm.management.transmissionInterval) {
                    transmission_interval = *((uint32_t *)((EI1_DENM_t *)its_msg)->denm.management.transmissionInterval);

                    if (((EI1_DENM_t *)its_msg)->denm.management.validityDuration) {
                        transmission_duration = *((uint32_t *)((EI1_DENM_t *)its_msg)->denm.management.validityDuration) * 1000;
                    } else {
                        transmission_duration = 600 * 1000;
                    }
                }
            }
            break;
        case EIS_ItsMessageType_ivim:
            managed_msg = true;
            uint8_t service_type = service_eval(SERVICE_IVI, its_msg, &id, NULL, 0);

            if (service_type != SERVICE_NEW &&
                    service_type != SERVICE_UPDATE &&
                    service_type != SERVICE_CANCELLATION &&
                    service_type != SERVICE_NEGATION) {
                fwd = false;
            }

            if (service_type == SERVICE_UPDATE ||
                    service_type == SERVICE_CANCELLATION ||
                    service_type == SERVICE_NEGATION) {
                is_update = true;
            }

            if (fwd) {
                uint64_t valid_to, valid_from;

                if (!((EI1_IVIM_t *)its_msg)->ivi.mandatory.validFrom) {
                    valid_from = itss_time_get();
                } else {
                    asn_INTEGER2ulong((INTEGER_t *)((EI1_IVIM_t *)its_msg)->ivi.mandatory.validFrom, (unsigned long long *)&valid_from);
                }

                if (!((EI1_IVIM_t *)its_msg)->ivi.mandatory.validTo) {
                    valid_to = valid_from + facilities.infrastructure.default_service_duration;
                } else {
                    asn_INTEGER2ulong((INTEGER_t *)((EI1_IVIM_t *)its_msg)->ivi.mandatory.validTo, (unsigned long long *)&valid_to);
                }

                transmission_start = valid_from;
                transmission_interval = facilities.infrastructure.replay_interval;
                transmission_duration = valid_to - valid_from;
            }
            break;
        default:
            break;
        }
        if (!facilities.replay) {
            transmission_interval = 0;
            transmission_duration = 0;
        }
    }

    // Respond to [applications]
    facilities_request_result_accepted(responder);

    // Forward message to [networking]
    if (fwd) {
        npr->transport.choice.btp.btpType = EIS_BTPType_btpB;

        npr->id = id;
        npr->data.buf = malloc(2048);

        asn_enc_rval_t enc;
        if (process_msg) {
            enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, npr->data.buf, 2048);
            if (enc.encoded == -1) {
                log_error("failed encoding ITS message into UPER (%s)", enc.failed_type->name);
                rv = 1;
                goto cleanup;
            }
            npr->data.size = (enc.encoded + 7) / 8;
        } else {
            memcpy(npr->data.buf, frm->data.buf, frm->data.size);
            npr->data.size = frm->data.size;
        }

        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;

        if (transmission_start) {
            npr->network.choice.gn.repetitionStart = malloc(sizeof(long));
            *npr->network.choice.gn.repetitionStart = transmission_start;
        }

        if (transmission_interval) {
            npr->network.choice.gn.repetitionInterval = malloc(sizeof(long));
            *npr->network.choice.gn.repetitionInterval = transmission_interval;
        }

        if (transmission_duration) {
            npr->network.choice.gn.maximumRepetitionTime = malloc(sizeof(long));
            *npr->network.choice.gn.maximumRepetitionTime = transmission_duration;
        }

        if (is_update) {
            npr->network.choice.gn.isUpdate = malloc(sizeof(long));
            *npr->network.choice.gn.isUpdate = 1;
        }

        npr->network.choice.gn.securityProfile.sign = true;

        uint8_t tr_oer[2048];
        tr_oer[0] = 4;  // [facilities] service id
        enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer + 1, 2047);
        if (enc.encoded == -1) {
            log_error("failed encoding TR (%s)", enc.failed_type->name);
            rv = 1;
            goto cleanup;
        }

        itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_NETWORKING, id, "NR.packet.btp");
    }

    // Logging
    if (facilities.logging.recorder) {
        uint16_t buffer_len = 2048;
        uint8_t buffer[buffer_len];
        int e = itss_management_record_packet_sdu(
            buffer,
            buffer_len,
            frm->data.buf,
            frm->data.size,
            id,
            itss_time_get(),
            ITSS_FACILITIES,
            true);
        if (e != -1) {
            itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set");
        }
    }

cleanup:
    if (its_msg_def && !managed_msg)
        ASN_STRUCT_FREE(*its_msg_def, its_msg);
    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);

    return rv;
}

int facilities_request_active_episodes(void *responder, EIS_FacilitiesRequest_t *freq) {
    int rv = 0;

    EIS_FacilitiesReply_t *frep = calloc(1, sizeof(EIS_FacilitiesReply_t));
    uint8_t *frep_oer = NULL;

    frep->present = EIS_FacilitiesReply_PR_data;
    frep->choice.data.present = EIS_FacilitiesDataReply_PR_episodes;

    pthread_mutex_lock(&facilities.den.lock);
    pthread_mutex_lock(&facilities.infrastructure.lock);

    uint16_t nae = facilities.den.n_active_events;
    uint16_t nas = facilities.infrastructure.n_active_services;

    uint16_t na = 0;

    for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) {
        switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) {
            case EIS_EpisodeType_denm:
                na += nae;
                break;
            case EIS_EpisodeType_ivim:
                na += nas;
                break;
        }
    }

    frep->choice.data.choice.episodes.list.count = na;
    frep->choice.data.choice.episodes.list.size = na * sizeof(void *);
    frep->choice.data.choice.episodes.list.array = malloc(na * sizeof(void *));

    for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) {
        switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) {
            case EIS_EpisodeType_denm:
                for (int i = 0, n = 0; n < nae; ++i) {
                    if (facilities.den.events[i]->enabled && facilities.den.events[i]->state == EVENT_ACTIVE) {
                        frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(EIS_ItsMessage_t));
                        frep->choice.data.choice.episodes.list.array[j]->itsMessageType = EIS_ItsMessageType_denm;
                        frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048);
                        asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI1_DENM, NULL, facilities.den.events[i]->denm, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048);
                        if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */
                            log_error("failed encoding DENM for FDResult (%s)", enc.failed_type->name);
                            frep->choice.data.choice.episodes.list.array[j]->data.size = 0;
                            continue;
                        }
                        frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8;
                        ++j;
                        ++n;
                    }
                }
                break;
            case EIS_EpisodeType_ivim:
                for (int i = 0, n = 0; n < nas; ++i) {
                    if (facilities.infrastructure.services[i]->enabled && facilities.infrastructure.services[i]->state == SERVICE_ACTIVE) {
                        frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(EIS_ItsMessage_t));
                        frep->choice.data.choice.episodes.list.array[j]->itsMessageType = EIS_ItsMessageType_ivim;
                        frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048);
                        asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI1_IVIM, NULL, facilities.infrastructure.services[i]->ivim, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048);
                        if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */
                            log_error("failed encoding IVIM for FDResult (%s)", enc.failed_type->name);
                            frep->choice.data.choice.episodes.list.array[j]->data.size = 0;
                            continue;
                        }
                        frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8;
                        ++j;
                        ++n;
                    }
                }
                break;
            default:
                log_error("unrecognized FR event type (%lld)", *freq->choice.data.choice.activeEpisodes.list.array[e]);
                pthread_mutex_unlock(&facilities.den.lock);
                pthread_mutex_unlock(&facilities.infrastructure.lock);

                facilities_request_result_rejected(responder);
                rv = 1;
                goto cleanup;
        }
    }

    pthread_mutex_unlock(&facilities.den.lock);
    pthread_mutex_unlock(&facilities.infrastructure.lock);

    frep_oer = malloc(32768);
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, frep, frep_oer, 32768);
    if (enc.encoded == -1) {
        log_error("failed encoding FReply (%s)", enc.failed_type->name);

        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    itss_0send(responder, frep_oer, enc.encoded);

cleanup:
    free(frep_oer);
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, frep);

    return rv;
}

int facilities_request_attribute_types(void *responder, EIS_FacilitiesRequest_t *freq) {
    int rv = 0;

    EIS_FacilitiesReply_t *frep = calloc(1, sizeof(EIS_FacilitiesReply_t));
    frep->present = EIS_FacilitiesReply_PR_data;

    int nra = freq->choice.data.choice.attributeTypes.list.count;
    frep->choice.data.present = EIS_FacilitiesDataReply_PR_attributes;
    frep->choice.data.choice.attributes.list.count = nra;
    frep->choice.data.choice.attributes.list.size = sizeof(void *) * nra;
    frep->choice.data.choice.attributes.list.array = malloc(sizeof(void *) * nra);
    for (int j = 0; j < nra; ++j) {
        switch (*freq->choice.data.choice.attributeTypes.list.array[j]) {
            case EIS_FacilitiesAttributeType_stationId:
                frep->choice.data.choice.attributes.list.array[j] = calloc(1, sizeof(EIS_FacilitiesAttribute_t));
                frep->choice.data.choice.attributes.list.array[j]->data.size = 8;
                frep->choice.data.choice.attributes.list.array[j]->data.buf = malloc(8);
                pthread_mutex_lock(&facilities.id.lock);
                *((uint64_t *)frep->choice.data.choice.attributes.list.array[j]->data.buf) = facilities.id.station_id;
                pthread_mutex_unlock(&facilities.id.lock);
                break;

            default:
                log_debug("unrecognized FR attribute type request");
                facilities_request_result_rejected(responder);
                rv = 1;
                goto cleanup;
        }
    }
    uint8_t frep_oer[256];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, frep, frep_oer, 256);
    if (enc.encoded == -1) {
        log_error("failed encoding FReply (%s)", enc.failed_type->name);
        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    itss_0send(responder, frep_oer, enc.encoded);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, frep);

    return rv;
}

int facilities_request_loaded_protected_zones(void *responder, EIS_FacilitiesRequest_t *freq) {
    int rv = 0;

    EIS_FacilitiesReply_t *frep = calloc(1, sizeof(EIS_FacilitiesReply_t));

    frep->present = EIS_FacilitiesReply_PR_data;
    frep->choice.data.present = EIS_FacilitiesDataReply_PR_protectedCommunicationZones;

    pthread_mutex_lock(&facilities.lightship.lock);

    frep->choice.data.choice.protectedCommunicationZones.list.count = facilities.lightship.protected_zones.pz_len;
    frep->choice.data.choice.protectedCommunicationZones.list.size = facilities.lightship.protected_zones.pz_len * sizeof(void *);
    frep->choice.data.choice.protectedCommunicationZones.list.array = malloc(facilities.lightship.protected_zones.pz_len * sizeof(void *));

    uint8_t buf[256];
    for (int z = 0; z < facilities.lightship.protected_zones.pz_len; ++z) {
        frep->choice.data.choice.protectedCommunicationZones.list.array[z] = calloc(1, sizeof(EIS_ProtectedCommunicationZone_t));
        asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EIS_ProtectedCommunicationZone, NULL, facilities.lightship.protected_zones.pz[z], buf, 256);
        uper_decode_complete(NULL, &asn_DEF_EIS_ProtectedCommunicationZone, (void **)&frep->choice.data.choice.protectedCommunicationZones.list.array[z], buf, (enc.encoded + 7) / 8);
    }

    pthread_mutex_unlock(&facilities.lightship.lock);

    uint8_t frep_oer[1024];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, frep, frep_oer, 1024);
    if (enc.encoded == -1) {
        log_error("failed encoding FDResult (%s)", enc.failed_type->name);
        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    itss_0send(responder, frep_oer, enc.encoded);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, frep);

    return rv;
}

int facilities_request_chaininfo_set(void *responder, EIS_ChainInformationSet_t *cis) {
    int rv = 0;

    pthread_mutex_lock(&facilities.coordination.lock);
    facilities.coordination.chain.id = cis->idSet;
    for (int i = 0; i < cis->linkSet.list.count; ++i) {
        memcpy(
            facilities.coordination.chain.links[i],
            cis->linkSet.list.array[i]->buf,
            cis->linkSet.list.array[i]->size);
    }
    facilities.coordination.chain.n_links = cis->linkSet.list.count;
    // TODO region
    pthread_mutex_unlock(&facilities.coordination.lock);

    EIS_FacilitiesReply_t *frep = calloc(1, sizeof(EIS_FacilitiesReply_t));
    frep->present = EIS_FacilitiesReply_PR_data;
    frep->choice.data.present = EIS_FacilitiesDataReply_PR_chainInfoSet;
    frep->choice.data.choice.chainInfoSet = EIS_FacilitiesResultCode_accepted;

    uint8_t frep_oer[1024];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesReply, NULL, frep, frep_oer, 1024);
    if (enc.encoded == -1) {
        log_error("failed encoding FDResult (%s)", enc.failed_type->name);
        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    itss_0send(responder, frep_oer, enc.encoded);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesReply, frep);

    return rv;
}

static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* npi, void** security_socket) {
    int rv = 0;

    EIS_BTPPacketIndication_t* bpi = &npi->transport.choice.btp;

    EIS_FacilitiesIndication_t *fi = NULL;

    asn_TYPE_descriptor_t *its_msg_descriptor = NULL;
    void *its_msg = NULL;
    int its_msg_type = 0;

    bool fwd = false;
    bool stored = false;
    bool process_msg = true;

    uint16_t buf_len = 2048;
    uint8_t buf[buf_len];

    uint64_t id = 0;

    // Parse message
    switch (bpi->destinationPort) {
        case EIS_Port_cam:
            its_msg_descriptor = &asn_DEF_EI1_CAM;
            its_msg = calloc(1, sizeof(EI1_CAM_t));
            its_msg_type = EI1_messageID_cam;
            break;

        case EIS_Port_denm:
            its_msg_descriptor = &asn_DEF_EI1_DENM;
            its_msg = calloc(1, sizeof(EI1_DENM_t));
            its_msg_type = EI1_messageID_denm;
            break;

        case EIS_Port_ivim:
            its_msg_descriptor = &asn_DEF_EI1_IVIM;
            its_msg = calloc(1, sizeof(EI1_IVIM_t));
            its_msg_type = EI1_messageID_ivim;
            break;

        case EIS_Port_cpm:
            its_msg_descriptor = &asn_DEF_EI1_CPM;
            its_msg = calloc(1, sizeof(EI1_CPM_t));
            its_msg_type = 14;
            break;

        case EIS_Port_saem:
            its_msg_descriptor = &asn_DEF_EI1_SAEM;
            its_msg = calloc(1, sizeof(EI1_SAEM_t));
            its_msg_type = EI1_messageID_saem;
            break;

        case 7011: /* tolling */
            its_msg_descriptor = &asn_DEF_EI1_TPM;
            its_msg = calloc(1, sizeof(EI1_TPM_t));
            its_msg_type = 117;
            break;

        case 2043: /* maneuvers */
            its_msg_descriptor = &asn_DEF_EI1_VCM;
            its_msg = calloc(1, sizeof(EI1_VCM_t));
            its_msg_type = 43;
            break;

        case EIS_Port_poi: /* EVCSNM */
            its_msg_descriptor = &asn_DEF_EI1_EvcsnPdu;
            its_msg = calloc(1, sizeof(EI1_EvcsnPdu_t));
            its_msg_type = EI1_messageID_evcsn;
            break;

        case EIS_Port_evrsr: /* EVRSRM */
            its_msg_descriptor = &asn_DEF_EI1_EV_RSR;
            its_msg = calloc(1, sizeof(EI1_EV_RSR_t));
            its_msg_type = EI1_messageID_evcsn;
            break;

        default:
            if (!facilities.upf) {
                log_debug("messsage with unhandled BTP port received (%lld), ignoring", bpi->destinationPort);
                goto cleanup;
            } else {
                process_msg = false;
                fwd = true;
                its_msg_type = 7120;
            }
    }

    if (process_msg) {
        asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_descriptor, (void **)&its_msg, npi->data.buf, npi->data.size);
        if (dec.code) {
            log_debug("<- invalid %s received", its_msg_descriptor->name);
            rv = 1;
            goto cleanup;
        }

        // Get permisisons
        uint8_t *ssp = NULL;
        uint16_t ssp_len;
        uint8_t* neighbour_cert = NULL;

        switch (npi->network.present) {
            case EIS_NetworkingPacketRequestNW_PR_gn:
                if  (npi->network.choice.gn.securityPermissions) {
                    ssp = npi->network.choice.gn.securityPermissions->ssp.buf;
                    ssp_len = npi->network.choice.gn.securityPermissions->ssp.size;
                }
                if (npi->network.choice.gn.securityNeighbour) {
                    neighbour_cert = npi->network.choice.gn.securityNeighbour->buf;
                }
                break;
            case EIS_NetworkingPacketRequestNW_PR_gn6a:
                if  (npi->network.choice.gn6a.gn.securityPermissions) {
                    ssp = npi->network.choice.gn6a.gn.securityPermissions->ssp.buf;
                    ssp_len = npi->network.choice.gn6a.gn.securityPermissions->ssp.size;
                }
                if (npi->network.choice.gn6a.gn.securityNeighbour) {
                    neighbour_cert = npi->network.choice.gn6a.gn.securityNeighbour->buf;
                }
                break;
            default:
                break;
        }

        // Manage message
        switch (bpi->destinationPort) {
            case EIS_Port_cam:
                switch (check_cam(bpi, its_msg, ssp, ssp_len)) {
                    case CAM_OK:
                        fwd = true;
                        break;
                    case CAM_INVALID:
                    case CAM_BAD_PERMISSIONS:
                    default:
                        break;
                }
                break;

            case EIS_Port_denm:
#ifdef DEBUG
                uint8_t *xml_denm = malloc(32768);
                asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_EI1_DENM, its_msg);
                log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded, xml_denm);
                free(xml_denm);
#endif
                stored = true;
                switch (event_manage(its_msg, &id, ssp, ssp_len)) {
                    case EVENT_NEW:
                    case EVENT_CANCELLATION:
                    case EVENT_NEGATION:
                    case EVENT_UPDATE:
                    case EVENT_NUMBER_EXCEEDED:
                        fwd = true;
                        break;
                    case EVENT_INVALID:
                    case EVENT_PASSED:
                    case EVENT_REPEATED:
                    case EVENT_BAD_PERMISSIONS:
                        break;
                }
                break;

            case EIS_Port_ivim:
                stored = true;
                switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) {
                    case SERVICE_NEW:
                    case SERVICE_CANCELLATION:
                    case SERVICE_NEGATION:
                    case SERVICE_UPDATE:
                    case SERVICE_NUMBER_EXCEEDED:
                        fwd = true;
                        break;
                    case SERVICE_INVALID:
                    case SERVICE_REPEATED:
                    case SERVICE_PASSED:
                    case SERVICE_BAD_PERMISSIONS:
                    default:
                        break;
                }
                break;

            case EIS_Port_saem:
                switch (saem_check(its_msg, neighbour_cert)) {
                    case SAEM_NEW:
                        fwd = true;
                        break;
                    default:
                        break;
                }
                break;

            case 7011:
                if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_SPKI) { /* do not wait for facilities process if spki */
                    fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
                    fwd = false;
                } else {
                    fwd = true;
                }
                if (facilities.tolling.enabled) {
                    tpm_recv(its_msg, security_socket, neighbour_cert, NULL);
                }
                break;

            case 2043:
                fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
                fwd = false;
                if (facilities.coordination.active) {
                    vcm_check(its_msg);
                }
                break;

            case EIS_Port_poi:
                if (facilities.evm_args.activate) {
                    evcsnm_check(its_msg);
                    fwd = true;
                }
                break;

            case EIS_Port_evrsr:
                if (facilities.evm_args.activate) {
                    evrsrm_check(its_msg);
                    fwd = true;
                    EI1_EV_RSR_t *evrsr_request = (EI1_EV_RSR_t *)its_msg;
                    evrsrm_recv(evrsr_request);
                }
                break;

            default:
                break;
        }
    }

    // Forward to [applications]
    if (fwd) {
        fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
    }

    // 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, npi->id, false, its_msg_type, NULL, npi->data.buf, npi->data.size);
    }
    if (facilities.logging.recorder) {
        int e = itss_management_record_packet_sdu(
                buf,
                buf_len,
                npi->data.buf,
                npi->data.size,
                npi->id,
                itss_time_get(),
                ITSS_FACILITIES,
                false);
        if (e != -1) {
            itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, npi->id, "MReq.packet.set");
        }
    }

cleanup:
    if (its_msg && !stored) {
        ASN_STRUCT_FREE(*its_msg_descriptor, its_msg);
    }
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);

    return rv;
}


static int networking_packet_indication_tcp(EIS_NetworkingPacketIndication_t* npi, void** security_socket) {
    int rv = 0;

    EIS_TCPPacketIndication_t* tpi = &npi->transport.choice.tcp;

    uint16_t buf_len = 2048;
    uint8_t buf[buf_len];

    EIS_SecurityRequest_t* sreq = NULL;
    EIS_SecurityReply_t* srep = NULL;
    EIS_NetworkingRequest_t* nr = NULL;
    EIS_FacilitiesIndication_t* fi = NULL;

    void* its_msg = NULL;

    sreq = calloc(1, sizeof(EIS_SecurityRequest_t));
    sreq->present = EIS_SecurityRequest_PR_tlsRecv;
    sreq->choice.tlsRecv.data.size = npi->data.size;
    sreq->choice.tlsRecv.data.buf = malloc(npi->data.size);
    memcpy(sreq->choice.tlsRecv.data.buf, npi->data.buf, npi->data.size);

    uint64_t id = npi->id;
    pthread_mutex_lock(&facilities.tolling.lock);
    tlsc_t *tlsc = tolling_tlsc_get(tpi->sourceAddress->buf, 7011);
    if (tlsc) {
        id = tlsc->id;
    } else {
        tlsc = tolling_tlsc_new(tpi->sourceAddress->buf, 7011);
        id = tlsc->id;
    }
    ++tlsc->nmsg;
    pthread_mutex_unlock(&facilities.tolling.lock);
    sreq->choice.tlsSend.connId = id;

    uint8_t b_tx[2048], b_rx[2048];
    b_tx[0] = 4;
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityRequest, NULL, sreq, b_tx + 1, 2047);

    log_debug("->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded + 1);
    itss_0send(*security_socket, b_tx, enc.encoded + 1);
    int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000);
    log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl);

    if (oer_decode(NULL, &asn_DEF_EIS_SecurityReply, (void **)&srep, b_rx, rl).code) {
        log_error("SecurityReply.tlsRecv decode failure");
        rv = 1;
        goto cleanup;
    }

    if (srep->returnCode == EIS_SecurityReplyReturnCode_rejected) {
        log_error("SecurityReply.tlsRecv rejected");

        EIS_SecurityRequest_t *sREQ = calloc(1, sizeof(EIS_SecurityRequest_t));
        sREQ->present = EIS_SecurityRequest_PR_tlsShutdown;
        sREQ->choice.tlsShutdown.connId = id;
        b_tx[0] = 4;
        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityRequest, NULL, sREQ, b_tx + 1, 2047);
        log_debug("->[security] SecurityRequest.tlsShutdown (%ldB)", enc.encoded + 1);
        itss_0send(*security_socket, b_tx, enc.encoded + 1);
        int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000);
        log_debug("<-[security] SecurityReply.tlsShutdown (%dB)", rl);

        rv = 1;
        goto cleanup;
    }

    log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state);

    // Forward to [networking]
    if (srep->data->choice.tlsRecv.state != 1) {
        nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
        nr->present = EIS_NetworkingRequest_PR_packet;
        EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet;
        npr->transport.present = EIS_NetworkingPacketRequestTP_PR_tcp;
        npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;

        npr->data.size = srep->data->choice.tlsRecv.data.size;
        npr->data.buf = malloc(srep->data->choice.tlsRecv.data.size);
        memcpy(npr->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);

        npr->transport.present = EIS_NetworkingPacketRequestTP_PR_tcp;
        npr->transport.choice.tcp.sourcePort = tpi->destinationPort;
        npr->transport.choice.tcp.destinationPort = tpi->sourcePort;

        npr->transport.choice.tcp.destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
        npr->transport.choice.tcp.destinationAddress->buf = malloc(16);
        npr->transport.choice.tcp.destinationAddress->size = 16;
        memcpy(npr->transport.choice.tcp.destinationAddress->buf, tpi->sourceAddress->buf, 16);
        npr->transport.choice.tcp.destinationPort = 7011;
        npr->transport.choice.tcp.sourcePort = 7011;

        if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN ||
            (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS && tlsc->nmsg < 2)) {
            npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn6a;
            npr->network.choice.gn6a.gn.packetTransportType = EIS_PacketTransportType_shb;
            npr->network.choice.gn6a.gn.destinationAddress.buf = calloc(1, 6);
            npr->network.choice.gn6a.gn.destinationAddress.size = 6;
            npr->network.choice.gn6a.gn.securityProfile.encrypt = false;
            npr->network.choice.gn6a.gn.securityProfile.sign = true;

            npr->network.choice.gn6a.ip.destinationAddress.buf = malloc(16);
            npr->network.choice.gn6a.ip.destinationAddress.size = 16;
            memcpy(npr->network.choice.gn6a.ip.destinationAddress.buf, tpi->sourceAddress->buf, 16);
        } else {
            npr->network.present = EIS_NetworkingPacketRequestNW_PR_ip;
            npr->network.choice.ip.destinationAddress.buf = malloc(16);
            npr->network.choice.ip.destinationAddress.size = 16;
            memcpy(npr->network.choice.ip.destinationAddress.buf, tpi->sourceAddress->buf, 16);
        }

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

        buf[0] = 4;
        enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, buf + 1, buf_len - 1);
        if (enc.encoded == -1) {
            log_error("NR encoding error (%s)", enc.failed_type->name);
            rv = 1;
            goto cleanup;
        }

        itss_queue_send(facilities.tx_queue, buf, enc.encoded + 1, ITSS_NETWORKING, npr->id, "NR.packet.tcp");
    } else {
        if (facilities.tolling.enabled && srep->data->choice.tlsRecv.data.size) {
            asn_dec_rval_t dec = uper_decode_complete(NULL, &asn_DEF_EI1_TPM, (void **)&its_msg, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);
            if (dec.code) {
                log_debug("<- invalid TPM received");
                rv = 1;
                goto cleanup;
            }
            if (!dec.code) {
                tpm_recv(its_msg, security_socket, NULL, tpi->sourceAddress->buf);

                // Fwd to [applications]
                fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
                fi->present = EIS_FacilitiesIndication_PR_message;
                EIS_FacilitiesMessageIndication_t *fmi = &fi->choice.message;

                fmi->id = id;

                fmi->itsMessageType = 7011;

                fmi->data.size = srep->data->choice.tlsRecv.data.size;
                fmi->data.buf = malloc(srep->data->choice.tlsRecv.data.size);
                memcpy(fmi->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);

                uint8_t buffer[ITSS_SDU_MAX_LEN];
                buffer[0] = 4;  // Facilities
                asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1);

                itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message");

                // 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, id, false, 117, NULL, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);
                }
                if (facilities.logging.recorder) {
                    int e = itss_management_record_packet_sdu(
                            buf,
                            buf_len,
                            srep->data->choice.tlsRecv.data.buf,
                            srep->data->choice.tlsRecv.data.size,
                            id,
                            itss_time_get(),
                            ITSS_FACILITIES,
                            false);
                    if (e != -1) {
                        itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, id, "MReq.packet.set");
                    }
                }
            }
        }
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityRequest, sreq);
    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityReply, srep);
    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);

    return rv;
}

int networking_packet_indication(EIS_NetworkingPacketIndication_t* npi, void** security_socket) {

    int rv = 0;

    switch (npi->transport.present) {
        case EIS_NetworkingPacketIndicationTP_PR_btp:
            networking_packet_indication_btp(npi, security_socket);
            break;
        case EIS_NetworkingPacketIndicationTP_PR_tcp:
            networking_packet_indication_tcp(npi, security_socket);
            break;
        default:
            log_error("unhandled transport protocol");
            rv = 1;
            goto cleanup;

    }

cleanup:
    return rv;
}