#include <rtai_mbx.h>
#include <rtai_sched.h>

#include <uint128.h>
#include <sys/rtai_modbus.h>

#define DEBUG 1

#define SENSOR_PART_TURNTABLE 1<<0
#define SENSOR_PART_DRILL 1<<1
#define SENSOR_PART_MESURING 1<<2
#define SENSOR_DRILL_UP 1<<3
#define SENSOR_DRILL_DOWN 1<<4
#define SENSOR_TURNTABLE_POS 1<<5
#define SENSOR_PART_TEST 1<<6

#define ACTOR_DRILL 1<<0
#define ACTOR_TURNTABLE 1<<1
#define ACTOR_DRILL_DOWN 1<<2
#define ACTOR_DRILL_UP 1<<3
#define ACTOR_PART_HOLD 1<<4
#define ACTOR_TESTER 1<<5
#define ACTOR_EXIT 1<<6
#define ACTOR_ENTRANCE 1<<7

MODULE_LICENSE("GPL");

static RT_TASK entry_task;

int readAll(int connection, int *value) {
    if (rt_modbus_get(fd_node, DIGITAL_IN, 0, (unsigned short *) &value)) {
        return 1;
    }
    return 0;
}

int read(int connection, int part, int *result) {
    int value;
    int code = readAll(connection, &value);
    *result = (value & part) != 0;
    return code;
}

int disable(int connection, int part) {
    int value;
    int code = readAll(connection, &value);
    if (code) return code;
    if (rt_modbus_set(connection, DIGITAL_OUT, output &= part))
        return 1;
    return 0;
}

int enable(int connection, int part) {
    int value;
    int code = readAll(connection, &value);
    if (code) return code;
    if (rt_modbus_set(connection, DIGITAL_OUT, output |= part))
        return 1;
    return 0;
}

static void
turntable(long x) {
    int connection;
    int ready = 0;

    rt_printk("turntable: task started\n");

    if ((connection = rt_modbus_connect("modbus-node")) == -1) {
        rt_printk("turntable: task exited\n");
        return;
    }
    rt_printk("turntable: MODBUS communication opened\n");

    while (1) {
        int partOnTable;
        if (read(connection, SENSOR_PART_TURNTABLE, &partOnTable)) goto fail;

        if (partOnTable) {
            rt_printk("halloo welt");
            if (enable(connection, ACTOR_TURNTABLE)) goto fail;

        } else {
            if (disable(connection, ACTOR_TURNTABLE)) goto fail;
        }

        if (ready)
            goto fail;

        rt_sleep(1000 * nano2count(1000000));
    }

/* Aufraeumen */
    fail:
    rt_modbus_disconnect(connection);
    rt_printk("turntable: task exited with failure\n");
}

static void __exit

example_exit(void) {
    rt_task_delete(&entry_task);
    stop_rt_timer();
    printk("module Bearbeiten2 unloaded\n");
}

static int __init

example_init(void) {
    rt_set_oneshot_mode();
    start_rt_timer(0);
    modbus_init();

    if (rt_task_init(&entry_task, entry, 0, 1024, 0, 0, NULL)) {
        printk("cannot initialize control task\n");
        goto fail;
    }

    rt_task_resume(&entry_task);
    printk("loaded module Bearbeiten2\n");
    return (0);

    fail:
    stop_rt_timer();
    return (1);
}

module_exit(example_exit)
module_init(example_init)