#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <itss-facilities/FacilitiesDataIndication.h>
#include <itss-facilities/FacilitiesDataRequest.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 decap(void* ctx) {

    return 0;
}


int forward_up(void* ctx) {
    printf("Testing forwarding up:\n");
    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;

    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"); 
        return 1;
    }
    printf("  - Sent CAM BDI (%ldB)\n", bdi->data.size);

    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"); 
        return 1;
    }
    printf("  - Received CAM FDI (%ldB)\n", fdi->data.size);

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

    return 0;
}


int main() {

    int rv = 0;

    void* ctx = zmq_ctx_new();

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

    return rv;
}