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

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

#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 turntable_task, drill_task, tester_task, output_task;
static MBX drill_mbx, tester_mbx, output_mbx;
static SEM semaphore;

char node[] = "modbus-node";

/**
 * read all bits of the specified type
 * this function is *not* thread safe.
 * @param type (DIGITAL_IN, DIGITAL_OUT, ANALOG_IN, ANALOG_OUT)
 * @param connection modbus connection id
 * @param result value to write the result to
 * @return status code(0: success, 1: failure)
 */
int readAll(int type, int connection, int *result) {
    return rt_modbus_get(fd_node, type, 0, (unsigned short *) &result));
}

/**
 *
 * this function is thread safe.
 * @param connection
 * @param part
 * @param result value to write the result to
 * @return status code(0: success, 1: failure)
 */
int read(int connection, int part, int *result) {
    rt_sem_wait(&semaphore);
    int value;
    int code = readAll(DIGITAL_IN, connection, &value);
    *result = (value & part) != 0;
    rt_sem_signal(&semaphore);
    return code;
}

/**
 * disable a specified actor of the system.
 * this function is thread safe.
 * @param connection modbus connection id
 * @param actor bitmask of the actor to disable
 * @return status code(0: success, 1: failure)
 */
int disable(int connection, int actor) {
    rt_sem_wait(&semaphore);
    int value;
    int code = readAll(DIGITAL_OUT, connection, &value);
    if (code) return code;
    int result = rt_modbus_set(connection, DIGITAL_OUT, output &= ~actor)
    rt_sem_signal(&semaphore);
    return result;
}

/**
 * enable one specified actor of the system.
 * this function is thread safe.
 * @param connection modbus connection id
 * @param actor bitmask of the actor to enable
 * @return status code (0: success, 1: failure)
 */
int enable(int connection, int actor) {
    rt_sem_wait(&semaphore);
    int output;
    int code = readAll(DIGITAL_OUT, connection, &output);
    if (code) return code;
    int result = rt_modbus_set(connection, DIGITAL_OUT, output |= actor)
    rt_sem_signal(&semaphore);
    return result;
}

void sleep() {
    rt_sleep(1000 * nano2count(1000000));
}

static void turntable(long x) {
    int connection;

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

    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("turntable: could not connect to %s\n", node);
        return;
    }
    rt_printk("turntable: MODBUS communication opened\n");

    disable(connection, ACTOR_TURNTABLE);

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

        if (partOnTable) {
            if (enable(connection, ACTOR_TURNTABLE)) goto fail;

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

        rt_task_resume(&drill_task);
        rt_task_resume(&tester_task);
        rt_task_resume(&output_task);

        //rt_sleep(1000 * nano2count(1000000));
    }

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

static void drill(long x) {
    int connection;

    rt_printk("drill: task started\n");
    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("drill: could not connect to %s\n", node);
        return;
    }
    rt_printk("drill: MODBUS communication opened\n");
    disable(connection, ACTOR_DRILL);
    disable(connection, ACTOR_PART_HOLD);
    disable(connection, ACTOR_DRILL_DOWN);

    enable(connection, ACTOR_DRILL_UP);
    int drill_up = 0;
    if (read(connection, SENSOR_DRILL_UP, drill_up)) goto fail;
    if (!drill_up) {
        do {
            read(connection, SENSOR_DRILL_UP, drill_up);
        } while (drill_up == 0);
    }
    disable(connection, ACTOR_DRILL_UP);

    while (1) {

        sleep();
    }


    fail:
    rt_modbus_disconnect(connection);
    rt_printk("drill: task exited with failure\n");
}

static void tester(long x) {
    int connection;

    rt_printk("tester: task started\n");
    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("tester: could not connect to %s\n", node);
        return;
    }

    while (1) {

        sleep();
    }


    fail:
    rt_modbus_disconnect(connection);
    rt_printk("tester: task exited with failure\n");
}

static void output(long x) {
    int connection;

    rt_printk("output: task started\n");
    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("output: could not connect to %s\n", node);
        return;
    }

    while (1) {

        sleep();
    }


    fail:
    rt_modbus_disconnect(connection);
    rt_printk("output: task exited with failure\n");
}

static void __exit

example_exit(void) {
    rt_task_delete(&turntable_task);
    rt_task_delete(&drill_task);
    rt_task_delete(&tester_task);
    rt_task_delete(&output_task);
    stop_rt_timer();
    rt_sem_delete(&semaphore);
    printk("module Bearbeiten2 unloaded\n");
}

static int __init

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

    rt_sem_init(&semaphore, 1);

    if (rt_task_init(&turntable_task, turntable, 0, 1024, 0, 0, NULL)) {
        printk("turntable: cannot initialize task\n");
        goto fail;
    }
    if (rt_task_init(&drill_task, drill, 0, 1024, 0, 0, NULL)) {
        printk("drill: cannot initialize task\n");
        goto fail;
    }
    if (rt_task_init(&turntable_task, tester, 0, 1024, 0, 0, NULL)) {
        printk("tester: cannot initialize task\n");
        goto fail;
    }
    if (rt_task_init(&turntable_task, output, 0, 1024, 0, 0, NULL)) {
        printk("output: cannot initialize task\n");
        goto fail;
    }

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

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

module_exit(example_exit)
module_init(example_init)