#include <cstdlib>
#include <iostream>
#include <sys/mman.h>

#include <native/task.h>
#include <native/queue.h>
#include <native/timer.h>

using namespace std;

static RT_TASK task;
static RT_QUEUE queue;
static volatile bool end = false;

static void sigHandler(int sig __attribute__((unused))) {
    cout << "signal handler\n";
    end = true;
}

int main(int argc, char** argv) {
    int res;

    // lock memory
    mlockall(MCL_CURRENT | MCL_FUTURE);

    // setup signal handler
    signal(SIGINT, sigHandler);
    signal(SIGTERM, sigHandler);
    signal(SIGHUP, sigHandler);
    signal(SIGALRM, sigHandler);

    // shadow task
    cout << "rt_shadow_task\n";
    res = rt_task_shadow(
            &task,              // task
            "QSERVER",          // name
            10,                 // priority
            T_FPU               // mode T_FPU, T_CPU(i), T_SUSP, ...
            );
    if (res < 0) {
        cerr << "rt_task_shadow() failed: " << res << endl;
        goto cleanup_end;
    }

    // create queue
    cout << "rt_queue_create\n";
    res = rt_queue_create(
            &queue,             // queue
            "QQUEUE",           // name
            1000000,            // pool size
            Q_UNLIMITED,        // qlimit
            Q_FIFO | Q_SHARED   // mode
            );
    if (res == -EEXIST) {
        cout << "queue already exists, trying to bind it\n";
        cout << "rt_queue_bind\n";
        res = rt_queue_bind(&queue, "QQUEUE", TM_NONBLOCK);
    }
    if (res < 0) {
        cerr << "rt_queue_create() or rt_queue_bind() failed: " << res << endl;
        goto cleanup_task;
    }

    // endless loop
    while (!end && (res == 0 || res == -ETIMEDOUT)) {
        cout << ".";
        cout.flush();
        void *msg;
        res = rt_queue_receive(&queue, &msg, rt_timer_ns2ticks(1000000000ll));
        if (res >= 0) {
            cout << "rt_queue_free\n";
            rt_queue_free(&queue, msg);
        } else if (res != -ETIMEDOUT) {
            cerr << "rt_queue_receive() failed: " << res << endl;
        }
    }

cleanup_queue:
    cout << "rt_queue_delete\n";
    rt_queue_delete(&queue);
cleanup_task:
cleanup_end:
    cout << "application exit\n";
    return (EXIT_SUCCESS);
}