#include "dcm.h"
#include "facilities.h"

#include <it2s-tender/space.h>
#include <itss-transport/TransportRequest.h>
#include <dcm/DCM.h>

#define SLEEP_TIME_MS 100

int dcm_check() {

    return 0;
}

int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
    int rv = 0;

    it2s_tender_st_s* trajectory = NULL;

    DCM_t* dcm = calloc(1, sizeof(DCM_t));

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

    int32_t lat, lon;
    uint16_t trajectory_len = 0;
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    lat = facilities->epv.space.latitude;
    lon = facilities->epv.space.longitude;
    trajectory_len = facilities->epv.trajectory.len;
    trajectory = malloc(trajectory_len * sizeof(it2s_tender_st_s));
    memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
    it2s_tender_unlock_space(&facilities->epv);

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

    dcm->dcm.plannedTrajectory.list.count = trajectory_len;
    dcm->dcm.plannedTrajectory.list.size = trajectory_len * sizeof(void*);
    dcm->dcm.plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
    for (int i = 0; i < trajectory_len; ++i) {
        dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
        dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
        asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
    }

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DCM, NULL, dcm, dcm_uper, 512);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [dc] failed encoding DCM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup; 
    }
    *dcm_uper_len = (enc.encoded + 7) / 8;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_DCM, dcm);
    free(trajectory);

    return rv;
}

void* dc_service(void* fc) {

    facilities_t* facilities = (facilities_t*) fc;

    uint8_t dcm[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);

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

    int rv;

    while (!facilities->exit) {

        rv = mk_dcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
        if (rv) {
            continue;
        }

        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
        if (enc.encoded == -1) {
            syslog_err("[facilities] encoding TR for cam failed");
            continue;
        }

        queue_send(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
        
        usleep(SLEEP_TIME_MS * 1000);
    }


    return NULL;
}