#include "vcm.h"
#include "facilities.h"

#include <it2s-tender/epv.h>
#include <it2s-tender/space.h>
#include <it2s-tender/trajectory.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <itss-transport/TransportRequest.h>
#include <itss-facilities/FacilitiesIndication.h>
#include <itss-management/ManagementRequest.h>
#include <vcm/VCM.h>

static int do_paths_intersect(
        itss_st_t* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, 
        itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
        int* index
        ) {

    double A1[2], A2[2], B1[2], B2[2];
    uint64_t tsA, tsB;
    for (int a = 0; a < tA_len-1; ++a) {
        A1[0] = tA[a].latitude;
        A1[1] = tA[a].longitude;
        A2[0] = tA[a+1].latitude;
        A2[1] = tA[a+1].longitude;
        for (int b = 0; b < tB_len-1; ++b) {
            if (tA[a].timestamp > tB[b].timestamp + 2000 ||
                tA[a].timestamp < tB[b].timestamp - 2000) {
                continue;
            }

            B1[0] = tB[b].latitude;
            B1[1] = tB[b].longitude;
            B2[0] = tB[b+1].latitude;
            B2[1] = tB[b+1].longitude;

            if (itss_do_segments_intersect(A1, A2, B1, B2)) {
                    *index = a;
                    return 1;
            }
        }
    }

    return 0;
}

static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) {

    coordination_t* coordination = &facilities.coordination;

    pthread_mutex_lock(&facilities.id.lock);
    uint32_t my_station_id = facilities.id.station_id;
    pthread_mutex_unlock(&facilities.id.lock);

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

    itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    ssize_t trajectoryA_len = 0;

    TransportRequest_t* tr = NULL;
    FacilitiesIndication_t* fi = NULL;
    VCM_t* vcm_rep = calloc(1, sizeof(VCM_t));

    int rv = 0;
    const uint16_t buf_len = 2048;
    uint8_t buf1[buf_len];

    vcm_rep->header.messageID = 43;
    vcm_rep->header.protocolVersion = 1;
    vcm_rep->header.stationID = my_station_id;
            
    vcm_rep->vcm.currentPosition.latitude = lat;
    vcm_rep->vcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);

    if (coordination->chain.id) {
        vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
        vcm_rep->vcm.chain->id = coordination->chain.id;
        vcm_rep->vcm.chain->link.buf = malloc(32);
        vcm_rep->vcm.chain->link.size = 32;
        memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 32);
        vcm_rep->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t));
        vcm_rep->vcm.chain->area->buf = malloc(4);
        vcm_rep->vcm.chain->area->size = 4;
    }

    vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
    ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;

    // Vehicle DimensionsDimensions
    mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
    mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
    mvc_rep->vehicleWidth = facilities.vehicle.width;

    itss_trajectory_lock();
    trajectoryA_len = epv.trajectory.len;
    memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
    itss_trajectory_unlock();

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    // Planned trajectory
    mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
    mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
    mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
    for (int i = 0; i < trajectoryA_len - 1; ++i) {
        mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
        mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
        mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
    }

    mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
    mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0;
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = 0;
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = NULL;
    mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
    mvc_rep->negotiation->choice.reply.nonce = vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request.nonce;

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len);
    if (enc.encoded == -1) {
        log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    ssize_t vcm_rep_len = (enc.encoded + 7) / 8;

    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->btpType = BTPType_btpB;
    bpr->id = itss_id(buf1, vcm_rep_len);
    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 = 2043;
    bpr->gn.trafficClass = 2;
    bpr->data.buf = malloc(vcm_rep_len);
    memcpy(bpr->data.buf, buf1, vcm_rep_len);
    bpr->data.size = vcm_rep_len;
    buf1[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");

    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.id = bpr->id;
    fi->choice.message.itsMessageType = 2043;
    fi->choice.message.data.size = bpr->data.size;
    fi->choice.message.data.buf = malloc(bpr->data.size);
    memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
    buf1[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");

    coordination->t_last_send_vcm = now;

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

cleanup:
    ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
}

static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
    int rv = 0;

    coordination_t* coordination = &facilities.coordination;

    pthread_mutex_lock(&facilities.id.lock);
    uint32_t my_station_id = facilities.id.station_id;
    pthread_mutex_unlock(&facilities.id.lock);

    // Is request for me?
    CoordinationRequest_t* request = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
    bool is_req4me = false;
    for (int i = 0; i < request->desiredTrajectories.list.count && !is_req4me; ++i) {
        for (int j = 0; j < request->desiredTrajectories.list.array[i]->affectingStations.list.count; ++j) {
            log_debug("[vc] VCM.request meant for %d", *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]);
            if (my_station_id == *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]) {
                is_req4me = true;
                break;
            }
        }
    }
    if (!is_req4me) {
        log_debug("[vc] received VCM.request not affecting me");
        return 0;
    }

    uint64_t now = itss_time_get();

    if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */
        log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID);
        vcm_reject(vcm, neighbour);
        return 1;
    }

    const ssize_t buf_len = 512;
    uint8_t buf1[buf_len], buf2[buf_len];

    // Change speed
    // Just check who is closer to mid trajectory point, if ego is closer, accelerate
    // TODO a real trajectory analysis
    STDeltaTrajectory_t* stdt = &request->desiredTrajectories.list.array[0]->trajectory;
    itss_st_t* trj = calloc(stdt->list.count/2 + 1, sizeof(itss_st_t));
    trj[0].latitude = vcm->vcm.currentPosition.latitude;
    trj[0].longitude = vcm->vcm.currentPosition.longitude;
    asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trj[0].timestamp);
    int h = 0;
    for (h = 1; h < stdt->list.count/2; ++h) {
        trj[h].latitude = trj[h-1].latitude + stdt->list.array[h-1]->deltaLatitude;
        trj[h].longitude = trj[h-1].longitude + stdt->list.array[h-1]->deltaLongitude;
        trj[h].timestamp = trj[h-1].timestamp + stdt->list.array[h-1]->deltaTime;
    }
    if (h) --h;

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

    double dreq = itss_haversine(
            trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
            vcm->vcm.currentPosition.latitude / 1.0e7, vcm->vcm.currentPosition.longitude / 1.0e7
            );
    double dego = itss_haversine(
            trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
            lat / 1.0e7, lon / 1.0e7
            );
    free(trj);

    ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
    mreq->present = ManagementRequest_PR_attributes;
    mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
    mreq->choice.attributes.choice.set.speed = calloc(1, sizeof(ManagementSpeedSet_t));
    ManagementSpeedSet_t* mgss = mreq->choice.attributes.choice.set.speed;
    mgss->rate = 5; /* km/h/s */
    mgss->temporary = true; /* go back to original speed after a while */
    mgss->type.present = ManagementSpeedSetType_PR_diff; /* differential change set */
    mgss->type.choice.diff = (dreq > dego) ? 10 : -10; /* % */
    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, mreq, buf1, buf_len);
    if (enc.encoded == -1) {
        log_error("[vc] failed to encode MReq.speedSet (%s)", enc.failed_type->name);
    }
    itss_0send(coordination->mgmt_socket, buf1, enc.encoded);
    if (itss_0recv_rt(&coordination->mgmt_socket, buf2, buf_len, buf1, enc.encoded, 500) == -1) {
        log_error("[vc]-> MReq.speedSet ->[management] <TIMEOUT>");
    }
    ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);

    // Respond
    VCM_t* vcm_rep = NULL;
    TransportRequest_t* tr = NULL;
    FacilitiesIndication_t* fi = NULL;


    itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    ssize_t trajectoryA_len = 0;


    neighbour->proposed = true;
    neighbour->t_proposal = now;

    vcm_rep = calloc(1, sizeof(VCM_t));

    vcm_rep->header.messageID = 43;
    vcm_rep->header.protocolVersion = 1;
    vcm_rep->header.stationID = my_station_id;
            
    vcm_rep->vcm.currentPosition.latitude = lat;
    vcm_rep->vcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);

    if (coordination->chain.id) {
        vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
        vcm_rep->vcm.chain->id = coordination->chain.id;
        vcm_rep->vcm.chain->link.buf = malloc(32);
        vcm_rep->vcm.chain->link.size = 32;
        memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 32);
        vcm_rep->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t));
        vcm_rep->vcm.chain->area->buf = malloc(4);
        vcm_rep->vcm.chain->area->size = 4;
    }

    vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
    ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;

    // Vehicle DimensionsDimensions
    mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
    mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
    mvc_rep->vehicleWidth = facilities.vehicle.width;

    itss_trajectory_lock();
    trajectoryA_len = epv.trajectory.len;
    memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
    itss_trajectory_unlock();

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    // Planned trajectory
    mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
    mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
    mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
    for (int i = 0; i < trajectoryA_len - 1; ++i) {
        mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
        mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
        mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
    }

    // Accepted trajectory
    if (!request->desiredTrajectories.list.count) {
        log_debug("[vc] received VCM request has no desired trajectories");
        rv = 1;
        goto cleanup;
    }
    ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0];

    // TODO check if vehicles are really going to intersect

    mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
    mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
    *mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
    mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
    mvc_rep->negotiation->choice.reply.nonce = request->nonce;

    enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len);
    if (enc.encoded == -1) {
        log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    ssize_t vcm_rep_len = (enc.encoded + 7) / 8;

    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->btpType = BTPType_btpB;
    bpr->id = itss_id(buf1, vcm_rep_len);
    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 = 2043;
    bpr->gn.trafficClass = 2;
    bpr->data.buf = malloc(vcm_rep_len);
    memcpy(bpr->data.buf, buf1, vcm_rep_len);
    bpr->data.size = vcm_rep_len;
    buf1[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");

    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.id = bpr->id;
    fi->choice.message.itsMessageType = 2043;
    fi->choice.message.data.size = bpr->data.size;
    fi->choice.message.data.buf = malloc(bpr->data.size);
    memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
    buf1[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");

    coordination->t_last_send_vcm = now;

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

cleanup:
    ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
    return rv;
}

static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
    int rv = 0;

    coordination_t* coordination = &facilities.coordination;

    if (!coordination->session.req) {
        log_debug("[vc] unknown reply context - session expired or another ITS-S rejected");
        return 1;
    }

    CoordinationReply_t* reply = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;

    itss_time_lock();
    uint64_t now_us = itss_ts_get(TIME_MICROSECONDS);
    itss_time_unlock();

    if (neighbour->intersecting) {

        if (reply->acceptedTrajectoriesIds.list.count == 0) {
            log_debug("[vc] maneuver %lld rejected by %d", reply->requesterId, vcm->header.stationID);
            ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
            coordination->session.req = NULL;
            coordination->session.ts = itss_time_get();
            memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
            memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
            coordination->session.n_affs_trjs = 0;
            neighbour->intersecting = false;
            neighbour->proposed = false;
            return 1;
        }
        for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) {
            int tid = *reply->acceptedTrajectoriesIds.list.array[t];
            if (tid > MC_TRAJECTORIES_N_MAX - 1) {
                return 1;
            }
            for (int n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) {
                if (neighbour->station_id == coordination->session.affs[tid][n]) {
                    for (int i = n; i < coordination->session.n_affs_neighs[tid] - 1; ++i) {
                        coordination->session.affs[tid][i] = coordination->session.affs[tid][i+1];
                    }
                    --coordination->session.n_affs_neighs[tid];
                    break;
                }
            }
        }
        bool all_rep = true;
        for (int t = 0; t < coordination->session.n_affs_trjs; ++t) {
            if (coordination->session.n_affs_neighs[t] != 0) {
                all_rep = false;
                break;
            }
        }
        char fin[50];
        fin[0] = 0;
        if (all_rep) {
            ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
            coordination->session.req = NULL;
            coordination->session.ts = itss_time_get();
            memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
            memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
            coordination->session.n_affs_trjs = 0;
            sprintf(fin, ", all replies received");
        }
        log_info("[vc] received VCM.reply from %d with %d accepted trajectories%s | took %ld us", 
                vcm->header.stationID,
                reply->acceptedTrajectoriesIds.list.count, 
                fin,
                now_us-neighbour->t_iid);

        neighbour->intersecting = false;
        neighbour->proposed = false;

    } else {
        log_info("[vc] received VCM.reply is response to another ITS-S VCM.request");
    }

    return rv;
}

static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {

    int rv = 0;

    coordination_t* coordination = &facilities.coordination;

    VCM_t* vcm_req = NULL;
    TransportRequest_t* tr = NULL;
    FacilitiesIndication_t* fi = NULL;

    const ssize_t buf_len = 1024;
    uint8_t buf[buf_len];

    itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    uint16_t trajectoryA_len = 0;

    uint64_t now = itss_time_get();


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

    itss_trajectory_lock();
    trajectoryA_len = epv.trajectory.len;
    memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
    itss_trajectory_unlock();

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    // Initiate conflict resolution
    vcm_req = calloc(1, sizeof(VCM_t));

    vcm_req->header.messageID = 43;
    vcm_req->header.protocolVersion = 1;
    pthread_mutex_lock(&facilities.id.lock);
    vcm_req->header.stationID = facilities.id.station_id;
    pthread_mutex_unlock(&facilities.id.lock);
            
    vcm_req->vcm.currentPosition.latitude = lat;
    vcm_req->vcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);

    if (coordination->chain.id) {
        vcm_req->vcm.chain = calloc(1, sizeof(ChainInformation_t));
        vcm_req->vcm.chain->id = coordination->chain.id;
        vcm_req->vcm.chain->link.buf = malloc(32);
        vcm_req->vcm.chain->link.size = 32;
        memcpy(vcm_req->vcm.chain->link.buf, coordination->chain.link, 32);
        vcm_req->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t));
        vcm_req->vcm.chain->area->buf = malloc(4);
        vcm_req->vcm.chain->area->size = 4;
    }

    vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
    ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle;

    // Vehicle Dimensions
    mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
    mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
    mvc->vehicleWidth = facilities.vehicle.width;

    // Planned trajectory
    mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
    mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
    mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
    for (int i = 0; i < trajectoryA_len - 1; ++i) {
        mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
        mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
        mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
    }

    // Desired trajectory
    mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
    mvc->negotiation->present = CoordinationNegotiation_PR_request;
    mvc->negotiation->choice.request.desiredTrajectories.list.count = 1;
    mvc->negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
    mvc->negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
    mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
    ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0];
    mvc->negotiation->choice.request.requesterId = vcm->header.stationID;
    mvc->negotiation->choice.request.nonce = rand();
    neighbour->nonce = mvc->negotiation->choice.request.nonce;

    pt->trajectory.list.count = trajectoryA_len - 1;
    pt->trajectory.list.size = sizeof(void*) * (trajectoryA_len - 1);
    pt->trajectory.list.array = malloc(sizeof(void*) * (trajectoryA_len - 1));
    for (int i = 0; i < trajectoryA_len - 1; ++i) {
        pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        pt->trajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
        pt->trajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
        pt->trajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
    }

    pt->offer = 5;
    pt->priority = 1;
    pt->id = 0;

    itss_time_lock();
    uint64_t now_us = itss_ts_get(TIME_MICROSECONDS) ;
    itss_time_unlock();

    neighbour->intersecting = true;
    neighbour->t_iid = now_us;

    uint32_t intneigh[MC_AFF_STATIONS_N_MAX];
    intneigh[0] = neighbour->station_id;
    int n_intneigh = 1;
    for (int n = 0; n < coordination->neighbours_len && n_intneigh < MC_AFF_STATIONS_N_MAX; ++n) {
        if (coordination->neighbours[n].station_id == neighbour->station_id) {
            continue;
        }

        int index;
        if (do_paths_intersect(
                trajectoryA, trajectoryA_len,
                facilities.vehicle.length, facilities.vehicle.length, 
                coordination->neighbours[n].trajectory, coordination->neighbours[n].trajectory_len, 
                coordination->neighbours[n].length, coordination->neighbours[n].width, 
                &index
                )) {
            coordination->neighbours[n].intersecting = true;
            coordination->neighbours[n].t_iid = now_us;
            intneigh[n_intneigh] = coordination->neighbours[n].station_id;
            ++n_intneigh;
        }
    } 

#ifndef NDEBUG
    char buffer[512];
    int wr = 0;
#endif
    pt->affectingStations.list.count = n_intneigh;
    pt->affectingStations.list.size = n_intneigh*sizeof(void*);
    pt->affectingStations.list.array = malloc(n_intneigh * sizeof(void*));
    for (int i = 0; i < n_intneigh; ++i) {
        pt->affectingStations.list.array[i] = malloc(sizeof(long long));
        *pt->affectingStations.list.array[i] = intneigh[i];
#ifndef NDEBUG
        wr += sprintf(buffer + wr, " %d", intneigh[i]);
#endif
    }
    log_debug("[vc] this VCM.request affects %d station(s):%s", n_intneigh, buffer);

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_req, buf, buf_len);
    if (enc.encoded == -1) {
        log_error("[vc] VCM.request encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    ssize_t vcm_req_len = (enc.encoded + 7) / 8;

    coordination->session.req = vcm_req;
    coordination->session.ts = now;
    coordination->session.n_affs_trjs = 1;
    coordination->session.n_affs_neighs[0] = n_intneigh;
    memcpy(coordination->session.affs[0], intneigh, sizeof(intneigh));

    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->id = itss_id(buf, vcm_req_len);
    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 = 2043;
    bpr->gn.trafficClass = 2;
    bpr->data.buf = malloc(vcm_req_len);
    memcpy(bpr->data.buf, buf, vcm_req_len);
    bpr->data.size = vcm_req_len;
    buf[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.request encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");

    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.id = bpr->id;
    fi->choice.message.itsMessageType = 2043;
    fi->choice.message.data.size = bpr->data.size;
    fi->choice.message.data.buf = malloc(bpr->data.size);
    memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
    buf[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");

    coordination->t_last_send_vcm = now;

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

cleanup:
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
    return rv;
}

static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) {

    ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;

    itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
    uint16_t trajectoryA_len = 0;
    uint16_t trajectoryB_len = 0;

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

    itss_trajectory_lock();
    trajectoryA_len = epv.trajectory.len;
    memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
    itss_trajectory_unlock();

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
    trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
    asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
    ++trajectoryB_len;
    for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
        trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->deltaLatitude + trajectoryB[i-1].latitude;
        trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->deltaLongitude + trajectoryB[i-1].longitude;
        trajectoryB[i].timestamp = mvc->plannedTrajectory.list.array[i]->deltaTime + trajectoryB[i-1].timestamp;
        ++trajectoryB_len;
    }

    // Save neighbour trajectory
    memcpy(neighbour->trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
    neighbour->trajectory_len = trajectoryB_len;
    neighbour->length = mvc->vehicleLength.vehicleLengthValue;
    neighbour->width = mvc->vehicleWidth;

    int index = -1;

    if (trajectoryA_len > 1 && trajectoryB_len > 1) {
        bool intersecting = do_paths_intersect(
                trajectoryA, trajectoryA_len, facilities.vehicle.length, facilities.vehicle.width, 
                trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth,
                &index);
        if (intersecting) {
            log_info("[vc] intersection danger with %lld @ (%f, %f) in %ld ms", 
                    vcm->header.stationID, 
                    trajectoryA[index].latitude/1.0e7, 
                    trajectoryA[index].longitude/1.0e7, 
                    trajectoryA[index].timestamp - now);

            intersection_detected(vcm, neighbour);
        }
    }
}


int vcm_check(VCM_t* vcm) {
    coordination_t* coordination = &facilities.coordination;

    int rv = 0;


    uint64_t now = itss_time_get();


    pthread_mutex_lock(&coordination->lock);

    // Find neighbours
    int ni = -1;
    for (int i = 0; i < coordination->neighbours_len; ++i) {
        if (coordination->neighbours[i].station_id == vcm->header.stationID) {
            ni = i;
            break;
        }
    }

    // Add it if not found
    if (ni == -1 && coordination->neighbours_len < MC_MAX_NEIGHBOURS) {
        coordination->neighbours[coordination->neighbours_len].station_id = vcm->header.stationID;
        ni = coordination->neighbours_len;
        ++coordination->neighbours_len;
    }

    if (ni == -1) {
        rv = 1;
        goto cleanup;
    }

    mc_neighbour_s* neighbour = &coordination->neighbours[ni];

    switch (vcm->vcm.maneuverContainer.present) {
        case ManeuverContainer_PR_vehicle:
            ;
            ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
            if (mvc->negotiation) {
                switch (mvc->negotiation->present) {
                    case CoordinationNegotiation_PR_request:
                        vcm_check_handle_request(vcm, neighbour);
                        break; 

                    case CoordinationNegotiation_PR_reply:
                        vcm_check_handle_reply(vcm, neighbour);
                        break; 

                    default:
                        break;
                }
            } else { 
                if (
                    (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */
                    now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */
                    ) { /* in maneuver */
                    log_debug("[vc] ignoring VCM from %d - currently in maneuver", vcm->header.stationID); 
                } else { /* not in maneuver OR timeout reached */
                    if (coordination->session.req) { /* clear previous request if exists */
                        ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
                        coordination->session.req = NULL;
                        coordination->session.ts = now;
                        memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
                        memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
                        coordination->session.n_affs_trjs = 0;
                    }

                    /* check if received VCM intersects with ego trajectory */
                    intersection_check(vcm, neighbour);
                }
            }
            break;
        case ManeuverContainer_PR_rsu:
            break;
        default:
            log_debug("[vc] received VCM contains unrecognized ManeuverContainer type");
            rv = 1;
    }

cleanup:
    pthread_mutex_unlock(&coordination->lock);

    return rv;
}

static int mk_vcm(uint8_t* vcm_uper, uint16_t* vcm_uper_len) {
    int rv = 0;

    coordination_t* coordination = &facilities.coordination;

    itss_st_t trajectory[TRAJECTORY_MAX_LEN];

    VCM_t* vcm = calloc(1, sizeof(VCM_t));

    vcm->header.messageID = 43;
    vcm->header.protocolVersion = 1;
    uint64_t now = itss_time_get();
    pthread_mutex_lock(&facilities.id.lock);
    vcm->header.stationID = facilities.id.station_id;
    pthread_mutex_unlock(&facilities.id.lock);

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

    itss_trajectory_lock();
    trajectory_len = epv.trajectory.len;
    memcpy(trajectory, epv.trajectory.path, trajectory_len * sizeof(itss_st_t));
    itss_trajectory_unlock();

    vcm->vcm.currentPosition.latitude = lat;
    vcm->vcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now);

    if (coordination->chain.id) {
        vcm->vcm.chain = calloc(1, sizeof(ChainInformation_t));
        vcm->vcm.chain->id = coordination->chain.id;
        vcm->vcm.chain->link.buf = malloc(32);
        vcm->vcm.chain->link.size = 32;
        memcpy(vcm->vcm.chain->link.buf, coordination->chain.link, 32);
        vcm->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t));
        vcm->vcm.chain->area->buf = malloc(4);
        vcm->vcm.chain->area->size = 4;
    }

    if (facilities.station_type == StationType_roadSideUnit) {
        vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
        ManeuverRSUContainer_t* mrc = &vcm->vcm.maneuverContainer.choice.rsu;
        mrc->recommendedTrajectories = NULL; // TODO
    } else {
        vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
        ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
        // Vehicle Dimensions
        mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
        mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
        mvc->vehicleWidth = facilities.vehicle.width;

        mvc->plannedTrajectory.list.count = trajectory_len;
        mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
        mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
        mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(STPoint_t));
        mvc->plannedTrajectory.list.array[0]->deltaLatitude = trajectory[0].latitude - lat;
        mvc->plannedTrajectory.list.array[0]->deltaLongitude = trajectory[0].longitude - lon;
        mvc->plannedTrajectory.list.array[0]->deltaTime = trajectory[0].timestamp - now;
        for (int i = 1; i < trajectory_len; ++i) {
            mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
            mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectory[i].latitude - trajectory[i-1].latitude;
            mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectory[i].longitude - trajectory[i-1].longitude;
            mvc->plannedTrajectory.list.array[i]->deltaTime = trajectory[i].timestamp - trajectory[i-1].timestamp;
        }
    }

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm, vcm_uper, 512);
    if (enc.encoded == -1) {
        log_error("[vc] failed encoding VCM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup; 
    }
    *vcm_uper_len = (enc.encoded + 7) / 8;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_VCM, vcm);

    return rv;
}

static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {

    bool send = false;

    if (now > coordination->t_last_send_vcm + coordination->vcm_period_max) {
        send = true;
    }

    return send;
}

void* vc_service() {

    coordination_t* coordination = (coordination_t*) &facilities.coordination;

    uint8_t vcm[512];

    TransportRequest_t* 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->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 = 2043;
    bpr->gn.trafficClass = 2;
    bpr->data.buf = malloc(512);

    FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi = calloc(1, sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    fi->choice.message.itsMessageType = 2043;
    fi->choice.message.data.buf = malloc(512);

    uint8_t buf[1024];
    buf[0] = 4;

    int rv;

    while (!facilities.exit) {

        uint64_t now = itss_time_get();

        pthread_mutex_lock(&coordination->lock);
        if (vcm_timer_check(coordination, now)) {
            rv = mk_vcm(bpr->data.buf, (uint16_t *) &bpr->data.size);
            if (rv) {
                pthread_mutex_unlock(&coordination->lock);
                continue;
            }

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

            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, 1023);
            if (enc.encoded == -1) {
                log_error("encoding TR for VCM failed");
                continue;
            }
            itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");

            fi->choice.message.id = bpr->id;
            fi->choice.message.data.size = bpr->data.size;
            memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
            enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, 1023);
            if (enc.encoded == -1) {
                log_error("encoding FI for VCM failed");
                continue;
            }
            itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");

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

            coordination->t_last_send_vcm = now;
        }
        pthread_mutex_unlock(&coordination->lock);

        usleep(50 * 1000);
    }

    itss_0close(coordination->mgmt_socket);

    return NULL;
}

void coordination_init() {
    coordination_t* coo = &facilities.coordination;
    pthread_mutex_init(&coo->lock, NULL);
    coo->mgmt_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
}