#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <itss-facilities/FacilitiesDataIndication.h>
#include <itss-facilities/FacilitiesDataRequest.h>
#include <itss-management/ManagementRequest.h>
#include <itss-management/ManagementReply.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

#include <pthread.h>
#include <zmq.h>
#include <unistd.h>

void hex2bin(char* hex, uint8_t** bin, uint16_t* bin_len) {
    *bin_len = strlen(hex) / 2;
    *bin = malloc(*bin_len);
    char substr[3];
    substr[2] = '\0';
    for (int i = 0; i < *bin_len; ++i) {
        memcpy(substr, hex + i * 2, 2);
        (*bin)[i] = (uint8_t)strtoul(substr, NULL, 16);
    }
}

int init(void* ctx) {
    printf("Initializing management simulator \n");
    fflush(stdout);

    void* management_socket = zmq_socket(ctx, ZMQ_REP);
    struct stat st = {0};
    if (stat("/tmp/itss/", &st) == -1) {
        mkdir("/tmp/itss", 0777);
    }

    int ra = zmq_bind(management_socket, "ipc:///tmp/itss/management");
    uint8_t buffer[2048];

    // Ignore first request (set)
    zmq_recv(management_socket, buffer, 2048, 0);
    printf("  - Received MREQ.set \n");
    fflush(stdout);
    int code = 0;
    zmq_send(management_socket, &code, 1, 0);

    // Request get
    zmq_recv(management_socket, buffer, 2048, 0);
    int rv = 0;

    ManagementRequest_t* mreq = NULL;
    asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, (void**) &mreq, buffer, 2048);
    if (dec.code) {
        rv = 1;
        printf("  FAIL: MREQ decode error\n");
        fflush(stdout);
        goto cleanup;
    }
    printf("  - Received MREQ.get\n");
    fflush(stdout);

    ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t));
    switch (mreq->present) {
        case ManagementRequest_PR_get:
            mrep->returnCode = ManagementReplyReturnCode_accepted;
            mrep->attributes = calloc(1, sizeof(ManagementReplyAttributes_t));

            if (mreq->choice.get.stationId) {
                mrep->attributes->stationId = malloc(sizeof(long));
                *mrep->attributes->stationId = 78;
            }

            if (mreq->choice.get.gpsType) {
                mrep->attributes->gpsType = malloc(sizeof(long));
                *mrep->attributes->gpsType = 0;
            }

            if (mreq->choice.get.clockType) {
                mrep->attributes->clockType = malloc(sizeof(long));
                *mrep->attributes->clockType = 0;
            }

            if (mreq->choice.get.clock) {
                mrep->attributes->clock = calloc(1, sizeof(TimestampIts_t));
                asn_ulong2INTEGER(mrep->attributes->clock, 666);
            }

            if (mreq->choice.get.clockOffset) {
                mrep->attributes->clockOffset = calloc(1, sizeof(TimestampIts_t));
                asn_ulong2INTEGER(mrep->attributes->clockOffset, 0);
            }

            if (mreq->choice.get.coordinates) {
                mrep->attributes->coordinates = calloc(1, sizeof(WGS84Coordinates_t));
                mrep->attributes->coordinates->latitude = 0;
                mrep->attributes->coordinates->longitude = 0;
            }

            if (mreq->choice.get.altitude) {
                mrep->attributes->altitude = calloc(1, sizeof(Altitude_t));
                mrep->attributes->altitude->altitudeValue = 0;
                mrep->attributes->altitude->altitudeConfidence = 1;
            }

            if (mreq->choice.get.speed) {
                mrep->attributes->speed = calloc(1, sizeof(Speed_t));
                mrep->attributes->speed->speedValue = 0;
                mrep->attributes->speed->speedConfidence = 1;
            }

            if (mreq->choice.get.heading) {
                mrep->attributes->heading = calloc(1, sizeof(Heading_t));
                mrep->attributes->heading->headingValue = 0;
                mrep->attributes->heading->headingConfidence = 1;
            }
            break;

        case ManagementRequest_PR_set:
        default:
            rv = 1;
            printf("  FAIL: MREQ. not GET (%d)\n", mreq->present);
            fflush(stdout);
            goto cleanup;
    }

    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementReply, NULL, mrep, buffer, 256);
    zmq_send(management_socket, buffer, enc.encoded, 0);

    printf("  OK\n");
    fflush(stdout);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
    ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep);

    return rv;
}

int cam_gen(void* ctx) {
    printf("Testing CAM generation:\n"); fflush(stdout);

    void* transport_socket = zmq_socket(ctx, ZMQ_REP);

    int rv = zmq_bind(transport_socket, "ipc:///tmp/itss/transport");
    printf("rv %d\n", rv); fflush(stdout);
    uint8_t buffer[2048];
    zmq_recv(transport_socket, buffer, 2048, 0);
    int code = 0;
    zmq_send(transport_socket, &code, 1, 0);

    zmq_close(transport_socket);

    BTPDataRequest_t* bdr = NULL;
    asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_BTPDataRequest, (void**) &bdr, buffer+1, 2047);
    if (dec.code) {
        printf("  FAIL\n"); fflush(stdout);
        return 1;
    }
    printf("  - Received CAM BDR (%ldB)\n", bdr->data.size); fflush(stdout);

    if (bdr->destinationPort == Port_cam) {
        printf("  OK\n"); fflush(stdout);
    } else {
        printf("  FAIL\n"); fflush(stdout);
        return 1;
    }

    return 0;
}


int forward_up(void* ctx) {
    printf("Testing forwarding up:\n");
    fflush(stdout);
    BTPDataIndication_t* bdi = calloc(1, sizeof(BTPDataIndication_t));

    char* cam_hex = "02027dfddf4403ca4059bba5f38cc40dba9ffffffc2230eff200e11fc0078082a88a8337fee3fff600004dffea800618d08018efff14003ac68800c77ff8e002263460063bffd1000fb1a30031dffe2800958d30018efff840048c68800c77ffae002c63480063bffbd001a31a40031dfff28002d8cf0018c0";

    uint8_t* cam;
    uint16_t cam_len;
    hex2bin(cam_hex, &cam, &cam_len);

    bdi->data.buf = cam;
    bdi->data.size = cam_len;
    bdi->gnDestinationAddress.buf = calloc(1, 6);
    bdi->gnDestinationAddress.size = 6;
    bdi->destinationPort = Port_cam;

    uint8_t b_bdi_cam[512];
    b_bdi_cam[0] = 3;

    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_BTPDataIndication, bdi, b_bdi_cam+1, 511);

    void* facilities_socket = zmq_socket(ctx, ZMQ_REQ);

    int code = 0;

    zmq_connect(facilities_socket, "ipc:///tmp/itss/facilities");
    zmq_send(facilities_socket, b_bdi_cam, enc.encoded+1, 0);
    zmq_recv(facilities_socket, &code, 1, 0);
    if (code) {
        printf("  FAIL\n"); 
        fflush(stdout);
        return 1;
    }
    printf("  - Sent CAM BDI (%ldB)\n", bdi->data.size);
    fflush(stdout);

    void* applications_socket = zmq_socket(ctx, ZMQ_REP);

    zmq_bind(applications_socket, "ipc:///tmp/itss/applications");
    uint8_t buffer[2048];
    zmq_recv(applications_socket, buffer, 2048, 0);
    code = 0;
    zmq_send(applications_socket, &code, 1, 0);

    zmq_close(applications_socket);
    zmq_close(facilities_socket);

    FacilitiesDataIndication_t* fdi = NULL;
    asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesDataIndication, (void**) &fdi, buffer+1, 2047);
    if (dec.code) {
        printf("  FAIL\n"); 
        fflush(stdout);
        return 1;
    }
    printf("  - Received CAM FDI (%ldB)\n", fdi->data.size);
    fflush(stdout);

    if (fdi->data.size == bdi->data.size) {
        printf("  OK\n"); 
        fflush(stdout);
    } else {
        printf("  FAIL\n"); 
        fflush(stdout);
        return 1;
    }

    return 0;
}


int main() {

    int rv = 0;

    void* ctx = zmq_ctx_new();

    rv += init(ctx);
    if (rv) return 1;

    rv += forward_up(ctx);
    rv += cam_gen(ctx);

    return rv;
}